upload content

build/camera_calibration/build/lib/camera_calibration/camera_calibrator.py

@@ -0,0 +1,366 @@
+#!/usr/bin/python
+#
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2009, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of the Willow Garage nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import cv2
+import message_filters
+import numpy
+import os
+import rclpy
+from rclpy.node import Node
+import sensor_msgs.msg
+import sensor_msgs.srv
+import threading
+import time
+from camera_calibration.calibrator import MonoCalibrator, StereoCalibrator, ChessboardInfo, Patterns
+from collections import deque
+from message_filters import ApproximateTimeSynchronizer
+from std_msgs.msg import String
+from std_srvs.srv import Empty
+
+class SpinThread(threading.Thread):
+    """
+    Thread that spins the ros node, while imshow runs in the main thread
+    """
+
+    def __init__(self, node):
+        threading.Thread.__init__(self)
+        self.node = node
+
+    def run(self):
+        rclpy.spin(self.node)
+        
+
+class ConsumerThread(threading.Thread):
+    def __init__(self, queue, function):
+        threading.Thread.__init__(self)
+        self.queue = queue
+        self.function = function
+
+    def run(self):
+        while True:
+            # wait for an image (could happen at the very beginning when the queue is still empty)
+            while len(self.queue) == 0:
+                time.sleep(0.1)
+            self.function(self.queue[0])
+
+
+class CalibrationNode(Node):
+    def __init__(self, name, boards, service_check = True, synchronizer = message_filters.TimeSynchronizer, flags = 0,
+                 pattern=Patterns.Chessboard, camera_name='', checkerboard_flags = 0):
+        super().__init__(name)
+
+        self.set_camera_info_service = self.create_client(sensor_msgs.srv.SetCameraInfo,
+                                                          "camera/set_camera_info")
+        self.set_left_camera_info_service = self.create_client(sensor_msgs.srv.SetCameraInfo,
+                                                               "left_camera/set_camera_info")
+        self.set_right_camera_info_service = self.create_client(sensor_msgs.srv.SetCameraInfo,
+                                                                "right_camera/set_camera_info")
+
+        if service_check:
+            # assume any non-default service names have been set.  Wait for the service to become ready
+            for cli in [self.set_camera_info_service, self.set_left_camera_info_service, self.set_right_camera_info_service]:
+                #remapped = rclpy.remap_name(svcname)
+                #if remapped != svcname:
+                #fullservicename = "%s/set_camera_info" % remapped
+                print("Waiting for service", cli.srv_name, "...")
+                # check all services so they are ready.
+                try:
+                    cli.wait_for_service(timeout_sec=5)
+                    print("OK")
+                except Exception as e:
+                    print("Service not found: %s".format(e))
+                    rclpy.shutdown()
+
+        self._boards = boards
+        self._calib_flags = flags
+        self._checkerboard_flags = checkerboard_flags
+        self._pattern = pattern
+        self._camera_name = camera_name
+        lsub = message_filters.Subscriber(self, sensor_msgs.msg.Image, 'left')
+        rsub = message_filters.Subscriber(self, sensor_msgs.msg.Image, 'right')
+        ts = synchronizer([lsub, rsub], 4)
+        ts.registerCallback(self.queue_stereo)
+
+        msub = message_filters.Subscriber(self, sensor_msgs.msg.Image, 'image')
+        msub.registerCallback(self.queue_monocular)
+
+        self.q_mono = deque([], 1)
+        self.q_stereo = deque([], 1)
+
+        self.c = None
+
+        mth = ConsumerThread(self.q_mono, self.handle_monocular)
+        mth.setDaemon(True)
+        mth.start()
+
+        sth = ConsumerThread(self.q_stereo, self.handle_stereo)
+        sth.setDaemon(True)
+        sth.start()
+
+    def redraw_stereo(self, *args):
+        pass
+    def redraw_monocular(self, *args):
+        pass
+
+    def queue_monocular(self, msg):
+        self.q_mono.append(msg)
+
+    def queue_stereo(self, lmsg, rmsg):
+        self.q_stereo.append((lmsg, rmsg))
+
+    def handle_monocular(self, msg):
+        if self.c == None:
+            if self._camera_name:
+                self.c = MonoCalibrator(self._boards, self._calib_flags, self._pattern, name=self._camera_name,
+                                        checkerboard_flags=self._checkerboard_flags)
+            else:
+                self.c = MonoCalibrator(self._boards, self._calib_flags, self._pattern,
+                                        checkerboard_flags=self.checkerboard_flags)
+
+        # This should just call the MonoCalibrator
+        drawable = self.c.handle_msg(msg)
+        self.displaywidth = drawable.scrib.shape[1]
+        self.redraw_monocular(drawable)
+
+    def handle_stereo(self, msg):
+        if self.c == None:
+            if self._camera_name:
+                self.c = StereoCalibrator(self._boards, self._calib_flags, self._pattern, name=self._camera_name,
+                                          checkerboard_flags=self._checkerboard_flags)
+            else:
+                self.c = StereoCalibrator(self._boards, self._calib_flags, self._pattern,
+                                          checkerboard_flags=self._checkerboard_flags)
+
+        drawable = self.c.handle_msg(msg)
+        self.displaywidth = drawable.lscrib.shape[1] + drawable.rscrib.shape[1]
+        self.redraw_stereo(drawable)
+
+
+    def check_set_camera_info(self, response):
+        if response.done():
+            if response.result() is not None:
+                if response.result().success:
+                    return True
+
+        for i in range(10):
+            print("!" * 80)
+        print()
+        print("Attempt to set camera info failed: " + response.result() if response.result() is not None else "Not available")
+        print()
+        for i in range(10):
+            print("!" * 80)
+        print()
+        self.get_logger().error('Unable to set camera info for calibration. Failure message: %s' % response.result() if response.result() is not None else "Not available")
+        return False
+
+    def do_upload(self):
+        self.c.report()
+        print(self.c.ost())
+        info = self.c.as_message()
+
+        req = sensor_msgs.srv.SetCameraInfo.Request()
+        rv = True
+        if self.c.is_mono:
+            req.camera_info = info
+            response = self.set_camera_info_service.call_async(req)
+            rv = self.check_set_camera_info(response)
+        else:
+            req.camera_info = info[0]
+            response = self.set_left_camera_info_service.call_async(req)
+            rv = rv and self.check_set_camera_info(response)
+            req.camera_info = info[1]
+            response = self.set_right_camera_info_service.call_async(req)
+            rv = rv and self.check_set_camera_info(response)
+        return rv
+
+
+class OpenCVCalibrationNode(CalibrationNode):
+    """ Calibration node with an OpenCV Gui """
+    FONT_FACE = cv2.FONT_HERSHEY_SIMPLEX
+    FONT_SCALE = 0.6
+    FONT_THICKNESS = 2
+
+    def __init__(self, *args, **kwargs):
+
+        CalibrationNode.__init__(self, *args, **kwargs)
+
+        self.queue_display = deque([], 1)
+        self.initWindow()
+
+    def spin(self):
+        sth = SpinThread(self)
+        sth.setDaemon(True)
+        sth.start()
+
+        while True:
+            # wait for an image (could happen at the very beginning when the queue is still empty)
+            while len(self.queue_display) == 0:
+                time.sleep(0.1)
+            im = self.queue_display[0]
+            cv2.imshow("display", im)
+            k = cv2.waitKey(6) & 0xFF
+            if k in [27, ord('q')]:
+                rclpy.shutdown()
+            elif k == ord('s'):
+                self.screendump(im)
+
+    def initWindow(self):
+        cv2.namedWindow("display", cv2.WINDOW_NORMAL)
+        cv2.setMouseCallback("display", self.on_mouse)
+        cv2.createTrackbar("scale", "display", 0, 100, self.on_scale)
+
+    @classmethod
+    def putText(cls, img, text, org, color = (0,0,0)):
+        cv2.putText(img, text, org, cls.FONT_FACE, cls.FONT_SCALE, color, thickness = cls.FONT_THICKNESS)
+
+    @classmethod
+    def getTextSize(cls, text):
+        return cv2.getTextSize(text, cls.FONT_FACE, cls.FONT_SCALE, cls.FONT_THICKNESS)[0]
+
+    def on_mouse(self, event, x, y, flags, param):
+        if event == cv2.EVENT_LBUTTONDOWN and self.displaywidth < x:
+            if self.c.goodenough:
+                if 180 <= y < 280:
+                    self.c.do_calibration()
+            if self.c.calibrated:
+                if 280 <= y < 380:
+                    self.c.do_save()
+                elif 380 <= y < 480:
+                    # Only shut down if we set camera info correctly, #3993
+                    if self.do_upload():
+                        rclpy.shutdown()
+
+    def on_scale(self, scalevalue):
+        if self.c.calibrated:
+            self.c.set_alpha(scalevalue / 100.0)
+
+    def button(self, dst, label, enable):
+        dst.fill(255)
+        size = (dst.shape[1], dst.shape[0])
+        if enable:
+            color = (155, 155, 80)
+        else:
+            color = (224, 224, 224)
+        cv2.circle(dst, (size[0] // 2, size[1] // 2), min(size) // 2, color, -1)
+        (w, h) = self.getTextSize(label)
+        self.putText(dst, label, ((size[0] - w) // 2, (size[1] + h) // 2), (255,255,255))
+
+    def buttons(self, display):
+        x = self.displaywidth
+        self.button(display[180:280,x:x+100], "CALIBRATE", self.c.goodenough)
+        self.button(display[280:380,x:x+100], "SAVE", self.c.calibrated)
+        self.button(display[380:480,x:x+100], "COMMIT", self.c.calibrated)
+
+    def y(self, i):
+        """Set up right-size images"""
+        return 30 + 40 * i
+
+    def screendump(self, im):
+        i = 0
+        while os.access("/tmp/dump%d.png" % i, os.R_OK):
+            i += 1
+        cv2.imwrite("/tmp/dump%d.png" % i, im)
+
+    def redraw_monocular(self, drawable):
+        height = drawable.scrib.shape[0]
+        width = drawable.scrib.shape[1]
+
+        display = numpy.zeros((max(480, height), width + 100, 3), dtype=numpy.uint8)
+        display[0:height, 0:width,:] = drawable.scrib
+        display[0:height, width:width+100,:].fill(255)
+
+
+        self.buttons(display)
+        if not self.c.calibrated:
+            if drawable.params:
+                 for i, (label, lo, hi, progress) in enumerate(drawable.params):
+                    (w,_) = self.getTextSize(label)
+                    self.putText(display, label, (width + (100 - w) // 2, self.y(i)))
+                    color = (0,255,0)
+                    if progress < 1.0:
+                        color = (0, int(progress*255.), 255)
+                    cv2.line(display,
+                            (int(width + lo * 100), self.y(i) + 20),
+                            (int(width + hi * 100), self.y(i) + 20),
+                            color, 4)
+
+        else:
+            self.putText(display, "lin.", (width, self.y(0)))
+            linerror = drawable.linear_error
+            if linerror < 0:
+                msg = "?"
+            else:
+                msg = "%.2f" % linerror
+                #print "linear", linerror
+            self.putText(display, msg, (width, self.y(1)))
+
+        self.queue_display.append(display)
+
+    def redraw_stereo(self, drawable):
+        height = drawable.lscrib.shape[0]
+        width = drawable.lscrib.shape[1]
+
+        display = numpy.zeros((max(480, height), 2 * width + 100, 3), dtype=numpy.uint8)
+        display[0:height, 0:width,:] = drawable.lscrib
+        display[0:height, width:2*width,:] = drawable.rscrib
+        display[0:height, 2*width:2*width+100,:].fill(255)
+
+        self.buttons(display)
+
+        if not self.c.calibrated:
+            if drawable.params:
+                for i, (label, lo, hi, progress) in enumerate(drawable.params):
+                    (w,_) = self.getTextSize(label)
+                    self.putText(display, label, (2 * width + (100 - w) // 2, self.y(i)))
+                    color = (0,255,0)
+                    if progress < 1.0:
+                        color = (0, int(progress*255.), 255)
+                    cv2.line(display,
+                            (int(2 * width + lo * 100), self.y(i) + 20),
+                            (int(2 * width + hi * 100), self.y(i) + 20),
+                            color, 4)
+
+        else:
+            self.putText(display, "epi.", (2 * width, self.y(0)))
+            if drawable.epierror == -1:
+                msg = "?"
+            else:
+                msg = "%.2f" % drawable.epierror
+            self.putText(display, msg, (2 * width, self.y(1)))
+            # TODO dim is never set anywhere. Supposed to be observed chessboard size?
+            if drawable.dim != -1:
+                self.putText(display, "dim", (2 * width, self.y(2)))
+                self.putText(display, "%.3f" % drawable.dim, (2 * width, self.y(3)))
+
+        self.queue_display.append(display)

build/camera_calibration/build/lib/camera_calibration/camera_checker.py

@@ -0,0 +1,201 @@
+#!/usr/bin/python
+#
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2009, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of the Willow Garage nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import cv2
+import cv_bridge
+import functools
+import message_filters
+import numpy
+import rclpy
+from rclpy.node import Node
+import sensor_msgs.msg
+import sensor_msgs.srv
+import threading
+
+from camera_calibration.calibrator import MonoCalibrator, StereoCalibrator, ChessboardInfo
+from message_filters import ApproximateTimeSynchronizer
+
+try:
+    from queue import Queue
+except ImportError:
+    from Queue import Queue
+
+
+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)
+        self.queue = queue
+        self.function = function
+
+    def run(self):
+        while rclpy.ok():
+            m = self.queue.get()
+            if self.queue.empty():
+                break
+        self.function(m)
+
+class CameraCheckerNode(Node):
+
+    def __init__(self, name, chess_size, dim, approximate=0):
+        super().__init__(name)
+        self.board = ChessboardInfo()
+        self.board.n_cols = chess_size[0]
+        self.board.n_rows = chess_size[1]
+        self.board.dim = dim
+
+        # make sure n_cols is not smaller than n_rows, otherwise error computation will be off
+        if self.board.n_cols < self.board.n_rows:
+            self.board.n_cols, self.board.n_rows = self.board.n_rows, self.board.n_cols
+
+        image_topic = "monocular/image_rect"
+        camera_topic = "monocular/camera_info"
+
+        tosync_mono = [
+            (image_topic, sensor_msgs.msg.Image),
+            (camera_topic, sensor_msgs.msg.CameraInfo),
+        ]
+
+        if approximate <= 0:
+            sync = message_filters.TimeSynchronizer
+        else:
+            sync = functools.partial(ApproximateTimeSynchronizer, slop=approximate)
+
+        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"
+        left_camera_topic = "stereo/left/camera_info"
+        right_topic = "stereo/right/image_rect"
+        right_camera_topic = "stereo/right/camera_info"
+
+        tosync_stereo = [
+            (left_topic, sensor_msgs.msg.Image),
+            (left_camera_topic, sensor_msgs.msg.CameraInfo),
+            (right_topic, sensor_msgs.msg.Image),
+            (right_camera_topic, sensor_msgs.msg.CameraInfo)
+        ]
+
+        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()
+
+        self.q_mono = Queue()
+        self.q_stereo = Queue()
+
+        mth = ConsumerThread(self.q_mono, self.handle_monocular)
+        mth.setDaemon(True)
+        mth.start()
+
+        sth = ConsumerThread(self.q_stereo, self.handle_stereo)
+        sth.setDaemon(True)
+        sth.start()
+
+        self.mc = MonoCalibrator([self.board])
+        self.sc = StereoCalibrator([self.board])
+
+    def queue_monocular(self, msg, cmsg):
+        self.q_mono.put((msg, cmsg))
+
+    def queue_stereo(self, lmsg, lcmsg, rmsg, rcmsg):
+        self.q_stereo.put((lmsg, lcmsg, rmsg, rcmsg))
+
+    def mkgray(self, msg):
+        return self.mc.mkgray(msg)
+
+    def image_corners(self, im):
+        (ok, corners, b) = self.mc.get_corners(im)
+        if ok:
+            return corners
+        else:
+            return None
+
+    def handle_monocular(self, msg):
+
+        (image, camera) = msg
+        gray = self.mkgray(image)
+        C = self.image_corners(gray)
+        if C is not None:
+            linearity_rms = self.mc.linear_error(C, self.board)
+
+            # Add in reprojection check
+            image_points = C
+            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)
+            # 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)
+            reprojected_h = camera_matrix * object_points_world
+            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))
+
+            # Print the results
+            print("Linearity RMS Error: %.3f Pixels      Reprojection RMS Error: %.3f Pixels" % (linearity_rms, reprojection_rms))
+        else:
+            print('no chessboard')
+
+    def handle_stereo(self, msg):
+
+        (lmsg, lcmsg, rmsg, rcmsg) = msg
+        lgray = self.mkgray(lmsg)
+        rgray = self.mkgray(rmsg)
+
+        L = self.image_corners(lgray)
+        R = self.image_corners(rgray)
+        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))
+
+            print("epipolar error: %f pixels   dimension: %f m" % (epipolar, dimension))
+        else:
+            print("no chessboard")

build/camera_calibration/build/lib/camera_calibration/nodes/cameracalibrator.py

@@ -0,0 +1,153 @@
+#!/usr/bin/python
+#
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2009, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of the Willow Garage nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import cv2
+import functools
+import message_filters
+import rclpy
+from camera_calibration.camera_calibrator import OpenCVCalibrationNode
+from camera_calibration.calibrator import ChessboardInfo, Patterns
+from message_filters import ApproximateTimeSynchronizer
+
+
+def main():
+    from optparse import OptionParser, OptionGroup
+    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.")
+    group.add_option("-p", "--pattern",
+                     type="string", default="chessboard",
+                     help="calibration pattern to detect - 'chessboard', 'circles', 'acircles'")
+    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")
+    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("--no-service-check",
+                     action="store_false", dest="service_check", default=True,
+                     help="disable check for set_camera_info services at startup")
+    parser.add_option_group(group)
+    group = OptionGroup(parser, "Calibration Optimizer Options")
+    group.add_option("--fix-principal-point",
+                     action="store_true", default=False,
+                     help="fix the principal point at the image center")
+    group.add_option("--fix-aspect-ratio",
+                     action="store_true", default=False,
+                     help="enforce focal lengths (fx, fy) are equal")
+    group.add_option("--zero-tangent-dist",
+                     action="store_true", default=False,
+                     help="set tangential distortion coefficients (p1, p2) to zero")
+    group.add_option("-k", "--k-coefficients",
+                     type="int", default=2, metavar="NUM_COEFFS",
+                     help="number of radial distortion coefficients to use (up to 6, default %default)")
+    group.add_option("--disable_calib_cb_fast_check", action='store_true', default=False,
+                     help="uses the CALIB_CB_FAST_CHECK flag for findChessboardCorners")
+    parser.add_option_group(group)
+    options, _ = parser.parse_args(rclpy.utilities.remove_ros_args())
+
+    if len(options.size) != len(options.square):
+        parser.error("Number of size and square inputs must be the same!")
+
+    if not options.square:
+        options.square.append("0.108")
+        options.size.append("8x6")
+
+    boards = []
+    for (sz, sq) in zip(options.size, options.square):
+        size = tuple([int(c) for c in sz.split('x')])
+        boards.append(ChessboardInfo(size[0], size[1], float(sq)))
+
+    if options.approximate == 0.0:
+        sync = message_filters.TimeSynchronizer
+    else:
+        sync = functools.partial(ApproximateTimeSynchronizer, slop=options.approximate)
+
+    num_ks = options.k_coefficients
+
+    calib_flags = 0
+    if options.fix_principal_point:
+        calib_flags |= cv2.CALIB_FIX_PRINCIPAL_POINT
+    if options.fix_aspect_ratio:
+        calib_flags |= cv2.CALIB_FIX_ASPECT_RATIO
+    if options.zero_tangent_dist:
+        calib_flags |= cv2.CALIB_ZERO_TANGENT_DIST
+    if (num_ks > 3):
+        calib_flags |= cv2.CALIB_RATIONAL_MODEL
+    if (num_ks < 6):
+        calib_flags |= cv2.CALIB_FIX_K6
+    if (num_ks < 5):
+        calib_flags |= cv2.CALIB_FIX_K5
+    if (num_ks < 4):
+        calib_flags |= cv2.CALIB_FIX_K4
+    if (num_ks < 3):
+        calib_flags |= cv2.CALIB_FIX_K3
+    if (num_ks < 2):
+        calib_flags |= cv2.CALIB_FIX_K2
+    if (num_ks < 1):
+        calib_flags |= cv2.CALIB_FIX_K1
+
+    pattern = Patterns.Chessboard
+    if options.pattern == 'circles':
+        pattern = Patterns.Circles
+    elif options.pattern == 'acircles':
+        pattern = Patterns.ACircles
+    elif options.pattern != 'chessboard':
+        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, pattern, options.camera_name,
+                                 checkerboard_flags=checkerboard_flags)
+    node.spin()
+
+if __name__ == "__main__":
+    try:
+        main()
+    except Exception as e:
+        import traceback
+        traceback.print_exc()

build/camera_calibration/build/lib/camera_calibration/nodes/cameracheck.py

@@ -0,0 +1,58 @@
+#!/usr/bin/python
+#
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2009, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of the Willow Garage nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import rclpy
+from camera_calibration.camera_checker import CameraCheckerNode
+
+
+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")
+
+    options, _ = parser.parse_args(rclpy.utilities.remove_ros_args())
+    rclpy.init()
+
+    size = tuple([int(c) for c in options.size.split('x')])
+    dim = float(options.square)
+    approximate = float(options.approximate)
+    node = CameraCheckerNode("cameracheck", size, dim, approximate)
+    rclpy.spin(node)
+
+if __name__ == "__main__":
+    main()

build/camera_calibration/camera_calibration.egg-info/PKG-INFO

@@ -0,0 +1,12 @@
+Metadata-Version: 1.2
+Name: camera-calibration
+Version: 2.2.0
+Summary: Camera_calibration allows easy calibration of monocular or stereo cameras using a checkerboard calibration target .
+Home-page: UNKNOWN
+Author: James Bowman, Patrick Mihelich
+Maintainer: Vincent Rabaud, Steven Macenski, Joshua Whitley
+Maintainer-email: vincent.rabaud@gmail.com, stevenmacenski@gmail.com, whitleysoftwareservices@gmail.com
+License: BSD
+Description: UNKNOWN
+Keywords: ROS2
+Platform: UNKNOWN

build/camera_calibration/camera_calibration.egg-info/SOURCES.txt

@@ -0,0 +1,20 @@
+package.xml
+setup.cfg
+setup.py
+../../build/camera_calibration/camera_calibration.egg-info/PKG-INFO
+../../build/camera_calibration/camera_calibration.egg-info/SOURCES.txt
+../../build/camera_calibration/camera_calibration.egg-info/dependency_links.txt
+../../build/camera_calibration/camera_calibration.egg-info/entry_points.txt
+../../build/camera_calibration/camera_calibration.egg-info/requires.txt
+../../build/camera_calibration/camera_calibration.egg-info/top_level.txt
+../../build/camera_calibration/camera_calibration.egg-info/zip-safe
+resource/camera_calibration
+src/camera_calibration/__init__.py
+src/camera_calibration/calibrator.py
+src/camera_calibration/camera_calibrator.py
+src/camera_calibration/camera_checker.py
+src/camera_calibration/nodes/__init__.py
+src/camera_calibration/nodes/cameracalibrator.py
+src/camera_calibration/nodes/cameracheck.py
+test/test_directed.py
+test/test_multiple_boards.py

build/camera_calibration/camera_calibration.egg-info/dependency_links.txt

@@ -0,0 +1 @@
+

build/camera_calibration/camera_calibration.egg-info/entry_points.txt

@@ -0,0 +1,4 @@
+[console_scripts]
+cameracalibrator = camera_calibration.nodes.cameracalibrator:main
+cameracheck = camera_calibration.nodes.cameracheck:main
+

build/camera_calibration/camera_calibration.egg-info/requires.txt

@@ -0,0 +1 @@
+setuptools

build/camera_calibration/camera_calibration.egg-info/top_level.txt

@@ -0,0 +1 @@
+camera_calibration

build/camera_calibration/camera_calibration.egg-info/zip-safe

@@ -0,0 +1 @@
+

build/camera_calibration/colcon_build.rc

@@ -0,0 +1 @@
+0

build/camera_calibration/colcon_command_prefix_setup_py.sh

@@ -0,0 +1 @@
+# generated from colcon_core/shell/template/command_prefix.sh.em

build/camera_calibration/colcon_command_prefix_setup_py.sh.env

@@ -0,0 +1,70 @@
+AMENT_PREFIX_PATH=/home/ros2/dev2_ws/install/v4l2_camera:/home/ros2/dev2_ws/install/turtle_follower_py:/home/ros2/dev2_ws/install/camera_calibration:/home/ros2/dev2_ws/install/aruco_interfaces:/home/ros2/dev2_ws/install/aruco_detector:/opt/ros/foxy
+CMAKE_PREFIX_PATH=/home/ros2/dev2_ws/install/v4l2_camera:/home/ros2/dev2_ws/install/aruco_interfaces
+COLCON=1
+COLCON_PREFIX_PATH=/home/ros2/dev2_ws/install
+COLORTERM=truecolor
+DBUS_SESSION_BUS_ADDRESS=unix:path=/run/user/1000/bus
+DESKTOP_SESSION=ubuntu
+DISPLAY=:0
+GDMSESSION=ubuntu
+GJS_DEBUG_OUTPUT=stderr
+GJS_DEBUG_TOPICS=JS ERROR;JS LOG
+GNOME_DESKTOP_SESSION_ID=this-is-deprecated
+GNOME_SHELL_SESSION_MODE=ubuntu
+GNOME_TERMINAL_SCREEN=/org/gnome/Terminal/screen/ed9a88cb_996e_4783_90fb_206aa627c957
+GNOME_TERMINAL_SERVICE=:1.166
+GPG_AGENT_INFO=/run/user/1000/gnupg/S.gpg-agent:0:1
+GTK_MODULES=gail:atk-bridge
+HOME=/home/ros2
+IM_CONFIG_PHASE=1
+INVOCATION_ID=331653316740409188736b6261a239d2
+JOURNAL_STREAM=8:183562
+LANG=en_US.UTF-8
+LC_ADDRESS=de_DE.UTF-8
+LC_ALL=en_US.UTF-8
+LC_IDENTIFICATION=de_DE.UTF-8
+LC_MEASUREMENT=de_DE.UTF-8
+LC_MONETARY=de_DE.UTF-8
+LC_NAME=de_DE.UTF-8
+LC_NUMERIC=de_DE.UTF-8
+LC_PAPER=de_DE.UTF-8
+LC_TELEPHONE=de_DE.UTF-8
+LC_TIME=de_DE.UTF-8
+LD_LIBRARY_PATH=/home/ros2/dev2_ws/install/v4l2_camera/lib:/home/ros2/dev2_ws/install/aruco_interfaces/lib:/opt/ros/foxy/opt/yaml_cpp_vendor/lib:/opt/ros/foxy/opt/rviz_ogre_vendor/lib:/opt/ros/foxy/lib/x86_64-linux-gnu:/opt/ros/foxy/lib
+LESSCLOSE=/usr/bin/lesspipe %s %s
+LESSOPEN=| /usr/bin/lesspipe %s
+LOGNAME=ros2
+LS_COLORS=rs=0:di=01;34:ln=01;36:mh=00:pi=40;33:so=01;35:do=01;35:bd=40;33;01:cd=40;33;01:or=40;31;01:mi=00:su=37;41:sg=30;43:ca=30;41:tw=30;42:ow=34;42:st=37;44:ex=01;32:*.tar=01;31:*.tgz=01;31:*.arc=01;31:*.arj=01;31:*.taz=01;31:*.lha=01;31:*.lz4=01;31:*.lzh=01;31:*.lzma=01;31:*.tlz=01;31:*.txz=01;31:*.tzo=01;31:*.t7z=01;31:*.zip=01;31:*.z=01;31:*.dz=01;31:*.gz=01;31:*.lrz=01;31:*.lz=01;31:*.lzo=01;31:*.xz=01;31:*.zst=01;31:*.tzst=01;31:*.bz2=01;31:*.bz=01;31:*.tbz=01;31:*.tbz2=01;31:*.tz=01;31:*.deb=01;31:*.rpm=01;31:*.jar=01;31:*.war=01;31:*.ear=01;31:*.sar=01;31:*.rar=01;31:*.alz=01;31:*.ace=01;31:*.zoo=01;31:*.cpio=01;31:*.7z=01;31:*.rz=01;31:*.cab=01;31:*.wim=01;31:*.swm=01;31:*.dwm=01;31:*.esd=01;31:*.jpg=01;35:*.jpeg=01;35:*.mjpg=01;35:*.mjpeg=01;35:*.gif=01;35:*.bmp=01;35:*.pbm=01;35:*.pgm=01;35:*.ppm=01;35:*.tga=01;35:*.xbm=01;35:*.xpm=01;35:*.tif=01;35:*.tiff=01;35:*.png=01;35:*.svg=01;35:*.svgz=01;35:*.mng=01;35:*.pcx=01;35:*.mov=01;35:*.mpg=01;35:*.mpeg=01;35:*.m2v=01;35:*.mkv=01;35:*.webm=01;35:*.ogm=01;35:*.mp4=01;35:*.m4v=01;35:*.mp4v=01;35:*.vob=01;35:*.qt=01;35:*.nuv=01;35:*.wmv=01;35:*.asf=01;35:*.rm=01;35:*.rmvb=01;35:*.flc=01;35:*.avi=01;35:*.fli=01;35:*.flv=01;35:*.gl=01;35:*.dl=01;35:*.xcf=01;35:*.xwd=01;35:*.yuv=01;35:*.cgm=01;35:*.emf=01;35:*.ogv=01;35:*.ogx=01;35:*.aac=00;36:*.au=00;36:*.flac=00;36:*.m4a=00;36:*.mid=00;36:*.midi=00;36:*.mka=00;36:*.mp3=00;36:*.mpc=00;36:*.ogg=00;36:*.ra=00;36:*.wav=00;36:*.oga=00;36:*.opus=00;36:*.spx=00;36:*.xspf=00;36:
+MANAGERPID=7832
+OLDPWD=/home/ros2/dev2_ws/launch
+PAPERSIZE=a4
+PATH=/opt/ros/foxy/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin:/usr/games:/usr/local/games:/snap/bin
+PWD=/home/ros2/dev2_ws/build/camera_calibration
+PYTHONPATH=/home/ros2/dev2_ws/install/turtle_follower_py/lib/python3.8/site-packages:/home/ros2/dev2_ws/install/camera_calibration/lib/python3.8/site-packages:/home/ros2/dev2_ws/install/aruco_interfaces/lib/python3.8/site-packages:/home/ros2/dev2_ws/install/aruco_detector/lib/python3.8/site-packages:/opt/ros/foxy/lib/python3.8/site-packages
+QT_ACCESSIBILITY=1
+QT_IM_MODULE=ibus
+ROS_DISTRO=foxy
+ROS_LOCALHOST_ONLY=0
+ROS_PYTHON_VERSION=3
+ROS_VERSION=2
+SESSION_MANAGER=local/ubuntu:@/tmp/.ICE-unix/8042,unix/ubuntu:/tmp/.ICE-unix/8042
+SHELL=/bin/bash
+SHLVL=1
+SSH_AGENT_PID=8007
+SSH_AUTH_SOCK=/run/user/1000/keyring/ssh
+TERM=xterm-256color
+USER=ros2
+USERNAME=ros2
+VTE_VERSION=6003
+WINDOWPATH=2
+XAUTHORITY=/run/user/1000/gdm/Xauthority
+XDG_CONFIG_DIRS=/etc/xdg/xdg-ubuntu:/etc/xdg
+XDG_CURRENT_DESKTOP=ubuntu:GNOME
+XDG_DATA_DIRS=/usr/share/ubuntu:/usr/local/share/:/usr/share/:/var/lib/snapd/desktop
+XDG_MENU_PREFIX=gnome-
+XDG_RUNTIME_DIR=/run/user/1000
+XDG_SESSION_CLASS=user
+XDG_SESSION_DESKTOP=ubuntu
+XDG_SESSION_TYPE=x11
+XMODIFIERS=@im=ibus
+_=/usr/bin/colcon

build/camera_calibration/install.log

@@ -0,0 +1,25 @@
+/home/ros2/dev2_ws/install/camera_calibration/lib/python3.8/site-packages/camera_calibration/__init__.py
+/home/ros2/dev2_ws/install/camera_calibration/lib/python3.8/site-packages/camera_calibration/camera_calibrator.py
+/home/ros2/dev2_ws/install/camera_calibration/lib/python3.8/site-packages/camera_calibration/camera_checker.py
+/home/ros2/dev2_ws/install/camera_calibration/lib/python3.8/site-packages/camera_calibration/calibrator.py
+/home/ros2/dev2_ws/install/camera_calibration/lib/python3.8/site-packages/camera_calibration/nodes/cameracheck.py
+/home/ros2/dev2_ws/install/camera_calibration/lib/python3.8/site-packages/camera_calibration/nodes/__init__.py
+/home/ros2/dev2_ws/install/camera_calibration/lib/python3.8/site-packages/camera_calibration/nodes/cameracalibrator.py
+/home/ros2/dev2_ws/install/camera_calibration/lib/python3.8/site-packages/camera_calibration/__pycache__/__init__.cpython-38.pyc
+/home/ros2/dev2_ws/install/camera_calibration/lib/python3.8/site-packages/camera_calibration/__pycache__/camera_calibrator.cpython-38.pyc
+/home/ros2/dev2_ws/install/camera_calibration/lib/python3.8/site-packages/camera_calibration/__pycache__/camera_checker.cpython-38.pyc
+/home/ros2/dev2_ws/install/camera_calibration/lib/python3.8/site-packages/camera_calibration/__pycache__/calibrator.cpython-38.pyc
+/home/ros2/dev2_ws/install/camera_calibration/lib/python3.8/site-packages/camera_calibration/nodes/__pycache__/cameracheck.cpython-38.pyc
+/home/ros2/dev2_ws/install/camera_calibration/lib/python3.8/site-packages/camera_calibration/nodes/__pycache__/__init__.cpython-38.pyc
+/home/ros2/dev2_ws/install/camera_calibration/lib/python3.8/site-packages/camera_calibration/nodes/__pycache__/cameracalibrator.cpython-38.pyc
+/home/ros2/dev2_ws/install/camera_calibration/share/ament_index/resource_index/packages/camera_calibration
+/home/ros2/dev2_ws/install/camera_calibration/share/camera_calibration/package.xml
+/home/ros2/dev2_ws/install/camera_calibration/lib/python3.8/site-packages/camera_calibration-2.2.0-py3.8.egg-info/requires.txt
+/home/ros2/dev2_ws/install/camera_calibration/lib/python3.8/site-packages/camera_calibration-2.2.0-py3.8.egg-info/top_level.txt
+/home/ros2/dev2_ws/install/camera_calibration/lib/python3.8/site-packages/camera_calibration-2.2.0-py3.8.egg-info/dependency_links.txt
+/home/ros2/dev2_ws/install/camera_calibration/lib/python3.8/site-packages/camera_calibration-2.2.0-py3.8.egg-info/PKG-INFO
+/home/ros2/dev2_ws/install/camera_calibration/lib/python3.8/site-packages/camera_calibration-2.2.0-py3.8.egg-info/entry_points.txt
+/home/ros2/dev2_ws/install/camera_calibration/lib/python3.8/site-packages/camera_calibration-2.2.0-py3.8.egg-info/zip-safe
+/home/ros2/dev2_ws/install/camera_calibration/lib/python3.8/site-packages/camera_calibration-2.2.0-py3.8.egg-info/SOURCES.txt
+/home/ros2/dev2_ws/install/camera_calibration/lib/camera_calibration/cameracalibrator
+/home/ros2/dev2_ws/install/camera_calibration/lib/camera_calibration/cameracheck