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))
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)
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("[WARN] Couldn't open image " + fname + "!", file=sys.stderr) 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_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)
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)
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])
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]
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_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.assert_(err_intrinsics < setup.K_err, 'intrinsics error is %f' % err_intrinsics) print('intrinsics error is %f' % numpy.linalg.norm(mc.intrinsics - self.K, ord=numpy.inf))
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)
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
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
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
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])
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)
def __init__(self, name, chess_size, dim, approximate=0): super().__init__(name) self.board = ChessboardInfo() self.board.n_cols = chess_size[0] self.board.n_rows = chess_size[1] self.board.dim = dim # make sure n_cols is not smaller than n_rows, otherwise error computation will be off if self.board.n_cols < self.board.n_rows: self.board.n_cols, self.board.n_rows = self.board.n_rows, self.board.n_cols image_topic = "monocular/image_rect" camera_topic = "monocular/camera_info" tosync_mono = [ (image_topic, sensor_msgs.msg.Image), (camera_topic, sensor_msgs.msg.CameraInfo), ] if approximate <= 0: sync = message_filters.TimeSynchronizer else: sync = functools.partial(ApproximateTimeSynchronizer, slop=approximate) tsm = sync([ message_filters.Subscriber(self, type, topic) for (topic, type) in tosync_mono ], 10) tsm.registerCallback(self.queue_monocular) left_topic = "stereo/left/image_rect" left_camera_topic = "stereo/left/camera_info" right_topic = "stereo/right/image_rect" right_camera_topic = "stereo/right/camera_info" tosync_stereo = [(left_topic, sensor_msgs.msg.Image), (left_camera_topic, sensor_msgs.msg.CameraInfo), (right_topic, sensor_msgs.msg.Image), (right_camera_topic, sensor_msgs.msg.CameraInfo)] tss = sync([ message_filters.Subscriber(self, type, topic) for (topic, type) in tosync_stereo ], 10) tss.registerCallback(self.queue_stereo) self.br = cv_bridge.CvBridge() self.q_mono = Queue() self.q_stereo = Queue() mth = ConsumerThread(self.q_mono, self.handle_monocular) mth.setDaemon(True) mth.start() sth = ConsumerThread(self.q_stereo, self.handle_stereo) sth.setDaemon(True) sth.start() self.mc = MonoCalibrator([self.board]) self.sc = StereoCalibrator([self.board])
def 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()
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()
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)
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.mydir = roslib.packages.get_pkg_dir(PKG) self.tar = tarfile.open('%s/camera_calibration.tar.gz' % self.mydir, '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)] self.l = {} self.r = {} self.sizes = [(320,240), (640,480), (800,600), (1024,768)] for dim in self.sizes: self.l[dim] = []
import cv2 from camera_calibration.calibrator import MonoCalibrator, ChessboardInfo numImages = 30 images = [ cv2.imread('../Images/frame{:04d}.jpg'.format(i)) for i in range(numImages) ] board = ChessboardInfo() board.n_cols = 7 board.n_rows = 5 board.dim = 0.050 mc = MonoCalibrator([board], cv2.CALIB_FIX_K3) mc.cal(images) print(mc.as_message()) mc.do_save()
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()
help="visualize rectified images after calibration") parser.add_option("-a", "--alpha", type="float", default=1.0, metavar="ALPHA", 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])
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()
def main(): if len(sys.argv) < 3: print 'grab_cbs_auto cb_config.yaml output_bag.bag' return rospy.init_node("grab_cbs") f = file(sys.argv[1], 'r') cb_config = yaml.safe_load(f.read()) print cb_config f.close() is_kinect = rospy.get_param("/grab_cbs/is_kinect", True) # load cb stuff chessboard = ChessboardInfo() chessboard.n_cols = cb_config['cols'] # 6 chessboard.n_rows = cb_config['rows'] # 7 chessboard.dim = cb_config['dim'] # 0.0258 calib = Calibrator([chessboard]) bridge = CvBridge() l = DataListener(is_kinect, bridge, calib) tf_list = tf.TransformListener() cb_knowns = [] for j in range(chessboard.n_cols): for i in range(chessboard.n_rows): cb_knowns.append((chessboard.dim*i, chessboard.dim*j, 0.0)) bag = rosbag.Bag(sys.argv[2], 'w') i = 0 while not rospy.is_shutdown(): if raw_input("Press enter to take CB, type 'q' to quit: ") == "q": break tries = 0 while not rospy.is_shutdown() and tries < 3: corners = l.wait_for_new(5.) if corners is None: print "No corners detected" tries += 1 continue corners_2d = np.uint32(np.round(corners)).tolist() corners_3d = [] if is_kinect: for x,y,z in read_points(l.cur_pc, field_names=['x', 'y', 'z'], uvs=corners_2d): corners_3d.append((x,y,z)) frame_id = l.cur_pc.header else: obj_pts = cv.fromarray(np.array(cb_knowns)) img_pts = cv.fromarray(np.array(corners)) K = cv.fromarray(np.reshape(l.cam_info.K,(3,3))) D = cv.fromarray(np.array([l.cam_info.D])) R_vec = cv.fromarray(np.zeros((3,1))) t = cv.fromarray(np.zeros((3,1))) cv.FindExtrinsicCameraParams2(obj_pts, img_pts, K, D, R_vec, t) R_mat = cv.fromarray(np.zeros((3,3))) cv.Rodrigues2(R_vec, R_mat) T = PoseConv.to_homo_mat(np.mat(np.asarray(t)).T.A.tolist(), np.mat(np.asarray(R_mat)).A.tolist()) cb_knowns_mat = np.vstack((np.mat(cb_knowns).T, np.mat(np.ones((1, len(cb_knowns)))))) corners_3d = np.array((T * cb_knowns_mat)[:3,:].T) frame_id = l.cur_img.header print corners_3d if np.any(np.isnan(corners_3d)): print "Pointcloud malformed" tries += 1 continue now = rospy.Time.now() corners_pc = create_cloud_xyz32(frame_id, corners_3d) try: pose = tf_list.lookupTransform('/base_link', '/ee_link', rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): print "TF screwed up..." continue bag.write("/pose", PoseConv.to_pose_stamped_msg('/base_link', pose), now) bag.write("/pc", corners_pc, now) print "Wrote pose/CB to bag file" break i += 1 bag.close()