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()