Ejemplo n.º 1
0
    def test_rational_polynomial_model(self):
        """Test that the distortion coefficients returned for a rational_polynomial model are not empty."""
        for i, setup in enumerate(self.setups):
            board = ChessboardInfo()
            board.n_cols = setup.cols
            board.n_rows = setup.rows
            board.dim = self.board_width_dim

            mc = MonoCalibrator([board],
                                flags=cv2.CALIB_RATIONAL_MODEL,
                                pattern=setup.pattern)
            mc.cal(self.limages[i])
            self.assertEqual(
                len(mc.distortion.flat), 8,
                'length of distortion coefficients is %d' %
                len(mc.distortion.flat))
            self.assertTrue(
                all(mc.distortion.flat != 0),
                'some distortion coefficients are zero: %s' %
                str(mc.distortion.flatten()))
            self.assertEqual(mc.as_message().distortion_model,
                             'rational_polynomial')
            self.assert_good_mono(mc, self.limages[i], setup.lin_err)

            yaml = mc.yaml()
            # Issue #278
            self.assertIn('cols: 8', yaml)
Ejemplo n.º 2
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)
Ejemplo n.º 3
0
    def test_monocular(self):
        # Run the calibrator, produce a calibration, check it
        for i, setup in enumerate(self.setups):
            board = ChessboardInfo()
            board.n_cols = setup.cols
            board.n_rows = setup.rows
            board.dim = self.board_width_dim

            mc = MonoCalibrator([board],
                                flags=cv2.CALIB_FIX_K3,
                                pattern=setup.pattern)

            if 0:
                # display the patterns viewed by the camera
                for pattern_warped in self.limages[i]:
                    cv2.imshow("toto", pattern_warped)
                    cv2.waitKey(0)

            mc.cal(self.limages[i])
            self.assert_good_mono(mc, self.limages[i], setup.lin_err)

            # Make sure the intrinsics are similar
            err_intrinsics = numpy.linalg.norm(mc.intrinsics - self.k,
                                               ord=numpy.inf)
            self.assertTrue(
                err_intrinsics < setup.K_err,
                'intrinsics error is %f for resolution i = %d' %
                (err_intrinsics, i))
            print('intrinsics error is %f' %
                  numpy.linalg.norm(mc.intrinsics - self.k, ord=numpy.inf))
Ejemplo n.º 4
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])
Ejemplo n.º 5
0
    def loadImages(self,
                   cal_img_path,
                   name,
                   n_corners,
                   square_length,
                   n_disp_img=1e5,
                   display_flag=True):
        self.name = name
        self.cal_img_path = cal_img_path

        self.boards = []
        cols, rows = n_corners
        self.boards.append(ChessboardInfo(cols, rows, float(square_length)))
        self.c = MonoCalibrator(self.boards, self.calib_flags, self.pattern)

        if display_flag:
            fig = plt.figure('Corner Extraction', figsize=(12, 5))
            gs = gridspec.GridSpec(1, 2)
            gs.update(wspace=0.025, hspace=0.05)

        for i, file in enumerate(sorted(os.listdir(self.cal_img_path))):
            img = cv2.imread(self.cal_img_path + '/' + file,
                             0)  # Load the image
            img_msg = self.c.br.cv2_to_imgmsg(
                img, 'mono8')  # Convert to ROS Image msg
            # Extract chessboard corners using ROS camera_calibration package
            drawable = self.c.handle_msg(img_msg)

            if display_flag and i < n_disp_img:
                ax = plt.subplot(gs[0, 0])
                plt.imshow(img, cmap='gray')
                plt.axis('off')

                ax = plt.subplot(gs[0, 1])
                plt.imshow(drawable.scrib)
                plt.axis('off')

                plt.subplots_adjust(left=0.02,
                                    right=0.98,
                                    top=0.98,
                                    bottom=0.02)
                fig.canvas.set_window_title(
                    'Corner Extraction (Chessboard {0})'.format(i + 1))

                plt.show(block=False)
                plt.waitforbuttonpress()

        # Useful parameters
        # Length of a chessboard square
        self.d_square = square_length
        self.h_pixels, self.w_pixels = img.shape  # Image pixel dimensions
        # Number of examined images
        self.n_chessboards = len(self.c.good_corners)
        # Dimensions of extracted corner grid
        self.n_corners_y, self.n_corners_x = n_corners
        self.n_corners_per_chessboard = n_corners[0] * n_corners[1]
Ejemplo n.º 6
0
    def __init__(self):
        # Define the Checkerboard.  Note the OpenCV detector
        # apparently assumes more columns than rows.
        self.board = ChessboardInfo()
        self.board.n_cols = 4
        self.board.n_rows = 3
        self.board.dim = 0.0254 * (1 + 7 / 8.0)
        self.K, self.D = intrinsic_params_from_file()
        self.bridge = cv_bridge.CvBridge()

        # Instantiate a Calibrator, to extract corners etc.
        self.calibrator = Calibrator([self.board])
        self.R_cam_wrt_world = None
        self.x_cam_wrt_world = None
        self.R_world_wrt_cam = None
        self.x_world_wrt_cam = None
Ejemplo n.º 7
0
def main(args):
    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]")
    options, args = parser.parse_args()
    size = tuple([int(c) for c in options.size.split('x')])
    dim = float(options.square)

    images = []
    for fname in args:
        if os.path.isfile(fname):
            img = cv.LoadImage(fname)
            if img is None:
                print >> sys.stderr, "[WARN] Couldn't open image " + fname + "!"
                sys.exit(1)
            else:
                print "[INFO] Loaded " + fname + " (" + str(
                    img.width) + "x" + str(img.height) + ")"

            images.append(img)

    cboard = ChessboardInfo()
    cboard.dim = dim
    cboard.n_cols = size[0]
    cboard.n_rows = size[1]

    mc = MonoCalibrator([cboard])
    mc.cal(images)
    print mc.as_message()
    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)
Ejemplo n.º 9
0
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='autoware_camera_calibration',
                     help="name of the camera to appear in the calibration file")
    parser.add_option("-o", "--output",
                     type="string", default="yaml",
                     help="type of output - 'yaml' or 'tar'")
    parser.add_option("-d", "--detection",
                     type="string", default="cv2",
                     help="Chessboard detection algorithm, OpenCV2 or Matlab (python matlab engine) - 'cv2', 'matlab'")
    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")
    group.add_option("--min_samples",
                     type="int", default=40,
                     help="defines the minimum number of samples before allowing to calibrate regardless of the status")
    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=3, 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, args = parser.parse_args()

    if len(options.size) != len(options.square):
        parser.error("Number of size and square inputs must be the same!")

    if options.detection == "cv2":
        print('Using OpenCV 2 for chessboard corner detection')
    elif options.detection == "matlab":
        print('Using matlab for chessboard corner detection')
    else:
        print('Unrecognized detection method %s, defaulting to OpenCV 2' % options.detection)
        options.detection = "cv2"

    if options.output == "yaml":
            print('Saving as autoware yaml')
    elif options.output == "tar":
            print('Saving as tar')
    else:
            print('Unrecognized output method %s, defaulting to Autoware Yaml' % options.output)
            options.output = "yaml"

    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

    rospy.init_node('cameracalibrator')
    node = OpenCVCalibrationNode(boards, options.service_check, sync, calib_flags, pattern, options.camera_name,
                                 options.detection, options.output, min_good_enough=options.min_samples, checkerboard_flags=checkerboard_flags)
    rospy.spin()
Ejemplo n.º 10
0
import roslib
PKG = 'camera_calibration'
roslib.load_manifest(PKG)
import rostest
import rospy
import cv
import cv2
import unittest
import tarfile
import copy
import os, sys

from camera_calibration.calibrator import cvmat_iterator, MonoCalibrator, StereoCalibrator, CalibrationException, ChessboardInfo, image_from_archive

board = ChessboardInfo()
board.n_cols = 8
board.n_rows = 6
board.dim = 0.108


class TestDirected(unittest.TestCase):
    def setUp(self):
        self.tar = tarfile.open('camera_calibration.tar.gz', 'r')
        self.limages = [
            image_from_archive(self.tar, "wide/left%04d.pgm" % i)
            for i in range(3, 15)
        ]
        self.rimages = [
            image_from_archive(self.tar, "wide/right%04d.pgm" % i)
            for i in range(3, 15)
Ejemplo n.º 11
0
def main():
    from optparse import OptionParser, OptionGroup
    parser = OptionParser(
        "%prog --size SIZE1 --square SQUARE1 [ --size SIZE2 --square SQUARE2 ]",
        description=None)
    group = OptionGroup(
        parser, "Chessboard Options",
        "You must specify one or more chessboards as pairs of --size and --square options."
    )
    #group.add_option("--pattern",
    #                 type="string", default="chessboard",
    #                 help="calibration pattern to detect - 'chessboard' or 'circles'")
    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)"
    )
    parser.add_option_group(group)
    group = OptionGroup(parser, "Deprecated Options")
    group.add_option(
        "--rational-model",
        action="store_true",
        default=False,
        help=
        "enable distortion coefficients k4, k5 and k6 (for high-distortion lenses)"
    )
    group.add_option(
        "--fix-k1",
        action="store_true",
        default=False,
        help=
        "do not change the corresponding radial distortion coefficient during the optimization"
    )
    group.add_option("--fix-k2", action="store_true", default=False)
    group.add_option("--fix-k3", action="store_true", default=False)
    group.add_option("--fix-k4", action="store_true", default=False)
    group.add_option("--fix-k5", action="store_true", default=False)
    group.add_option("--fix-k6", action="store_true", default=False)
    parser.add_option_group(group)
    options, args = parser.parse_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(ApproximateSynchronizer, options.approximate)

    num_ks = options.k_coefficients
    # Deprecated flags modify k_coefficients
    if options.rational_model:
        print "Option --rational-model is deprecated"
        num_ks = 6
    if options.fix_k6:
        print "Option --fix-k6 is deprecated"
        num_ks = min(num_ks, 5)
    if options.fix_k5:
        print "Option --fix-k5 is deprecated"
        num_ks = min(num_ks, 4)
    if options.fix_k4:
        print "Option --fix-k4 is deprecated"
        num_ks = min(num_ks, 3)
    if options.fix_k3:
        print "Option --fix-k3 is deprecated"
        num_ks = min(num_ks, 2)
    if options.fix_k2:
        print "Option --fix-k2 is deprecated"
        num_ks = min(num_ks, 1)
    if options.fix_k1:
        print "Option --fix-k1 is deprecated"
        num_ks = 0

    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

    rospy.init_node('cameracalibrator')
    node = OpenCVCalibrationNode(boards, options.service_check, sync,
                                 calib_flags)
    rospy.spin()
Ejemplo n.º 12
0
def main():
    gray = (100,100,100)
    corner_len = 5
    chessboard = ChessboardInfo()
    chessboard.n_cols = 6
    chessboard.n_rows = 7
    chessboard.dim = 0.02273
    cboard_frame = "kinect_cb_corner"
#kinect_tracker_frame = "kinect"
#TODO
    use_pygame = False
    kinect_tracker_frame = "pr2_antenna"

    rospy.init_node("kinect_calib_test")
    img_list = ImageListener("/kinect_head/rgb/image_color")
    pix3d_srv = rospy.ServiceProxy("/pixel_2_3d", Pixel23d, True)
    tf_list = tf.TransformListener()
    if use_pygame:
        pygame.init()
        clock = pygame.time.Clock()
        screen = pygame.display.set_mode((640, 480))
    calib = Calibrator([chessboard])
    done = False
    corner_list = np.ones((2, corner_len)) * -1000.0
    corner_i = 0
    saved_corners_2d, saved_corners_3d, cb_locs = [], [], []
    while not rospy.is_shutdown():
        try:
            cb_pos, cb_quat = tf_list.lookupTransform(kinect_tracker_frame, 
                                                      cboard_frame, 
                                                      rospy.Time())
        except:
            rospy.sleep(0.001)
            continue
        cv_img = img_list.get_cv_img()
        if cv_img is not None:
            has_corners, corners, chess = calib.get_corners(cv_img)
            for corner2d in saved_corners_2d:
                cv.Circle(cv_img, corner2d, 4, [0, 255, 255])
            if has_corners:
                corner_i += 1
                corner = corners[0]
                if use_pygame:
                    for event in pygame.event.get():
                        if event.type == pygame.KEYDOWN:
                            print event.dict['key'], pygame.K_d
                            if event.dict['key'] == pygame.K_d:
                                done = True
                            if event.dict['key'] == pygame.K_q:
                                return
                    if done:
                        break
                corner_list[:, corner_i % corner_len] = corner
                if np.linalg.norm(np.var(corner_list, 1)) < 1.0:
                    corner_avg = np.mean(corner_list, 1)
                    corner_avg_tuple = tuple(corner_avg.round().astype(int).tolist())
                    cv.Circle(cv_img, corner_avg_tuple, 4, [0, 255, 0])
                    pix3d_resp = pix3d_srv(*corner_avg_tuple)
                    if pix3d_resp.error_flag == pix3d_resp.SUCCESS:
                        corner_3d_tuple = (pix3d_resp.pixel3d.pose.position.x,
                                           pix3d_resp.pixel3d.pose.position.y,
                                           pix3d_resp.pixel3d.pose.position.z)
                        if len(saved_corners_3d) == 0:
                            cb_locs.append(cb_pos)
                            saved_corners_2d.append(corner_avg_tuple)
                            saved_corners_3d.append(corner_3d_tuple)
                        else:
                            diff_arr = np.array(np.mat(saved_corners_3d) - np.mat(corner_3d_tuple))
                            if np.min(np.sqrt(np.sum(diff_arr ** 2, 1))) >= 0.03:
                                cb_locs.append(cb_pos)
                                saved_corners_2d.append(corner_avg_tuple)
                                saved_corners_3d.append(corner_3d_tuple)
                                print "Added sample", len(saved_corners_2d) - 1
                else:
                    cv.Circle(cv_img, corner, 4, [255, 0, 0])
            else:
                corner_list = np.ones((2, corner_len)) * -1000.0
        if use_pygame:
            if cv_img is None:
                screen.fill(gray)
            else:
                screen.blit(img_list.get_pg_img(cv_img), (0, 0))
            pygame.display.flip()
        rospy.sleep(0.001)
    A = np.mat(saved_corners_3d).T
    B = np.mat(cb_locs).T
    print A, B
    t, R = umeyama_method(A, B)
    print A, B, R, t
    print "-" * 60
    print "Transformation Parameters:"
    pos, quat = PoseConverter.to_pos_quat(t, R)
    print '%f %f %f %f %f %f %f' % tuple(pos + quat)
    t_r, R_r = ransac(A, B, 0.02, percent_set_train=0.5, percent_set_fit=0.6)
    print t_r, R_r
    pos, quat = PoseConverter.to_pos_quat(t_r, R_r)
    print '%f %f %f %f %f %f %f' % tuple(pos + quat)
Ejemplo n.º 13
0
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', 'charuco'\n" +
                     "  if 'charuco' is used, a --charuco_marker_size and --aruco_dict argument must be supplied\n" +
                     "  with each --size and --square argument")
    group.add_option("-s", "--size",
                     action="append", default=[],
                     help="chessboard size as NxM, counting interior corners (e.g. a standard chessboard is 7x7)")
    group.add_option("-q", "--square",
                     action="append", default=[],
                     help="chessboard square size in meters")
    group.add_option("-m", "--charuco_marker_size",
                     action="append", default=[],
                     help="ArUco marker size (meters); only valid with `-p charuco`")
    group.add_option("-d", "--aruco_dict",
                     action="append", default=[],
                     help="ArUco marker dictionary; only valid with `-p charuco`; one of 'aruco_orig', '4x4_250', " +
                     "'5x5_250', '6x6_250', '7x7_250'")
    parser.add_option_group(group)
    group = OptionGroup(parser, "ROS Communication Options")
    group.add_option("--approximate",
                     type="float", default=0.0,
                     help="allow specified slop (in seconds) when pairing images from unsynchronized stereo cameras")
    group.add_option("--no-service-check",
                     action="store_false", dest="service_check", default=True,
                     help="disable check for set_camera_info services at startup")
    group.add_option("--queue-size",
                     type="int", default=1,
                     help="image queue size (default %default, set to 0 for unlimited)")
    parser.add_option_group(group)
    group = OptionGroup(parser, "Calibration Optimizer Options")
    group.add_option("--fix-principal-point",
                     action="store_true", default=False,
                     help="for pinhole, fix the principal point at the image center")
    group.add_option("--fix-aspect-ratio",
                     action="store_true", default=False,
                     help="for pinhole, enforce focal lengths (fx, fy) are equal")
    group.add_option("--zero-tangent-dist",
                     action="store_true", default=False,
                     help="for pinhole, set tangential distortion coefficients (p1, p2) to zero")
    group.add_option("-k", "--k-coefficients",
                     type="int", default=2, metavar="NUM_COEFFS",
                     help="for pinhole, number of radial distortion coefficients to use (up to 6, default %default)")

    group.add_option("--fisheye-recompute-extrinsicsts",
                     action="store_true", default=False,
                     help="for fisheye, extrinsic will be recomputed after each iteration of intrinsic optimization")
    group.add_option("--fisheye-fix-skew",
                     action="store_true", default=False,
                     help="for fisheye, skew coefficient (alpha) is set to zero and stay zero")
    group.add_option("--fisheye-fix-principal-point",
                     action="store_true", default=False,
                     help="for fisheye,fix the principal point at the image center")
    group.add_option("--fisheye-k-coefficients",
                     type="int", default=4, metavar="NUM_COEFFS",
                     help="for fisheye, number of radial distortion coefficients to use fixing to zero the rest (up to 4, default %default)")

    group.add_option("--fisheye-check-conditions",
                     action="store_true", default=False,
                     help="for fisheye, the functions will check validity of condition number")

    group.add_option("--disable_calib_cb_fast_check", action='store_true', default=False,
                     help="uses the CALIB_CB_FAST_CHECK flag for findChessboardCorners")
    group.add_option("--max-chessboard-speed", type="float", default=-1.0,
                     help="Do not use samples where the calibration pattern is moving faster \
                     than this speed in px/frame. Set to eg. 0.5 for rolling shutter cameras.")

    parser.add_option_group(group)

    options, args = parser.parse_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 = []
    if options.pattern == "charuco" and optionsValidCharuco(options, parser):
        for (sz, sq, ms, ad) in zip(options.size, options.square, options.charuco_marker_size, options.aruco_dict):
            size = tuple([int(c) for c in sz.split('x')])
            boards.append(ChessboardInfo('charuco', size[0], size[1], float(sq), float(ms), ad))
    else:
        for (sz, sq) in zip(options.size, options.square):
            size = tuple([int(c) for c in sz.split('x')])
            boards.append(ChessboardInfo(options.pattern, size[0], size[1], float(sq)))

    if options.approximate == 0.0:
        sync = message_filters.TimeSynchronizer
    else:
        sync = functools.partial(ApproximateTimeSynchronizer, slop=options.approximate)

    # Pinhole opencv calibration options parsing
    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

    # Opencv calibration flags parsing:
    num_ks = options.fisheye_k_coefficients
    fisheye_calib_flags = 0
    if options.fisheye_fix_principal_point:
        fisheye_calib_flags |= cv2.fisheye.CALIB_FIX_PRINCIPAL_POINT
    if options.fisheye_fix_skew:
        fisheye_calib_flags |= cv2.fisheye.CALIB_FIX_SKEW
    if options.fisheye_recompute_extrinsicsts:
        fisheye_calib_flags |= cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC
    if options.fisheye_check_conditions:
        fisheye_calib_flags |= cv2.fisheye.CALIB_CHECK_COND
    if (num_ks < 4):
        fisheye_calib_flags |= cv2.fisheye.CALIB_FIX_K4
    if (num_ks < 3):
        fisheye_calib_flags |= cv2.fisheye.CALIB_FIX_K3
    if (num_ks < 2):
        fisheye_calib_flags |= cv2.fisheye.CALIB_FIX_K2
    if (num_ks < 1):
        fisheye_calib_flags |= cv2.fisheye.CALIB_FIX_K1

    pattern = Patterns.Chessboard
    if options.pattern == 'circles':
        pattern = Patterns.Circles
    elif options.pattern == 'acircles':
        pattern = Patterns.ACircles
    elif options.pattern == 'charuco':
        pattern = Patterns.ChArUco
    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

    rospy.init_node('cameracalibrator')
    node = OpenCVCalibrationNode(boards, options.service_check, sync, calib_flags, fisheye_calib_flags, pattern, options.camera_name,
                                 checkerboard_flags=checkerboard_flags, max_chessboard_speed=options.max_chessboard_speed,
                                 queue_size=options.queue_size)
    rospy.spin()
Ejemplo n.º 14
0
def find_intrinsics(visualize=False):
    fatal = False
    good = []
    xys = []

    if 1:
        with open('setup.json', mode='r') as fd:
            setup = json.loads(fd.read())

    globber = '*-corners*.data'
    files = glob.glob(globber)
    files.sort()
    print 'all data files matching "%s"' % (globber, )
    print files
    for f in files:
        this_fatal = False
        parts = f.split('-')
        board = None
        for p in parts:
            if p.startswith('board'):
                boardname = p
                with open(boardname + '.json', mode='r') as fd:
                    buf = fd.read()
                boardd = json.loads(buf)
                board = ChessboardInfo()
                board.n_cols = boardd['n_cols']
                board.n_rows = boardd['n_rows']
                board.dim = boardd['dim']

        assert board is not None

        xy = np.loadtxt(f)
        xys.append(xy)
        if len(xy) != board.n_cols * board.n_rows:
            rospy.logfatal('Error: %d points in %s. Expected %d.' %
                           (len(xy), f, board.n_cols * board.n_rows))
            this_fatal = True
            fatal = True
        if visualize:
            if 0:
                plt.figure()
            fmt = 'o-'
            label = f
            if this_fatal:
                fmt = 'kx:'
                label = 'BAD: ' + f
            plt.plot(xy[:, 0], xy[:, 1], fmt, mfc='none', label=label)
            if this_fatal:
                for i in range(len(xy)):
                    plt.text(xy[i, 0], xy[i, 1], str(i))
        corners = [(x, y) for (x, y) in xy]
        good.append((corners, board))
    if visualize:
        plt.legend()
        plt.show()
    if fatal:
        sys.exit(1)

    mc = MonoCalibrator(
        [board]
    )  #, cv2.CALIB_FIX_K1 | cv2.CALIB_FIX_K2 | cv2.CALIB_FIX_K3 | cv2.CALIB_ZERO_TANGENT_DIST )
    mc.size = tuple(setup['size'])

    mc.cal_fromcorners(good)
    msg = mc.as_message()

    cam = pymvg.CameraModel.from_ros_like(intrinsics=msg)
    cam_mirror = cam.get_mirror_camera()

    if 1:

        # The intrinsics are valid for the whole physical display and
        # each virtual display.
        names = setup['names']

        for name in names:
            name = str(
                name)  # remove unicode -- ROS bag fails on unicode topic name
            if 'mirror' in name:
                c = cam_mirror
            else:
                c = cam
            msg = c.get_intrinsics_as_msg()
            fname = 'display-intrinsic-cal-%s.bag' % (name.replace('/', '-'), )
            bagout = rosbag.Bag(fname, 'w')
            topic = '/%s/camera_info' % name
            bagout.write(topic, msg)
            bagout.close()
            print 'saved to', fname

            print 'You can play this calibration with:\n'
            print 'rosbag play %s -l' % (fname, )
            print
Ejemplo n.º 15
0
        help=
        "zoom for visualization of rectifies images. Ranges from 0 (zoomed in, all pixels in calibrated image are valid) to 1 (zoomed out, all pixels in  original image are in calibrated image). default %default)"
    )

    options, args = parser.parse_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):
        info = ChessboardInfo()
        info.dim = float(sq)
        size = tuple([int(c) for c in sz.split('x')])
        info.n_cols = size[0]
        info.n_rows = size[1]

        boards.append(info)

    if not boards:
        parser.error("Must supply at least one chessboard")

    if not args:
        parser.error("Must give tarfile name")
    if not os.path.exists(args[0]):
        parser.error("Tarfile %s does not exist. Please select valid tarfile" %
                     args[0])
Ejemplo n.º 16
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])
Ejemplo n.º 17
0
def main(args=None):
    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")
    parser.add_option(
        "--image",
        type="string",
        default='image',
        help="name of the image topic to appear in the calibration file")
    parser.add_option(
        "--camera",
        type="string",
        default='camera',
        help="name of the camera topic to appear in the calibration file")
    parser.add_option(
        "--left_camera",
        type="string",
        default='left_camera',
        help="name of the left camera to appear in the calibration file")
    parser.add_option(
        "--right_camera",
        type="string",
        default='right_camera',
        help="name of the right 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=False,
        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, args = parser.parse_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(args=args)
    node = OpenCVCalibrationNode(boards,
                                 options.service_check,
                                 sync,
                                 calib_flags,
                                 pattern,
                                 camera_name=options.camera_name,
                                 camera=options.camera,
                                 left_camera=options.left_camera,
                                 right_camera=options.right_camera,
                                 image=options.image,
                                 checkerboard_flags=checkerboard_flags)
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()