From 547a1a88575e4accd8eee14ebdeb3ff6cb0d94f1 Mon Sep 17 00:00:00 2001 From: Myron Rodrigues <41271144+MRo47@users.noreply.github.com> Date: Mon, 3 Jun 2024 17:17:50 +0100 Subject: [PATCH 1/3] Added stereo calibration using charuco board (#976) From #972 Doing this first for rolling. This was a TODO in the repository, opening this PR to add this feature. - The main issue why this wasn't possible imo is the way `mk_obj_points` works. I'm using the inbuilt opencv function to get the points there. - The other is a condition when aruco markers are detected they are added as good points, This is fine in case of mono but in stereo these have to be the same number as the object points to find matches although this should be possible with aruco. (cherry picked from commit efb9005a5ca45f4ceb7563fc01d7e33d6f39214d) # Conflicts: # camera_calibration/src/camera_calibration/calibrator.py --- .../src/camera_calibration/calibrator.py | 63 +++++++++++++------ 1 file changed, 45 insertions(+), 18 deletions(-) diff --git a/camera_calibration/src/camera_calibration/calibrator.py b/camera_calibration/src/camera_calibration/calibrator.py index f0fe63cf5..efbb87865 100644 --- a/camera_calibration/src/camera_calibration/calibrator.py +++ b/camera_calibration/src/camera_calibration/calibrator.py @@ -476,8 +476,11 @@ def compute_goodenough(self): return list(zip(self._param_names, min_params, max_params, progress)) def mk_object_points(self, boards, use_board_size = False): + if self.pattern == Patterns.ChArUco: + opts = [board.charuco_board.chessboardCorners for board in boards] + return opts opts = [] - for i, b in enumerate(boards): + for b in boards: num_pts = b.n_cols * b.n_rows opts_loc = numpy.zeros((num_pts, 1, 3), numpy.float32) for j in range(num_pts): @@ -1136,12 +1139,9 @@ def cal_fromcorners(self, good): self.T = numpy.zeros((3, 1), dtype=numpy.float64) self.R = numpy.eye(3, dtype=numpy.float64) - if self.pattern == Patterns.ChArUco: - # TODO: implement stereo ChArUco calibration - raise NotImplemented("Stereo calibration not implemented for ChArUco boards") - if self.camera_model == CAMERA_MODEL.PINHOLE: print("stereo pinhole calibration...") +<<<<<<< HEAD if LooseVersion(cv2.__version__).version[0] == 2: cv2.stereoCalibrate(opts, lipts, ripts, self.size, self.l.intrinsics, self.l.distortion, @@ -1150,15 +1150,28 @@ def cal_fromcorners(self, good): self.T, # T criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5), flags = flags) +======= + if VersionInfo.parse(cv2.__version__).major < 3: + ret_values = cv2.stereoCalibrate(opts, lipts, ripts, self.size, + self.l.intrinsics, self.l.distortion, + self.r.intrinsics, self.r.distortion, + self.R, # R + self.T, # T + criteria=(cv2.TERM_CRITERIA_EPS + \ + cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5), + flags=flags) +>>>>>>> efb9005 (Added stereo calibration using charuco board (#976)) else: - cv2.stereoCalibrate(opts, lipts, ripts, - self.l.intrinsics, self.l.distortion, - self.r.intrinsics, self.r.distortion, - self.size, - self.R, # R - self.T, # T - criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5), - flags = flags) + ret_values = cv2.stereoCalibrate(opts, lipts, ripts, + self.l.intrinsics, self.l.distortion, + self.r.intrinsics, self.r.distortion, + self.size, + self.R, # R + self.T, # T + criteria=(cv2.TERM_CRITERIA_EPS + \ + cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5), + flags=flags) + print(f"Stereo RMS re-projection error: {ret_values[0]}") elif self.camera_model == CAMERA_MODEL.FISHEYE: print("stereo fisheye calibration...") if LooseVersion(cv2.__version__).version[0] == 2: @@ -1328,6 +1341,19 @@ def l2(p0, p1): [l2(pt3d[c + 0], pt3d[c + (cc * (cr - 1))]) / (cr - 1) for c in range(cc)]) return sum(lengths) / len(lengths) + def update_db(self, lgray, rgray, lcorners, rcorners, lids, rids, lboard): + """ + update database with images and good corners if good samples are detected + """ + params = self.get_parameters( + lcorners, lids, lboard, (lgray.shape[1], lgray.shape[0])) + if self.is_good_sample(params, lcorners, lids, self.last_frame_corners, self.last_frame_ids): + self.db.append((params, lgray, rgray)) + self.good_corners.append( + (lcorners, rcorners, lids, rids, lboard)) + print(("*** Added sample %d, p_x = %.3f, p_y = %.3f, p_size = %.3f, skew = %.3f" % + tuple([len(self.db)] + params))) + def handle_msg(self, msg): # TODO Various asserts that images have same dimension, same board detected... (lmsg, rmsg) = msg @@ -1384,11 +1410,12 @@ def handle_msg(self, msg): # Add sample to database only if it's sufficiently different from any previous sample if lcorners is not None and rcorners is not None and len(lcorners) == len(rcorners): - params = self.get_parameters(lcorners, lids, lboard, (lgray.shape[1], lgray.shape[0])) - if self.is_good_sample(params, lcorners, lids, self.last_frame_corners, self.last_frame_ids): - self.db.append( (params, lgray, rgray) ) - self.good_corners.append( (lcorners, rcorners, lids, rids, lboard) ) - print(("*** Added sample %d, p_x = %.3f, p_y = %.3f, p_size = %.3f, skew = %.3f" % tuple([len(self.db)] + params))) + # Add samples only with entire board in view if charuco + if self.pattern == Patterns.ChArUco: + if len(lcorners) == lboard.charuco_board.chessboardCorners.shape[0]: + self.update_db(lgray, rgray, lcorners, rcorners, lids, rids, lboard) + else: + self.update_db(lgray, rgray, lcorners, rcorners, lids, rids, lboard) self.last_frame_corners = lcorners self.last_frame_ids = lids From d248eed4eb083ca283be436be0484fe19b7cef8d Mon Sep 17 00:00:00 2001 From: Michael Ferguson Date: Tue, 29 Oct 2024 14:14:49 -0400 Subject: [PATCH 2/3] fix merge conflict --- .../src/camera_calibration/calibrator.py | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/camera_calibration/src/camera_calibration/calibrator.py b/camera_calibration/src/camera_calibration/calibrator.py index efbb87865..2325b4542 100644 --- a/camera_calibration/src/camera_calibration/calibrator.py +++ b/camera_calibration/src/camera_calibration/calibrator.py @@ -1141,16 +1141,6 @@ def cal_fromcorners(self, good): if self.camera_model == CAMERA_MODEL.PINHOLE: print("stereo pinhole calibration...") -<<<<<<< HEAD - if LooseVersion(cv2.__version__).version[0] == 2: - cv2.stereoCalibrate(opts, lipts, ripts, self.size, - self.l.intrinsics, self.l.distortion, - self.r.intrinsics, self.r.distortion, - self.R, # R - self.T, # T - criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5), - flags = flags) -======= if VersionInfo.parse(cv2.__version__).major < 3: ret_values = cv2.stereoCalibrate(opts, lipts, ripts, self.size, self.l.intrinsics, self.l.distortion, @@ -1160,7 +1150,6 @@ def cal_fromcorners(self, good): criteria=(cv2.TERM_CRITERIA_EPS + \ cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5), flags=flags) ->>>>>>> efb9005 (Added stereo calibration using charuco board (#976)) else: ret_values = cv2.stereoCalibrate(opts, lipts, ripts, self.l.intrinsics, self.l.distortion, From d69dcdf55fb693f4576d7cdf7f28133ed67ac66b Mon Sep 17 00:00:00 2001 From: Michael Ferguson Date: Tue, 29 Oct 2024 14:26:00 -0400 Subject: [PATCH 3/3] fix merge --- camera_calibration/src/camera_calibration/calibrator.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/camera_calibration/src/camera_calibration/calibrator.py b/camera_calibration/src/camera_calibration/calibrator.py index 2325b4542..14162d9dc 100644 --- a/camera_calibration/src/camera_calibration/calibrator.py +++ b/camera_calibration/src/camera_calibration/calibrator.py @@ -1141,7 +1141,7 @@ def cal_fromcorners(self, good): if self.camera_model == CAMERA_MODEL.PINHOLE: print("stereo pinhole calibration...") - if VersionInfo.parse(cv2.__version__).major < 3: + LooseVersion(cv2.__version__).version[0] == 2: ret_values = cv2.stereoCalibrate(opts, lipts, ripts, self.size, self.l.intrinsics, self.l.distortion, self.r.intrinsics, self.r.distortion,