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