Formatting calib code before refactoring (#999)

As discussed in #975 and #973 doing the linting first. using style from [here](https://github.com/ament/ament_lint/blob/rolling/ament_pycodestyle/ament_pycodestyle/configuration/ament_pycodestyle.ini)
ros-perception · Jun 10, 2024 · 9a4a42a · 9a4a42a
1 parent efb9005
commit 9a4a42a
Show file tree

Hide file tree

Showing 9 changed files with 639 additions and 425 deletions.
diff --git a/camera_calibration/setup.py b/camera_calibration/setup.py
@@ -8,9 +8,9 @@
     version='6.0.0',
     packages=["camera_calibration", "camera_calibration.nodes"],
     data_files=[
-    ('share/ament_index/resource_index/packages',
-      ['resource/' + PACKAGE_NAME]),
-    ('share/' + PACKAGE_NAME, ['package.xml']),
+        ('share/ament_index/resource_index/packages',
+         ['resource/' + PACKAGE_NAME]),
+        ('share/' + PACKAGE_NAME, ['package.xml']),
     ],
     py_modules=[],
     package_dir={'': 'src'},

diff --git a/camera_calibration/src/camera_calibration/calibrator.py b/camera_calibration/src/camera_calibration/calibrator.py
diff --git a/camera_calibration/src/camera_calibration/camera_calibrator.py b/camera_calibration/src/camera_calibration/camera_calibrator.py
diff --git a/camera_calibration/src/camera_calibration/camera_checker.py b/camera_calibration/src/camera_calibration/camera_checker.py
@@ -55,14 +55,17 @@
 def mean(seq):
     return sum(seq) / len(seq)
 
+
 def lmin(seq1, seq2):
     """ Pairwise minimum of two sequences """
     return [min(a, b) for (a, b) in zip(seq1, seq2)]
 
+
 def lmax(seq1, seq2):
     """ Pairwise maximum of two sequences """
     return [max(a, b) for (a, b) in zip(seq1, seq2)]
 
+
 class ConsumerThread(threading.Thread):
     def __init__(self, queue, function):
         threading.Thread.__init__(self)
@@ -76,6 +79,7 @@ def run(self):
                 break
         self.function(m)
 
+
 class CameraCheckerNode(Node):
 
     def __init__(self, name, chess_size, dim, approximate=0):
@@ -100,9 +104,11 @@ def __init__(self, name, chess_size, dim, approximate=0):
         if approximate <= 0:
             sync = message_filters.TimeSynchronizer
         else:
-            sync = functools.partial(ApproximateTimeSynchronizer, slop=approximate)
+            sync = functools.partial(
+                ApproximateTimeSynchronizer, slop=approximate)
 
-        tsm = sync([message_filters.Subscriber(self, type, topic) for (topic, type) in tosync_mono], 10)
+        tsm = sync([message_filters.Subscriber(self, type, topic)
+                   for (topic, type) in tosync_mono], 10)
         tsm.registerCallback(self.queue_monocular)
 
         left_topic = "stereo/left/image_rect"
@@ -117,7 +123,8 @@ def __init__(self, name, chess_size, dim, approximate=0):
             (right_camera_topic, sensor_msgs.msg.CameraInfo)
         ]
 
-        tss = sync([message_filters.Subscriber(self, type, topic) for (topic, type) in tosync_stereo], 10)
+        tss = sync([message_filters.Subscriber(self, type, topic)
+                   for (topic, type) in tosync_stereo], 10)
         tss.registerCallback(self.queue_stereo)
 
         self.br = cv_bridge.CvBridge()
@@ -162,24 +169,29 @@ def handle_monocular(self, msg):
 
             # Add in reprojection check
             image_points = C
-            object_points = self.mc.mk_object_points([self.board], use_board_size=True)[0]
+            object_points = self.mc.mk_object_points(
+                [self.board], use_board_size=True)[0]
             dist_coeffs = numpy.zeros((4, 1))
-            camera_matrix = numpy.array( [ [ camera.P[0], camera.P[1], camera.P[2]  ],
-                                           [ camera.P[4], camera.P[5], camera.P[6]  ],
-                                           [ camera.P[8], camera.P[9], camera.P[10] ] ] )
-            ok, rot, trans = cv2.solvePnP(object_points, image_points, camera_matrix, dist_coeffs)
+            camera_matrix = numpy.array([[camera.P[0], camera.P[1], camera.P[2]],
+                                         [camera.P[4], camera.P[5], camera.P[6]],
+                                         [camera.P[8], camera.P[9], camera.P[10]]])
+            ok, rot, trans = cv2.solvePnP(
+                object_points, image_points, camera_matrix, dist_coeffs)
             # Convert rotation into a 3x3 Rotation Matrix
             rot3x3, _ = cv2.Rodrigues(rot)
             # Reproject model points into image
-            object_points_world = numpy.asmatrix(rot3x3) * numpy.asmatrix(object_points.squeeze().T) + numpy.asmatrix(trans)
+            object_points_world = numpy.asmatrix(
+                rot3x3) * numpy.asmatrix(object_points.squeeze().T) + numpy.asmatrix(trans)
             reprojected_h = camera_matrix * object_points_world
-            reprojected   = (reprojected_h[0:2, :] / reprojected_h[2, :])
+            reprojected = (reprojected_h[0:2, :] / reprojected_h[2, :])
             reprojection_errors = image_points.squeeze().T - reprojected
 
-            reprojection_rms = numpy.sqrt(numpy.sum(numpy.array(reprojection_errors) ** 2) / numpy.product(reprojection_errors.shape))
+            reprojection_rms = numpy.sqrt(numpy.sum(numpy.array(
+                reprojection_errors) ** 2) / numpy.product(reprojection_errors.shape))
 
             # Print the results
-            print("Linearity RMS Error: %.3f Pixels      Reprojection RMS Error: %.3f Pixels" % (linearity_rms, reprojection_rms))
+            print("Linearity RMS Error: %.3f Pixels      Reprojection RMS Error: %.3f Pixels" % (
+                linearity_rms, reprojection_rms))
         else:
             print('no chessboard')
 
@@ -194,8 +206,10 @@ def handle_stereo(self, msg):
         if L is not None and R is not None:
             epipolar = self.sc.epipolar_error(L, R)
 
-            dimension = self.sc.chessboard_size(L, R, self.board, msg=(lcmsg, rcmsg))
+            dimension = self.sc.chessboard_size(
+                L, R, self.board, msg=(lcmsg, rcmsg))
 
-            print("epipolar error: %f pixels   dimension: %f m" % (epipolar, dimension))
+            print("epipolar error: %f pixels   dimension: %f m" %
+                  (epipolar, dimension))
         else:
             print("no chessboard")
diff --git a/camera_calibration/src/camera_calibration/nodes/cameracalibrator.py b/camera_calibration/src/camera_calibration/nodes/cameracalibrator.py
@@ -40,17 +40,20 @@
 from camera_calibration.calibrator import ChessboardInfo, Patterns
 from message_filters import ApproximateTimeSynchronizer
 
+
 def optionsValidCharuco(options, parser):
     """
     Validates the provided options when the pattern type is 'charuco'
     """
-    if options.pattern != 'charuco': return False
+    if options.pattern != 'charuco':
+        return False
 
     n_boards = len(options.size)
     if (n_boards != len(options.square) or n_boards != len(options.charuco_marker_size) or n_boards !=
             len(options.aruco_dict)):
-        parser.error("When using ChArUco boards, --size, --square, --charuco_marker_size, and --aruco_dict " +
-        "must be specified for each board")
+        parser.error(
+            "When using ChArUco boards, --size, --square, --charuco_marker_size, and --aruco_dict "
+            + "must be specified for each board")
         return False
 
     # TODO: check for fisheye and stereo (not implemented with ChArUco)
@@ -62,18 +65,19 @@ def main():
     parser = OptionParser("%prog --size SIZE1 --square SQUARE1 [ --size SIZE2 --square SQUARE2 ]",
                           description=None)
     parser.add_option("-c", "--camera_name",
-                     type="string", default='narrow_stereo',
-                     help="name of the camera to appear in the calibration file")
-    group = OptionGroup(parser, "Chessboard Options",
-                        "You must specify one or more chessboards as pairs of --size and --square options.")
+                      type="string", default='narrow_stereo',
+                      help="name of the camera to appear in the calibration file")
+    group = OptionGroup(
+        parser, "Chessboard Options",
+        "You must specify one or more chessboards as pairs of --size and --square options.")
     group.add_option("-p", "--pattern",
                      type="string", default="chessboard",
                      help="calibration pattern to detect - 'chessboard', 'circles', 'acircles', 'charuco'\n" +
                      "  if 'charuco' is used, a --charuco_marker_size and --aruco_dict argument must be supplied\n" +
                      "  with each --size and --square argument")
-    group.add_option("-s", "--size",
-                     action="append", default=[],
-                     help="chessboard size as NxM, counting interior corners (e.g. a standard chessboard is 7x7)")
+    group.add_option(
+        "-s", "--size", action="append", default=[],
+        help="chessboard size as NxM, counting interior corners (e.g. a standard chessboard is 7x7)")
     group.add_option("-q", "--square",
                      action="append", default=[],
                      help="chessboard square size in meters")
@@ -86,9 +90,9 @@ def main():
                      "'5x5_250', '6x6_250', '7x7_250'")
     parser.add_option_group(group)
     group = OptionGroup(parser, "ROS Communication Options")
-    group.add_option("--approximate",
-                     type="float", default=0.0,
-                     help="allow specified slop (in seconds) when pairing images from unsynchronized stereo cameras")
+    group.add_option(
+        "--approximate", type="float", default=0.0,
+        help="allow specified slop (in seconds) when pairing images from unsynchronized stereo cameras")
     group.add_option("--no-service-check",
                      action="store_false", dest="service_check", default=True,
                      help="disable check for set_camera_info services at startup")
@@ -106,22 +110,22 @@ def main():
     group.add_option("--zero-tangent-dist",
                      action="store_true", default=False,
                      help="for pinhole, set tangential distortion coefficients (p1, p2) to zero")
-    group.add_option("-k", "--k-coefficients",
-                     type="int", default=2, metavar="NUM_COEFFS",
-                     help="for pinhole, number of radial distortion coefficients to use (up to 6, default %default)")
+    group.add_option(
+        "-k", "--k-coefficients", type="int", default=2, metavar="NUM_COEFFS",
+        help="for pinhole, number of radial distortion coefficients to use (up to 6, default %default)")
 
-    group.add_option("--fisheye-recompute-extrinsicsts",
-                     action="store_true", default=False,
-                     help="for fisheye, extrinsic will be recomputed after each iteration of intrinsic optimization")
+    group.add_option(
+        "--fisheye-recompute-extrinsicsts", action="store_true", default=False,
+        help="for fisheye, extrinsic will be recomputed after each iteration of intrinsic optimization")
     group.add_option("--fisheye-fix-skew",
                      action="store_true", default=False,
                      help="for fisheye, skew coefficient (alpha) is set to zero and stay zero")
     group.add_option("--fisheye-fix-principal-point",
                      action="store_true", default=False,
                      help="for fisheye,fix the principal point at the image center")
-    group.add_option("--fisheye-k-coefficients",
-                     type="int", default=4, metavar="NUM_COEFFS",
-                     help="for fisheye, number of radial distortion coefficients to use fixing to zero the rest (up to 4, default %default)")
+    group.add_option(
+        "--fisheye-k-coefficients", type="int", default=4, metavar="NUM_COEFFS",
+        help="for fisheye, number of radial distortion coefficients to use fixing to zero the rest (up to 4, default %default)")
 
     group.add_option("--fisheye-check-conditions",
                      action="store_true", default=False,
@@ -147,16 +151,19 @@ def main():
     if options.pattern == "charuco" and optionsValidCharuco(options, parser):
         for (sz, sq, ms, ad) in zip(options.size, options.square, options.charuco_marker_size, options.aruco_dict):
             size = tuple([int(c) for c in sz.split('x')])
-            boards.append(ChessboardInfo('charuco', size[0], size[1], float(sq), float(ms), ad))
+            boards.append(ChessboardInfo(
+                'charuco', size[0], size[1], float(sq), float(ms), ad))
     else:
         for (sz, sq) in zip(options.size, options.square):
             size = tuple([int(c) for c in sz.split('x')])
-            boards.append(ChessboardInfo(options.pattern, size[0], size[1], float(sq)))
+            boards.append(ChessboardInfo(
+                options.pattern, size[0], size[1], float(sq)))
 
     if options.approximate == 0.0:
         sync = message_filters.TimeSynchronizer
     else:
-        sync = functools.partial(ApproximateTimeSynchronizer, slop=options.approximate)
+        sync = functools.partial(
+            ApproximateTimeSynchronizer, slop=options.approximate)
 
     # Pinhole opencv calibration options parsing
     num_ks = options.k_coefficients
@@ -211,21 +218,23 @@ def main():
     elif options.pattern == 'charuco':
         pattern = Patterns.ChArUco
     elif options.pattern != 'chessboard':
-        print('Unrecognized pattern %s, defaulting to chessboard' % options.pattern)
+        print('Unrecognized pattern %s, defaulting to chessboard' %
+              options.pattern)
 
     if options.disable_calib_cb_fast_check:
         checkerboard_flags = 0
     else:
         checkerboard_flags = cv2.CALIB_CB_FAST_CHECK
 
     rclpy.init()
-    node = OpenCVCalibrationNode("cameracalibrator", boards, options.service_check, sync,
-                                 calib_flags, fisheye_calib_flags, pattern, options.camera_name,
-                                 checkerboard_flags=checkerboard_flags, max_chessboard_speed=options.max_chessboard_speed,
-                                 queue_size=options.queue_size)
+    node = OpenCVCalibrationNode(
+        "cameracalibrator", boards, options.service_check, sync, calib_flags, fisheye_calib_flags,
+        pattern, options.camera_name, checkerboard_flags=checkerboard_flags,
+        max_chessboard_speed=options.max_chessboard_speed, queue_size=options.queue_size)
     node.spin()
     rclpy.shutdown()
 
+
 if __name__ == "__main__":
     try:
         main()

diff --git a/camera_calibration/src/camera_calibration/nodes/cameracheck.py b/camera_calibration/src/camera_calibration/nodes/cameracheck.py
@@ -39,11 +39,13 @@
 def main():
     from optparse import OptionParser
     parser = OptionParser()
-    parser.add_option("-s", "--size", default="8x6", help="specify chessboard size as nxm [default: %default]")
-    parser.add_option("-q", "--square", default=".108", help="specify chessboard square size in meters [default: %default]")
-    parser.add_option("--approximate",
-                      type="float", default=0.0,
-                      help="allow specified slop (in seconds) when pairing images from unsynchronized stereo cameras")
+    parser.add_option("-s", "--size", default="8x6",
+                      help="specify chessboard size as nxm [default: %default]")
+    parser.add_option("-q", "--square", default=".108",
+                      help="specify chessboard square size in meters [default: %default]")
+    parser.add_option(
+        "--approximate", type="float", default=0.0,
+        help="allow specified slop (in seconds) when pairing images from unsynchronized stereo cameras")
 
     options, _ = parser.parse_args(rclpy.utilities.remove_ros_args())
     rclpy.init()
@@ -54,5 +56,6 @@ def main():
     node = CameraCheckerNode("cameracheck", size, dim, approximate)
     rclpy.spin(node)
 
+
 if __name__ == "__main__":
     main()