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 efb9005)

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