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(
'autoware_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 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 for resolution i = %d' % (err_intrinsics, i))
print('intrinsics error is %f' % numpy.linalg.norm(mc.intrinsics - self.K, ord=numpy.inf))
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()
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])
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])
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
import roslib
import rostest
import rospy
import unittest
import tarfile
import copy
import os, sys
from autoware_camera_calibration.calibrator import StereoCalibrator, ChessboardInfo, image_from_archive
# Large board used for PR2 calibration
board = ChessboardInfo()
board.n_cols = 7
board.n_rows = 6
board.dim = 0.108
class TestMultipleBoards(unittest.TestCase):
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(
import rosunit
import rospy
import cv2
import collections
import copy
import numpy
import os
import sys
import tarfile
import unittest
from autoware_camera_calibration.calibrator import MonoCalibrator, StereoCalibrator, \
Patterns, 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):
tar_path = roslib.packages.find_resource('autoware_camera_calibration', 'autoware_camera_calibration.tar.gz')[0]
self.tar = tarfile.open(tar_path, '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] = []