Пример #1
0
    def test_multiple_boards(self):
        small_board = ChessboardInfo()
        small_board.n_cols = 5
        small_board.n_rows = 4
        small_board.dim = 0.025

        stereo_cal = StereoCalibrator([board, small_board])

        my_archive_name = roslib.packages.find_resource(
            'camera_calibration', 'multi_board_calibration.tar.gz')[0]
        stereo_cal.do_tarfile_calibration(my_archive_name)

        stereo_cal.report()
        stereo_cal.ost()

        # Check error for big image
        archive = tarfile.open(my_archive_name)
        l1_big = image_from_archive(archive, "left-0000.png")
        r1_big = image_from_archive(archive, "right-0000.png")
        epi_big = stereo_cal.epipolar_error_from_images(l1_big, r1_big)
        self.assert_(
            epi_big < 1.0,
            "Epipolar error for large checkerboard > 1.0. Error: %.2f" %
            epi_big)

        # Small checkerboard has larger error threshold for now
        l1_sm = image_from_archive(archive, "left-0012-sm.png")
        r1_sm = image_from_archive(archive, "right-0012-sm.png")
        epi_sm = stereo_cal.epipolar_error_from_images(l1_sm, r1_sm)
        self.assert_(
            epi_sm < 2.0,
            "Epipolar error for small checkerboard > 2.0. Error: %.2f" %
            epi_sm)
Пример #2
0
    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,
                    detection=self._detection,
                    output=self._output,
                    checkerboard_flags=self._checkerboard_flags,
                    min_good_enough=self._min_good_enough)
            else:
                self.c = MonoCalibrator(
                    self._boards,
                    self._calib_flags,
                    self._pattern,
                    detection=self._detection,
                    output=self._output,
                    checkerboard_flags=self.checkerboard_flags,
                    min_good_enough=self._min_good_enough)

        # This should just call the MonoCalibrator
        drawable = self.c.handle_msg(msg)
        self.displaywidth = drawable.scrib.shape[1]
        self.redraw_monocular(drawable)
Пример #3
0
    def handle_stereo(self, msg):
        if self.c == None:
            self.c = StereoCalibrator(self._boards, self._calib_flags)

        drawable = self.c.handle_msg(msg)
        self.displaywidth = drawable.lscrib.cols + drawable.rscrib.cols
        self.redraw_stereo(drawable)
Пример #4
0
    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,
                    detection=self._detection,
                    output=self._output,
                    checkerboard_flags=self._checkerboard_flags,
                    min_good_enough=self._min_good_enough)
            else:
                self.c = StereoCalibrator(
                    self._boards,
                    self._calib_flags,
                    self._pattern,
                    detection=self._detection,
                    output=self._output,
                    checkerboard_flags=self.checkerboard_flags,
                    min_good_enough=self._min_good_enough)

        drawable = self.c.handle_msg(msg)
        self.displaywidth = drawable.lscrib.shape[1] + drawable.rscrib.shape[1]
        self.redraw_stereo(drawable)
Пример #5
0
    def __init__(self, chess_size, dim, approximate=0):
        self.board = ChessboardInfo()
        self.board.n_cols = chess_size[0]
        self.board.n_rows = chess_size[1]
        self.board.dim = dim

        image_topic = rospy.resolve_name("monocular") + "/image_rect"
        camera_topic = rospy.resolve_name("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(topic, type)
            for (topic, type) in tosync_mono
        ], 10)
        tsm.registerCallback(self.queue_monocular)

        left_topic = rospy.resolve_name("stereo") + "/left/image_rect"
        left_camera_topic = rospy.resolve_name("stereo") + "/left/camera_info"
        right_topic = rospy.resolve_name("stereo") + "/right/image_rect"
        right_camera_topic = rospy.resolve_name(
            "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(topic, type)
            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])
Пример #6
0
    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)
            else:
                self.c = StereoCalibrator(self._boards, self._calib_flags, self._pattern)

        drawable = self.c.handle_msg(msg)
        self.displaywidth = drawable.lscrib.cols + drawable.rscrib.cols
        self.redraw_stereo(drawable)
Пример #7
0
    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)
            else:
                self.c = StereoCalibrator(self._boards, self._calib_flags, self._pattern)

        drawable = self.c.handle_msg(msg)
        self.displaywidth = drawable.lscrib.shape[1] + drawable.rscrib.shape[1]
        self.redraw_stereo(drawable)
Пример #8
0
    def test_multiple_boards(self):
        small_board = ChessboardInfo()
        small_board.n_cols = 5
        small_board.n_rows = 4
        small_board.dim = 0.025

        stereo_cal = StereoCalibrator([board, small_board])
        
        my_archive_name = roslib.packages.find_resource('camera_calibration', 'multi_board_calibration.tar.gz')[0]
        stereo_cal.do_tarfile_calibration(my_archive_name)

        stereo_cal.report()
        stereo_cal.ost()
        
        # Check error for big image
        archive = tarfile.open(my_archive_name)
        l1_big = image_from_archive(archive, "left-0000.png")
        r1_big = image_from_archive(archive, "right-0000.png")
        epi_big = stereo_cal.epipolar_error_from_images(l1_big, r1_big)
        self.assert_(epi_big < 1.0, "Epipolar error for large checkerboard > 1.0. Error: %.2f" % epi_big)

        # Small checkerboard has larger error threshold for now
        l1_sm = image_from_archive(archive, "left-0012-sm.png")
        r1_sm = image_from_archive(archive, "right-0012-sm.png")
        epi_sm =  stereo_cal.epipolar_error_from_images(l1_sm, r1_sm)
        self.assert_(epi_sm < 2.0, "Epipolar error for small checkerboard > 2.0. Error: %.2f" % epi_sm)
Пример #9
0
    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, distortion_model=self._distortion_model, checkerboard_flags=self._checkerboard_flags)
            else:
                self.c = MonoCalibrator(self._boards, self._calib_flags, self._pattern, distortion_model=self._distortion_model, 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)
Пример #10
0
    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)
            else:
                self.c = MonoCalibrator(self._boards, self._calib_flags, self._pattern)

        # This should just call the MonoCalibrator
        drawable = self.c.handle_msg(msg)
        self.displaywidth = drawable.scrib.cols
        self.redraw_monocular(drawable)
Пример #11
0
    def test_nochecker(self):
        # Run with same images, but looking for an incorrect chessboard size (8, 7).
        # Should raise an exception because of lack of input points.
        new_board = copy.deepcopy(board)
        new_board.n_cols = 8
        new_board.n_rows = 7

        sc = StereoCalibrator([new_board])
        self.assertRaises(CalibrationException, lambda: sc.cal(self.limages, self.rimages))
        mc = MonoCalibrator([new_board])
        self.assertRaises(CalibrationException, lambda: mc.cal(self.limages))
Пример #12
0
    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, distortion_model=self._distortion_model,  checkerboard_flags=self._checkerboard_flags)
            else:
                self.c = StereoCalibrator(self._boards, self._calib_flags, self._pattern, distortion_model=self._distortion_model,
                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 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)
Пример #14
0
    def handle_stereo(self, msg):
        if self.c == None:
            if self._camera_name:
                self.c = StereoCalibrator(self._boards, self._calib_flags, self._fisheye_calib_flags, self._pattern, name=self._camera_name,
                                          checkerboard_flags=self._checkerboard_flags,
                                          max_chessboard_speed = self._max_chessboard_speed)
            else:
                self.c = StereoCalibrator(self._boards, self._calib_flags, self._fisheye_calib_flags, self._pattern,
                                          checkerboard_flags=self._checkerboard_flags,
                                          max_chessboard_speed = self._max_chessboard_speed)

        drawable = self.c.handle_msg(msg)
        self.displaywidth = drawable.lscrib.shape[1] + drawable.rscrib.shape[1]
        self.redraw_stereo(drawable)
Пример #15
0
    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)
            else:
                self.c = MonoCalibrator(self._boards, self._calib_flags,
                                        self._pattern)

        # This should just call the MonoCalibrator
        drawable = self.c.handle_msg(msg)
        self.displaywidth = drawable.scrib.cols
        self.redraw_monocular(drawable)
Пример #16
0
    def handle_monocular(self, msg):
        if self.c == None:
            if self._camera_name:
                self.c = MonoCalibrator(self._boards, self._calib_flags, self._fisheye_calib_flags, self._pattern, name=self._camera_name,
                                        checkerboard_flags=self._checkerboard_flags,
                                        max_chessboard_speed = self._max_chessboard_speed)
            else:
                self.c = MonoCalibrator(self._boards, self._calib_flags, self._fisheye_calib_flags, self._pattern,
                                        checkerboard_flags=self.checkerboard_flags,
                                        max_chessboard_speed = self._max_chessboard_speed)

        # This should just call the MonoCalibrator
        drawable = self.c.handle_msg(msg)
        self.displaywidth = drawable.scrib.shape[1]
        self.redraw_monocular(drawable)
Пример #17
0
def cal_from_tarfile(boards, tarname, mono = False, upload = False):
    if mono:
        calibrator = MonoCalibrator(boards)
    else:
        calibrator = StereoCalibrator(boards)

    calibrator.do_tarfile_calibration(tarname)

    print calibrator.ost()

    if upload: 
        info = calibrator.as_message()
        if mono:
            set_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("camera"), sensor_msgs.srv.SetCameraInfo)

            response = set_camera_info_service(info)
            if not response.success:
                raise RuntimeError("connected to set_camera_info service, but failed setting camera_info")
        else:
            set_left_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("left_camera"), sensor_msgs.srv.SetCameraInfo)
            set_right_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("right_camera"), sensor_msgs.srv.SetCameraInfo)

            response1 = set_left_camera_info_service(info[0])
            response2 = set_right_camera_info_service(info[1])
            if not (response1.success and response2.success):
                raise RuntimeError("connected to set_camera_info service, but failed setting camera_info")
Пример #18
0
    def handle_stereo(self, msg):

        (lmsg, lcmsg, rmsg, rcmsg) = msg
        lgray = self.mkgray(lmsg)
        rgray = self.mkgray(rmsg)

        sc = StereoCalibrator([self.board])

        L = self.image_corners(lgray)
        R = self.image_corners(rgray)
        scm = image_geometry.StereoCameraModel()
        scm.fromCameraInfo(lcmsg, rcmsg)
        if L and R:
            d = [(y0 - y1) for ((_, y0), (_, y1)) in zip(L, R)]
            epipolar = math.sqrt(sum([i**2 for i in d]) / len(d))

            disparities = [(x0 - x1) for ((x0, y0), (x1, y1)) in zip(L, R)]
            pt3d = [scm.projectPixelTo3d((x, y), d) for ((x, y), d) in zip(L, disparities)]
            def l2(p0, p1):
                return math.sqrt(sum([(c0 - c1) ** 2 for (c0, c1) in zip(p0, p1)]))

            # Compute the length from each horizontal and vertical lines, and return the mean
            cc = self.board.n_cols
            cr = self.board.n_rows
            lengths = (
                [l2(pt3d[cc * r + 0], pt3d[cc * r + (cc - 1)]) / (cc - 1) for r in range(cr)] +
                [l2(pt3d[c + 0], pt3d[c + (cc * (cr - 1))]) / (cr - 1) for c in range(cc)])
            dimension = sum(lengths) / len(lengths)

            print "epipolar error: %f pixels   dimension: %f m" % (epipolar, dimension)
        else:
            print "no chessboard"
Пример #19
0
def cal_from_tarfile(boards, tarname, mono=False):
    if mono:
        calibrator = MonoCalibrator(boards)
    else:
        calibrator = StereoCalibrator(boards)

    calibrator.do_tarfile_calibration(tarname)

    print calibrator.ost()
    def test_multiple_boards(self):
        small_board = ChessboardInfo()
        small_board.n_cols = 5
        small_board.n_rows = 4
        small_board.dim = 0.025

        stereo_cal = StereoCalibrator([board, small_board])
        if not os.path.isfile('/tmp/multi_board_calibration.tar.gz'):
            url = 'http://download.ros.org/data/camera_calibration/multi_board_calibration.tar.gz'
            r = requests.get(url, allow_redirects=True)
            with open('/tmp/multi_board_calibration.tar.gz', 'wb') as mcf:
                mcf.write(r.content)

        my_archive_name = '/tmp/multi_board_calibration.tar.gz'
        stereo_cal.do_tarfile_calibration(my_archive_name)

        stereo_cal.report()
        stereo_cal.ost()

        # Check error for big image
        archive = tarfile.open(my_archive_name)
        l1_big = image_from_archive(archive, "left-0000.png")
        r1_big = image_from_archive(archive, "right-0000.png")
        epi_big = stereo_cal.epipolar_error_from_images(l1_big, r1_big)
        self.assertTrue(
            epi_big < 1.0,
            "Epipolar error for large checkerboard > 1.0. Error: %.2f" %
            epi_big)

        # Small checkerboard has larger error threshold for now
        l1_sm = image_from_archive(archive, "left-0012-sm.png")
        r1_sm = image_from_archive(archive, "right-0012-sm.png")
        epi_sm = stereo_cal.epipolar_error_from_images(l1_sm, r1_sm)
        self.assertTrue(
            epi_sm < 2.0,
            "Epipolar error for small checkerboard > 2.0. Error: %.2f" %
            epi_sm)
Пример #21
0
    def __init__(self, chess_size, dim):
        self.board = ChessboardInfo()
        self.board.n_cols = chess_size[0]
        self.board.n_rows = chess_size[1]
        self.board.dim = dim

        image_topic = rospy.resolve_name("monocular") + "/image_rect"
        camera_topic = rospy.resolve_name("monocular") + "/camera_info"

        tosync_mono = [
            (image_topic, sensor_msgs.msg.Image),
            (camera_topic, sensor_msgs.msg.CameraInfo),
        ]

        tsm = message_filters.TimeSynchronizer([message_filters.Subscriber(topic, type) for (topic, type) in tosync_mono], 10)
        tsm.registerCallback(self.queue_monocular)

        left_topic = rospy.resolve_name("stereo") + "/left/image_rect"
        left_camera_topic = rospy.resolve_name("stereo") + "/left/camera_info"
        right_topic = rospy.resolve_name("stereo") + "/right/image_rect"
        right_camera_topic = rospy.resolve_name("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 = message_filters.TimeSynchronizer([message_filters.Subscriber(topic, type) 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])
Пример #22
0
class CameraCheckerNode:

    def __init__(self, chess_size, dim):
        self.board = ChessboardInfo()
        self.board.n_cols = chess_size[0]
        self.board.n_rows = chess_size[1]
        self.board.dim = dim

        image_topic = rospy.resolve_name("monocular") + "/image_rect"
        camera_topic = rospy.resolve_name("monocular") + "/camera_info"

        tosync_mono = [
            (image_topic, sensor_msgs.msg.Image),
            (camera_topic, sensor_msgs.msg.CameraInfo),
        ]

        tsm = message_filters.TimeSynchronizer([message_filters.Subscriber(topic, type) for (topic, type) in tosync_mono], 10)
        tsm.registerCallback(self.queue_monocular)

        left_topic = rospy.resolve_name("stereo") + "/left/image_rect"
        left_camera_topic = rospy.resolve_name("stereo") + "/left/camera_info"
        right_topic = rospy.resolve_name("stereo") + "/right/image_rect"
        right_camera_topic = rospy.resolve_name("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 = message_filters.TimeSynchronizer([message_filters.Subscriber(topic, type) 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:
            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.T
            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 and R:
            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")
Пример #23
0
class CameraCheckerNode:

    def __init__(self, chess_size, dim):
        self.board = ChessboardInfo()
        self.board.n_cols = chess_size[0]
        self.board.n_rows = chess_size[1]
        self.board.dim = dim

        image_topic = rospy.resolve_name("monocular") + "/image_rect"
        camera_topic = rospy.resolve_name("monocular") + "/camera_info"

        tosync_mono = [
            (image_topic, sensor_msgs.msg.Image),
            (camera_topic, sensor_msgs.msg.CameraInfo),
        ]

        tsm = message_filters.TimeSynchronizer([message_filters.Subscriber(topic, type) for (topic, type) in tosync_mono], 10)
        tsm.registerCallback(self.queue_monocular)

        left_topic = rospy.resolve_name("stereo") + "/left/image_rect"
        left_camera_topic = rospy.resolve_name("stereo") + "/left/camera_info"
        right_topic = rospy.resolve_name("stereo") + "/right/image_rect"
        right_camera_topic = rospy.resolve_name("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 = message_filters.TimeSynchronizer([message_filters.Subscriber(topic, type) 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 and R:
            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")
Пример #24
0
class CalibrationNode:
    def __init__(self,
                 boards,
                 service_check=True,
                 synchronizer=message_filters.TimeSynchronizer,
                 flags=0,
                 pattern=Patterns.Chessboard,
                 camera_name='',
                 detection='cv2',
                 output='yaml',
                 checkerboard_flags=0,
                 min_good_enough=40):
        if service_check:
            # assume any non-default service names have been set.  Wait for the service to become ready
            for svcname in ["camera", "left_camera", "right_camera"]:
                remapped = rospy.remap_name(svcname)
                if remapped != svcname:
                    fullservicename = "%s/set_camera_info" % remapped
                    print("Waiting for service", fullservicename, "...")
                    try:
                        rospy.wait_for_service(fullservicename, 5)
                        print("OK")
                    except rospy.ROSException:
                        print("Service not found")
                        rospy.signal_shutdown('Quit')
        self._boards = boards
        self._detection = detection
        self._output = output
        self._calib_flags = flags
        self._checkerboard_flags = checkerboard_flags
        self._pattern = pattern
        self._camera_name = camera_name
        self._min_good_enough = min_good_enough
        rospack = rospkg.RosPack()
        pkg_path = rospack.get_path('calibration')
        self._autoware_image = cv2.imread(
            path.join(pkg_path, 'docs/smartcar_logo.png'),
            cv2.IMREAD_UNCHANGED)
        self._autoware_image = cv2.resize(self._autoware_image, (100, 25))
        lsub = message_filters.Subscriber('left', sensor_msgs.msg.Image)
        rsub = message_filters.Subscriber('right', sensor_msgs.msg.Image)
        ts = synchronizer([lsub, rsub], 4)
        ts.registerCallback(self.queue_stereo)

        msub = message_filters.Subscriber('image', sensor_msgs.msg.Image)
        msub.registerCallback(self.queue_monocular)

        self.set_camera_info_service = rospy.ServiceProxy(
            "%s/set_camera_info" % rospy.remap_name("camera"),
            sensor_msgs.srv.SetCameraInfo)
        self.set_left_camera_info_service = rospy.ServiceProxy(
            "%s/set_camera_info" % rospy.remap_name("left_camera"),
            sensor_msgs.srv.SetCameraInfo)
        self.set_right_camera_info_service = rospy.ServiceProxy(
            "%s/set_camera_info" % rospy.remap_name("right_camera"),
            sensor_msgs.srv.SetCameraInfo)

        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,
                    detection=self._detection,
                    output=self._output,
                    checkerboard_flags=self._checkerboard_flags,
                    min_good_enough=self._min_good_enough)
            else:
                self.c = MonoCalibrator(
                    self._boards,
                    self._calib_flags,
                    self._pattern,
                    detection=self._detection,
                    output=self._output,
                    checkerboard_flags=self.checkerboard_flags,
                    min_good_enough=self._min_good_enough)

        # 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,
                    detection=self._detection,
                    output=self._output,
                    checkerboard_flags=self._checkerboard_flags,
                    min_good_enough=self._min_good_enough)
            else:
                self.c = StereoCalibrator(
                    self._boards,
                    self._calib_flags,
                    self._pattern,
                    detection=self._detection,
                    output=self._output,
                    checkerboard_flags=self.checkerboard_flags,
                    min_good_enough=self._min_good_enough)

        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.success:
            return True

        for i in range(10):
            print("!" * 80)
        print()
        print("Attempt to set camera info failed: " + response.status_message)
        print()
        for i in range(10):
            print("!" * 80)
        print()
        rospy.logerr(
            'Unable to set camera info for calibration. Failure message: %s' %
            response.status_message)
        return False

    def do_upload(self):
        self.c.report()
        print(self.c.ost())
        info = self.c.as_message()

        rv = True
        if self.c.is_mono:
            response = self.set_camera_info_service(info)
            rv = self.check_set_camera_info(response)
        else:
            response = self.set_left_camera_info_service(info[0])
            rv = rv and self.check_set_camera_info(response)
            response = self.set_right_camera_info_service(info[1])
            rv = rv and self.check_set_camera_info(response)
        return rv
Пример #25
0
class CalibrationNode:
    def __init__(self,
                 boards,
                 service_check=True,
                 synchronizer=message_filters.TimeSynchronizer,
                 flags=0,
                 pattern=Patterns.Chessboard,
                 camera_name='',
                 checkerboard_flags=0):
        if service_check:
            # assume any non-default service names have been set.  Wait for the service to become ready
            for svcname in ["camera", "left_camera", "right_camera"]:
                remapped = rospy.remap_name(svcname)
                if remapped != svcname:
                    fullservicename = "%s/set_camera_info" % remapped
                    print("Waiting for service", fullservicename, "...")
                    try:
                        rospy.wait_for_service(fullservicename, 5)
                        print("OK")
                    except rospy.ROSException:
                        print("Service not found")
                        rospy.signal_shutdown('Quit')

        self._boards = boards
        self._calib_flags = flags
        self._checkerboard_flags = checkerboard_flags
        self._pattern = pattern
        self._camera_name = camera_name
        lsub = message_filters.Subscriber('left', sensor_msgs.msg.Image)
        rsub = message_filters.Subscriber('right', sensor_msgs.msg.Image)
        ts = synchronizer([lsub, rsub], 4)
        ts.registerCallback(self.queue_stereo)

        msub = message_filters.Subscriber('image', sensor_msgs.msg.Image)
        msub.registerCallback(self.queue_monocular)

        self.set_camera_info_service = rospy.ServiceProxy(
            "%s/set_camera_info" % rospy.remap_name("camera"),
            sensor_msgs.srv.SetCameraInfo)
        self.set_left_camera_info_service = rospy.ServiceProxy(
            "%s/set_camera_info" % rospy.remap_name("left_camera"),
            sensor_msgs.srv.SetCameraInfo)
        self.set_right_camera_info_service = rospy.ServiceProxy(
            "%s/set_camera_info" % rospy.remap_name("right_camera"),
            sensor_msgs.srv.SetCameraInfo)

        self.q_mono = deque([], 1)
        self.q_stereo = deque([], 1)

        self.c = None

        # add publisher when received the corner information TODO:
        self.left_pub = rospy.Publisher('/left_corners',
                                        Float32MultiArray,
                                        queue_size=100)
        self.right_pub = rospy.Publisher('/right_corners',
                                         Float32MultiArray,
                                         queue_size=100)

        self.lcorner_size_pub = rospy.Publisher('/get_corner_size_l',
                                                Int32,
                                                queue_size=100)
        self.rcorner_size_pub = rospy.Publisher('/get_corner_size_r',
                                                Int32,
                                                queue_size=100)
        # self.intrinsic_pub = rospy.Publisher('/camera_intrinsics', intrinsic_param, queue_size = 100)

        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 convert_point2f_to_list(
            self, input_corners
    ):  # change cv::Point2f(numpy.ndarray in python) to list

        corner_Xs = input_corners[:, :, 0]
        corner_Ys = input_corners[:, :, 1]
        new_corner = zip(corner_Xs, corner_Ys)

        corner_tuple = tuple(new_corner)
        return corner_tuple

    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)

        # added lines TODO:
        # left_temp = points()   #get msg type from corners
        # right_temp = points()   #get msg type from corners
        # corner_msgs = corners()

        lcorner_size_msg = Int32()
        rcorner_size_msg = Int32()

        # The mat is giving the ready-to-publish corner coordinates, push everything in a mat TODO:
        if drawable.lcorner is not None:

            lcorner_size = len(drawable.lcorner)

            # publish the coner size for computation
            lcorner_size_msg.data = lcorner_size
            self.lcorner_size_pub.publish(lcorner_size_msg)

            left_mat = Float32MultiArray()
            left_mat.layout.dim.append(MultiArrayDimension())
            left_mat.layout.dim.append(MultiArrayDimension())
            left_mat.layout.dim[0].label = "row"
            left_mat.layout.dim[1].label = "col"
            left_mat.layout.dim[
                0].size = lcorner_size  # if there are 9 points give 9 dimension
            left_mat.layout.dim[1].size = lcorner_size
            left_mat.layout.dim[0].stride = lcorner_size
            left_mat.layout.dim[1].stride = lcorner_size
            left_mat.layout.data_offset = 0
            left_mat.data = [0] * lcorner_size * lcorner_size
            dstride1 = left_mat.layout.dim[1].stride
            offset = left_mat.layout.data_offset

            print("left")  # debug
            print(drawable.lcorner)  # debug
            i_l = 0
            for temp_left in drawable.lcorner:
                temp_x = temp_left[0, 0]
                temp_y = temp_left[0, 1]
                left_mat.data[offset + i_l + dstride1 * 0] = temp_x
                left_mat.data[offset + i_l + dstride1 * 1] = temp_y
                i_l += 1

            self.left_pub.publish(left_mat)
        else:
            print()
            print("No LEFT corner coordinates")
            print()

        if drawable.rcorner is not None:

            rcorner_size = len(drawable.rcorner)

            rcorner_size_msg.data = rcorner_size
            self.rcorner_size_pub.publish(rcorner_size_msg)

            right_mat = Float32MultiArray()
            right_mat.layout.dim.append(MultiArrayDimension())
            right_mat.layout.dim.append(MultiArrayDimension())
            right_mat.layout.dim[0].label = "row"
            right_mat.layout.dim[1].label = "col"
            right_mat.layout.dim[0].size = rcorner_size
            right_mat.layout.dim[1].size = rcorner_size
            right_mat.layout.dim[0].stride = rcorner_size
            right_mat.layout.dim[1].stride = rcorner_size
            right_mat.layout.data_offset = 0
            right_mat.data = [0] * rcorner_size * rcorner_size
            dstride1 = right_mat.layout.dim[1].stride
            offset = right_mat.layout.data_offset

            print("right")  # debug
            print(drawable.rcorner)  #debug
            i_r = 0
            for temp_right in drawable.rcorner:
                temp_x = temp_right[0, 0]
                temp_y = temp_right[0, 1]
                right_mat.data[offset + i_r + dstride1 * 0] = temp_x
                right_mat.data[offset + i_r + dstride1 * 1] = temp_y
                i_r += 1

            self.right_pub.publish(right_mat)

        else:
            print()
            print("No RIGHT corner coordinates")
            print()

        # tryp to publisht the intrinsic matrix for left and right camera
        # if self.c.is_mono:
        #     pass
        # else:
        #     stereo_cam_info = self.c.as_message()

        #     left_response = self.set_left_camera_info_service(stereo_cam_info[0])
        #     left_intrinsic = left_response.camera_info.K

        #     right_response = self.set_right_camera_info_service(stereo_cam_info[1])
        #     right_intrinsic = right_response.camera_info.K

        #     camera_intrinsics_msg.left_intirnsic = left_intrinsic
        #     camera_intrinsics_msg.right_intirnsic = right_intrinsic

        #     self.intrinsic_pub.publish(camera_intrinsics_msg)

        rospy.sleep(0.1)

    # end of the handle_strero

    def check_set_camera_info(self, response):
        if response.success:
            return True

        for i in range(10):
            print("!" * 80)
        print()
        print("Attempt to set camera info failed: " + response.status_message)
        print()
        for i in range(10):
            print("!" * 80)
        print()
        rospy.logerr(
            'Unable to set camera info for calibration. Failure message: %s' %
            response.status_message)
        return False

    def do_upload(self):
        self.c.report()
        print(self.c.ost())
        info = self.c.as_message()

        rv = True
        if self.c.is_mono:
            response = self.set_camera_info_service(info)
            rv = self.check_set_camera_info(response)
        else:
            response = self.set_left_camera_info_service(info[0])
            rv = rv and self.check_set_camera_info(response)
            response = self.set_right_camera_info_service(info[1])
            rv = rv and self.check_set_camera_info(response)
        return rv
Пример #26
0
    def test_stereo(self):
        for dim in self.sizes:
            print "Dim =", dim
            sc = StereoCalibrator([board], cv2.CALIB_FIX_K3)
            sc.cal(self.l[dim], self.r[dim])

            sc.report()
            #print sc.ost()

            # NOTE: epipolar error currently increases with resolution.
            # At highest res expect error ~0.75
            epierror = sc.epipolar_error_from_images(self.l[dim][0], self.r[dim][0])
            print "Epipolar error =", epierror
            self.assert_(epierror < 0.8)

            self.assertAlmostEqual(sc.chessboard_size_from_images(self.l[dim][0], self.r[dim][0]), .108, 2)

            #print sc.as_message()

            img = self.l[dim][0]
            flat = sc.l.remap(img)
            self.assertEqual(cv.GetSize(img), cv.GetSize(flat))
            flat = sc.r.remap(img)
            self.assertEqual(cv.GetSize(img), cv.GetSize(flat))

            sc2 = StereoCalibrator([board])
            sc2.from_message(sc.as_message())
            # sc2.set_alpha(1.0)
            #sc2.report()
            self.assert_(len(sc2.ost()) > 0)
class CalibrationNode:
    def __init__(self, boards, service_check = True, synchronizer = message_filters.TimeSynchronizer, flags = 0,
                 pattern=Patterns.Chessboard, camera_name='', checkerboard_flags = 0):
        if service_check:
            # assume any non-default service names have been set.  Wait for the service to become ready
            for svcname in ["camera", "left_camera", "right_camera"]:
                remapped = rospy.remap_name(svcname)
                if remapped != svcname:
                    fullservicename = "%s/set_camera_info" % remapped
                    print("Waiting for service", fullservicename, "...")
                    try:
                        rospy.wait_for_service(fullservicename, 5)
                        print("OK")
                    except rospy.ROSException:
                        print("Service not found")
                        rospy.signal_shutdown('Quit')

        self._boards = boards
        self._calib_flags = flags
        self._checkerboard_flags = checkerboard_flags
        self._pattern = pattern
        self._camera_name = camera_name
        lsub = message_filters.Subscriber('left', sensor_msgs.msg.Image)
        rsub = message_filters.Subscriber('right', sensor_msgs.msg.Image)
        ts = synchronizer([lsub, rsub], 4)
        ts.registerCallback(self.queue_stereo)

        msub = message_filters.Subscriber('image', sensor_msgs.msg.Image)
        msub.registerCallback(self.queue_monocular)
        
        self.set_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("camera"),
                                                          sensor_msgs.srv.SetCameraInfo)
        self.set_left_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("left_camera"),
                                                               sensor_msgs.srv.SetCameraInfo)
        self.set_right_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("right_camera"),
                                                                sensor_msgs.srv.SetCameraInfo)

        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.success:
            return True

        for i in range(10):
            print("!" * 80)
        print()
        print("Attempt to set camera info failed: " + response.status_message)
        print()
        for i in range(10):
            print("!" * 80)
        print()
        rospy.logerr('Unable to set camera info for calibration. Failure message: %s' % response.status_message)
        return False

    def do_upload(self):
        self.c.report()
        print(self.c.ost())
        info = self.c.as_message()

        rv = True
        if self.c.is_mono:
            response = self.set_camera_info_service(info)
            rv = self.check_set_camera_info(response)
        else:
            response = self.set_left_camera_info_service(info[0])
            rv = rv and self.check_set_camera_info(response)
            response = self.set_right_camera_info_service(info[1])
            rv = rv and self.check_set_camera_info(response)
        return rv
Пример #28
0
    def test_stereo(self):
        epierrors = [0.1, 0.2, 0.4, 1.0]
        for i, dim in enumerate(self.sizes):
            print("Dim =", dim)
            sc = StereoCalibrator([board], cv2.CALIB_FIX_K3)
            sc.cal(self.l[dim], self.r[dim])

            sc.report()
            #print sc.ost()

            # NOTE: epipolar error currently increases with resolution.
            # At highest res expect error ~0.75
            epierror = 0
            n = 0
            for l_img, r_img in zip(self.l[dim], self.r[dim]):
                epierror_local = sc.epipolar_error_from_images(l_img, r_img)
                if epierror_local:
                    epierror += epierror_local
                    n += 1
            epierror /= n
            self.assert_(epierror < epierrors[i], 'Epipolar error is %f for resolution i = %d' % (epierror, i))

            self.assertAlmostEqual(sc.chessboard_size_from_images(self.l[dim][0], self.r[dim][0]), .108, 2)

            #print sc.as_message()

            img = self.l[dim][0]
            flat = sc.l.remap(img)
            self.assertEqual(cv.GetSize(img), cv.GetSize(flat))
            flat = sc.r.remap(img)
            self.assertEqual(cv.GetSize(img), cv.GetSize(flat))

            sc2 = StereoCalibrator([board])
            sc2.from_message(sc.as_message())
            # sc2.set_alpha(1.0)
            #sc2.report()
            self.assert_(len(sc2.ost()) > 0)
Пример #29
0
def cal_from_tarfile(boards,
                     tarname,
                     mono=False,
                     upload=False,
                     calib_flags=0,
                     visualize=False,
                     alpha=1.0):
    if mono:
        calibrator = MonoCalibrator(boards, calib_flags)
    else:
        calibrator = StereoCalibrator(boards, calib_flags)

    calibrator.do_tarfile_calibration(tarname)

    print(calibrator.ost())

    if upload:
        info = calibrator.as_message()
        if mono:
            set_camera_info_service = rospy.ServiceProxy(
                "%s/set_camera_info" % rospy.remap_name("camera"),
                sensor_msgs.srv.SetCameraInfo)

            response = set_camera_info_service(info)
            if not response.success:
                raise RuntimeError(
                    "connected to set_camera_info service, but failed setting camera_info"
                )
        else:
            set_left_camera_info_service = rospy.ServiceProxy(
                "%s/set_camera_info" % rospy.remap_name("left_camera"),
                sensor_msgs.srv.SetCameraInfo)
            set_right_camera_info_service = rospy.ServiceProxy(
                "%s/set_camera_info" % rospy.remap_name("right_camera"),
                sensor_msgs.srv.SetCameraInfo)

            response1 = set_left_camera_info_service(info[0])
            response2 = set_right_camera_info_service(info[1])
            if not (response1.success and response2.success):
                raise RuntimeError(
                    "connected to set_camera_info service, but failed setting camera_info"
                )

    if visualize:

        #Show rectified images
        calibrator.set_alpha(alpha)

        archive = tarfile.open(tarname, 'r')
        if mono:
            for f in archive.getnames():
                if f.startswith('left') and (f.endswith('.pgm')
                                             or f.endswith('png')):
                    filedata = archive.extractfile(f).read()
                    file_bytes = numpy.asarray(bytearray(filedata),
                                               dtype=numpy.uint8)
                    im = cv2.imdecode(file_bytes, cv2.IMREAD_COLOR)

                    bridge = cv_bridge.CvBridge()
                    try:
                        msg = bridge.cv2_to_imgmsg(im, "bgr8")
                    except cv_bridge.CvBridgeError as e:
                        print(e)

                    #handle msg returns the recitifed image with corner detection once camera is calibrated.
                    drawable = calibrator.handle_msg(msg)
                    vis = numpy.asarray(drawable.scrib[:, :])
                    #Display. Name of window:f
                    display(f, vis)
        else:
            limages = [
                f for f in archive.getnames() if (f.startswith('left') and (
                    f.endswith('pgm') or f.endswith('png')))
            ]
            limages.sort()
            rimages = [
                f for f in archive.getnames() if (f.startswith('right') and (
                    f.endswith('pgm') or f.endswith('png')))
            ]
            rimages.sort()

            if not len(limages) == len(rimages):
                raise RuntimeError(
                    "Left, right images don't match. %d left images, %d right"
                    % (len(limages), len(rimages)))

            for i in range(len(limages)):
                l = limages[i]
                r = rimages[i]

                if l.startswith('left') and (
                        l.endswith('.pgm')
                        or l.endswith('png')) and r.startswith('right') and (
                            r.endswith('.pgm') or r.endswith('png')):
                    # LEFT IMAGE
                    filedata = archive.extractfile(l).read()
                    file_bytes = numpy.asarray(bytearray(filedata),
                                               dtype=numpy.uint8)
                    im_left = cv2.imdecode(file_bytes, cv2.IMREAD_COLOR)

                    bridge = cv_bridge.CvBridge()
                    try:
                        msg_left = bridge.cv2_to_imgmsg(im_left, "bgr8")
                    except cv_bridge.CvBridgeError as e:
                        print(e)

                    #RIGHT IMAGE
                    filedata = archive.extractfile(r).read()
                    file_bytes = numpy.asarray(bytearray(filedata),
                                               dtype=numpy.uint8)
                    im_right = cv2.imdecode(file_bytes, cv2.IMREAD_COLOR)
                    try:
                        msg_right = bridge.cv2_to_imgmsg(im_right, "bgr8")
                    except cv_bridge.CvBridgeError as e:
                        print(e)

                    drawable = calibrator.handle_msg([msg_left, msg_right])

                    h, w = numpy.asarray(drawable.lscrib[:, :]).shape[:2]
                    vis = numpy.zeros((h, w * 2, 3), numpy.uint8)
                    vis[:h, :w, :] = numpy.asarray(drawable.lscrib[:, :])
                    vis[:h, w:w * 2, :] = numpy.asarray(drawable.rscrib[:, :])

                    display(l + " " + r, vis)
Пример #30
0
    def test_stereo(self):
        epierrors = [0.1, 0.2, 0.45, 1.0]
        for i, dim in enumerate(self.sizes):
            print("Dim =", dim)
            sc = StereoCalibrator([board], cv2.CALIB_FIX_K3)
            sc.cal(self.l[dim], self.r[dim])

            sc.report()
            #print sc.ost()

            # NOTE: epipolar error currently increases with resolution.
            # At highest res expect error ~0.75
            epierror = 0
            n = 0
            for l_img, r_img in zip(self.l[dim], self.r[dim]):
                epierror_local = sc.epipolar_error_from_images(l_img, r_img)
                if epierror_local:
                    epierror += epierror_local
                    n += 1
            epierror /= n
            self.assertTrue(
                epierror < epierrors[i],
                'Epipolar error is %f for resolution i = %d' % (epierror, i))

            self.assertAlmostEqual(
                sc.chessboard_size_from_images(self.l[dim][0], self.r[dim][0]),
                .108, 2)

            #print sc.as_message()

            img = self.l[dim][0]
            flat = sc.l.remap(img)
            self.assertEqual(img.shape, flat.shape)
            flat = sc.r.remap(img)
            self.assertEqual(img.shape, flat.shape)

            sc2 = StereoCalibrator([board])
            sc2.from_message(sc.as_message())
            # sc2.set_alpha(1.0)
            #sc2.report()
            self.assertTrue(len(sc2.ost()) > 0)
Пример #31
0
    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])
Пример #32
0
class CalibrationNode:
    def __init__(self, boards, service_check = True, synchronizer = message_filters.TimeSynchronizer, flags = 0,
                fisheye_flags = 0, pattern=Patterns.Chessboard, camera_name='', checkerboard_flags = 0,
                max_chessboard_speed = -1, queue_size = 1):
        if service_check:
            # assume any non-default service names have been set.  Wait for the service to become ready
            for svcname in ["camera", "left_camera", "right_camera"]:
                remapped = rospy.remap_name(svcname)
                if remapped != svcname:
                    fullservicename = "%s/set_camera_info" % remapped
                    print("Waiting for service", fullservicename, "...")
                    try:
                        rospy.wait_for_service(fullservicename, 5)
                        print("OK")
                    except rospy.ROSException:
                        print("Service not found")
                        rospy.signal_shutdown('Quit')

        self._boards = boards
        self._calib_flags = flags
        self._fisheye_calib_flags = fisheye_flags
        self._checkerboard_flags = checkerboard_flags
        self._pattern = pattern
        self._camera_name = camera_name
        self._max_chessboard_speed = max_chessboard_speed
        lsub = message_filters.Subscriber('left', sensor_msgs.msg.Image)
        rsub = message_filters.Subscriber('right', sensor_msgs.msg.Image)
        ts = synchronizer([lsub, rsub], 4)
        ts.registerCallback(self.queue_stereo)

        msub = message_filters.Subscriber('image', sensor_msgs.msg.Image)
        msub.registerCallback(self.queue_monocular)

        self.set_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("camera"),
                                                          sensor_msgs.srv.SetCameraInfo)
        self.set_left_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("left_camera"),
                                                               sensor_msgs.srv.SetCameraInfo)
        self.set_right_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("right_camera"),
                                                                sensor_msgs.srv.SetCameraInfo)

        self.q_mono = BufferQueue(queue_size)
        self.q_stereo = BufferQueue(queue_size)

        self.c = None

        self._last_display = 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.put(msg)

    def queue_stereo(self, lmsg, rmsg):
        self.q_stereo.put((lmsg, rmsg))

    def handle_monocular(self, msg):
        if self.c == None:
            if self._camera_name:
                self.c = MonoCalibrator(self._boards, self._calib_flags, self._fisheye_calib_flags, self._pattern, name=self._camera_name,
                                        checkerboard_flags=self._checkerboard_flags,
                                        max_chessboard_speed = self._max_chessboard_speed)
            else:
                self.c = MonoCalibrator(self._boards, self._calib_flags, self._fisheye_calib_flags, self._pattern,
                                        checkerboard_flags=self.checkerboard_flags,
                                        max_chessboard_speed = self._max_chessboard_speed)

        # 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._fisheye_calib_flags, self._pattern, name=self._camera_name,
                                          checkerboard_flags=self._checkerboard_flags,
                                          max_chessboard_speed = self._max_chessboard_speed)
            else:
                self.c = StereoCalibrator(self._boards, self._calib_flags, self._fisheye_calib_flags, self._pattern,
                                          checkerboard_flags=self._checkerboard_flags,
                                          max_chessboard_speed = self._max_chessboard_speed)

        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.success:
            return True

        for i in range(10):
            print("!" * 80)
        print()
        print("Attempt to set camera info failed: " + response.status_message)
        print()
        for i in range(10):
            print("!" * 80)
        print()
        rospy.logerr('Unable to set camera info for calibration. Failure message: %s' % response.status_message)
        return False

    def do_upload(self):
        self.c.report()
        print(self.c.ost())
        info = self.c.as_message()

        rv = True
        if self.c.is_mono:
            response = self.set_camera_info_service(info)
            rv = self.check_set_camera_info(response)
        else:
            response = self.set_left_camera_info_service(info[0])
            rv = rv and self.check_set_camera_info(response)
            response = self.set_right_camera_info_service(info[1])
            rv = rv and self.check_set_camera_info(response)
        return rv
Пример #33
0
    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)

        # added lines TODO:
        # left_temp = points()   #get msg type from corners
        # right_temp = points()   #get msg type from corners
        # corner_msgs = corners()

        lcorner_size_msg = Int32()
        rcorner_size_msg = Int32()

        # The mat is giving the ready-to-publish corner coordinates, push everything in a mat TODO:
        if drawable.lcorner is not None:

            lcorner_size = len(drawable.lcorner)

            # publish the coner size for computation
            lcorner_size_msg.data = lcorner_size
            self.lcorner_size_pub.publish(lcorner_size_msg)

            left_mat = Float32MultiArray()
            left_mat.layout.dim.append(MultiArrayDimension())
            left_mat.layout.dim.append(MultiArrayDimension())
            left_mat.layout.dim[0].label = "row"
            left_mat.layout.dim[1].label = "col"
            left_mat.layout.dim[
                0].size = lcorner_size  # if there are 9 points give 9 dimension
            left_mat.layout.dim[1].size = lcorner_size
            left_mat.layout.dim[0].stride = lcorner_size
            left_mat.layout.dim[1].stride = lcorner_size
            left_mat.layout.data_offset = 0
            left_mat.data = [0] * lcorner_size * lcorner_size
            dstride1 = left_mat.layout.dim[1].stride
            offset = left_mat.layout.data_offset

            print("left")  # debug
            print(drawable.lcorner)  # debug
            i_l = 0
            for temp_left in drawable.lcorner:
                temp_x = temp_left[0, 0]
                temp_y = temp_left[0, 1]
                left_mat.data[offset + i_l + dstride1 * 0] = temp_x
                left_mat.data[offset + i_l + dstride1 * 1] = temp_y
                i_l += 1

            self.left_pub.publish(left_mat)
        else:
            print()
            print("No LEFT corner coordinates")
            print()

        if drawable.rcorner is not None:

            rcorner_size = len(drawable.rcorner)

            rcorner_size_msg.data = rcorner_size
            self.rcorner_size_pub.publish(rcorner_size_msg)

            right_mat = Float32MultiArray()
            right_mat.layout.dim.append(MultiArrayDimension())
            right_mat.layout.dim.append(MultiArrayDimension())
            right_mat.layout.dim[0].label = "row"
            right_mat.layout.dim[1].label = "col"
            right_mat.layout.dim[0].size = rcorner_size
            right_mat.layout.dim[1].size = rcorner_size
            right_mat.layout.dim[0].stride = rcorner_size
            right_mat.layout.dim[1].stride = rcorner_size
            right_mat.layout.data_offset = 0
            right_mat.data = [0] * rcorner_size * rcorner_size
            dstride1 = right_mat.layout.dim[1].stride
            offset = right_mat.layout.data_offset

            print("right")  # debug
            print(drawable.rcorner)  #debug
            i_r = 0
            for temp_right in drawable.rcorner:
                temp_x = temp_right[0, 0]
                temp_y = temp_right[0, 1]
                right_mat.data[offset + i_r + dstride1 * 0] = temp_x
                right_mat.data[offset + i_r + dstride1 * 1] = temp_y
                i_r += 1

            self.right_pub.publish(right_mat)

        else:
            print()
            print("No RIGHT corner coordinates")
            print()

        # tryp to publisht the intrinsic matrix for left and right camera
        # if self.c.is_mono:
        #     pass
        # else:
        #     stereo_cam_info = self.c.as_message()

        #     left_response = self.set_left_camera_info_service(stereo_cam_info[0])
        #     left_intrinsic = left_response.camera_info.K

        #     right_response = self.set_right_camera_info_service(stereo_cam_info[1])
        #     right_intrinsic = right_response.camera_info.K

        #     camera_intrinsics_msg.left_intirnsic = left_intrinsic
        #     camera_intrinsics_msg.right_intirnsic = right_intrinsic

        #     self.intrinsic_pub.publish(camera_intrinsics_msg)

        rospy.sleep(0.1)
Пример #34
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
Пример #35
0
    def test_stereo(self):
        for dim in self.sizes:
            print "Dim =", dim
            sc = StereoCalibrator([board], cv2.CALIB_FIX_K3)
            sc.cal(self.l[dim], self.r[dim])

            sc.report()
            #print sc.ost()

            # NOTE: epipolar error currently increases with resolution.
            # At highest res expect error ~0.75
            epierror = sc.epipolar_error_from_images(self.l[dim][0],
                                                     self.r[dim][0])
            print "Epipolar error =", epierror
            self.assert_(epierror < 0.8)

            self.assertAlmostEqual(
                sc.chessboard_size_from_images(self.l[dim][0], self.r[dim][0]),
                .108, 2)

            #print sc.as_message()

            img = self.l[dim][0]
            flat = sc.l.remap(img)
            self.assertEqual(cv.GetSize(img), cv.GetSize(flat))
            flat = sc.r.remap(img)
            self.assertEqual(cv.GetSize(img), cv.GetSize(flat))

            sc2 = StereoCalibrator([board])
            sc2.from_message(sc.as_message())
            # sc2.set_alpha(1.0)
            #sc2.report()
            self.assert_(len(sc2.ost()) > 0)
class CalibrationNode(Node):
    def __init__(self,
                 boards,
                 service_check=False,
                 synchronizer=message_filters.TimeSynchronizer,
                 flags=0,
                 pattern=Patterns.Chessboard,
                 camera_name='camera',
                 camera='camera',
                 left_camera='left_camera',
                 right_camera='right_camera',
                 image='image',
                 checkerboard_flags=0):
        # TODO will enable this function as soon as set_camera_info enabled
        # if service_check:
        # assume any non-default service names have been set.  Wait for the service to become ready
        # for svcname in ["camera", "left_camera", "right_camera"]:
        #     remapped = svcname
        #     if remapped != svcname:
        #         fullservicename = "%s/set_camera_info" % remapped
        #         print("Waiting for service", fullservicename, "...")
        #         try:
        #             rclpy.wait_for_service(fullservicename, 5)
        #             print("OK")
        #         except:
        #             print("Service not found")
        #             rclpy.shutdown()
        super().__init__(self._node_name)
        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_camera)
        rsub = message_filters.Subscriber(self, sensor_msgs.msg.Image,
                                          right_camera)
        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.set_camera_info_cli = self.create_client(
            sensor_msgs.srv.SetCameraInfo, "%s/set_camera_info" % camera)
        self.set_left_camera_cli = self.create_client(
            sensor_msgs.srv.SetCameraInfo, "%s/set_camera_info" % left_camera)
        self.set_right_camera_cli = self.create_client(
            sensor_msgs.srv.SetCameraInfo, "%s/set_camera_info" % right_camera)

        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.result().success:
            return True

        for i in range(10):
            print("!" * 80)
        print()
        print("Attempt to set camera info failed: " +
              response.result().status_message)
        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().status_message)
        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_cli.call_async(req)
            rv = self.check_set_camera_info(response)
        else:
            req.camera_info = info[0]
            response = self.set_left_camera_info_service(req)
            rv = rv and self.check_set_camera_info(response)
            req.camera_info = info[1]
            response = self.set_right_camera_info_service(req)
            rv = rv and self.check_set_camera_info(response)
        return rv