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 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 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 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)
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] = [] self.r[dim] = []
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
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]) tarname = args[0]
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]) tarname = args[0] num_ks = options.k_coefficients
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 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 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()