From 983d6cb26c32b2b04f3de8061b13b7b30b7c1a6d Mon Sep 17 00:00:00 2001 From: MRo47 Date: Fri, 26 Apr 2024 17:58:18 +0000 Subject: [PATCH 1/3] fix dictionary names --- .../src/camera_calibration/calibrator.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/camera_calibration/src/camera_calibration/calibrator.py b/camera_calibration/src/camera_calibration/calibrator.py index 6f7744612..f0fe63cf5 100644 --- a/camera_calibration/src/camera_calibration/calibrator.py +++ b/camera_calibration/src/camera_calibration/calibrator.py @@ -80,14 +80,14 @@ def __init__(self, pattern="chessboard", n_cols = 0, n_rows = 0, dim = 0.0, mark "5x5_100" : cv2.aruco.DICT_5X5_100, "5x5_250" : cv2.aruco.DICT_5X5_250, "5x5_1000" : cv2.aruco.DICT_5X5_1000, - "6x6_50" : cv2.aruco.DICT_6x6_50, - "6x6_100" : cv2.aruco.DICT_6x6_100, - "6x6_250" : cv2.aruco.DICT_6x6_250, - "6x6_1000" : cv2.aruco.DICT_6x6_1000, - "7x7_50" : cv2.aruco.DICT_7x7_50, - "7x7_100" : cv2.aruco.DICT_7x7_100, - "7x7_250" : cv2.aruco.DICT_7x7_250, - "7x7_1000" : cv2.aruco.DICT_7x7_1000}[aruco_dict]) + "6x6_50" : cv2.aruco.DICT_6X6_50, + "6x6_100" : cv2.aruco.DICT_6X6_100, + "6x6_250" : cv2.aruco.DICT_6X6_250, + "6x6_1000" : cv2.aruco.DICT_6X6_1000, + "7x7_50" : cv2.aruco.DICT_7X7_50, + "7x7_100" : cv2.aruco.DICT_7X7_100, + "7x7_250" : cv2.aruco.DICT_7X7_250, + "7x7_1000" : cv2.aruco.DICT_7X7_1000}[aruco_dict]) self.charuco_board = cv2.aruco.CharucoBoard_create(self.n_cols, self.n_rows, self.dim, self.marker_size, self.aruco_dict) From d3c7fd0d39286de8bf4fd1be168031288791902c Mon Sep 17 00:00:00 2001 From: MRo47 Date: Sun, 28 Apr 2024 20:15:54 +0000 Subject: [PATCH 2/3] show reproj error + remove not implemented msg --- .../src/camera_calibration/calibrator.py | 62 ++++++++++++------- 1 file changed, 38 insertions(+), 24 deletions(-) diff --git a/camera_calibration/src/camera_calibration/calibrator.py b/camera_calibration/src/camera_calibration/calibrator.py index f0fe63cf5..b645bcb91 100644 --- a/camera_calibration/src/camera_calibration/calibrator.py +++ b/camera_calibration/src/camera_calibration/calibrator.py @@ -1136,29 +1136,29 @@ 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 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) + 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 LooseVersion(cv2.__version__).version[0] == 2: @@ -1328,6 +1328,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 +1397,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 6834a56a258b8fd091f6dfd86aaf27d3d2d2a9c6 Mon Sep 17 00:00:00 2001 From: MRo47 Date: Sun, 28 Apr 2024 20:17:31 +0000 Subject: [PATCH 3/3] get charuco chessboard pts if using charuco --- camera_calibration/src/camera_calibration/calibrator.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/camera_calibration/src/camera_calibration/calibrator.py b/camera_calibration/src/camera_calibration/calibrator.py index b645bcb91..dc794e3d3 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):