From 8a6865eadb312a67eb17cfc3b91f9eaefa743bfd Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 16 Jul 2024 17:01:26 +0200 Subject: [PATCH] Added stereo calibration using charuco board (backport #976) (#1002) 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.
This is an automatic backport of pull request #976 done by [Mergify](https://mergify.com). Co-authored-by: Myron Rodrigues <41271144+MRo47@users.noreply.github.com> --- .../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