From e360734b5cb1ff340f7b637f5f7b9f52c01a07d1 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] 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) --- .../src/camera_calibration/calibrator.py | 66 ++++++++++++------- 1 file changed, 41 insertions(+), 25 deletions(-) diff --git a/camera_calibration/src/camera_calibration/calibrator.py b/camera_calibration/src/camera_calibration/calibrator.py index 0793b4355..2e30fe578 100644 --- a/camera_calibration/src/camera_calibration/calibrator.py +++ b/camera_calibration/src/camera_calibration/calibrator.py @@ -491,8 +491,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): @@ -1151,29 +1154,28 @@ 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...") if VersionInfo.parse(cv2.__version__).major < 3: - 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) + 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) 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 VersionInfo.parse(cv2.__version__).major < 3: @@ -1343,6 +1345,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 @@ -1399,11 +1414,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