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 test_stereo(self):
epierrors = [0.1, 0.2, 0.45, 1.0]
for i, dim in enumerate(self.sizes):
print("Dim =", dim)
sc = StereoCalibrator([board], cv2.CALIB_FIX_K3)
sc.cal(self.l[dim], self.r[dim])
sc.report()
#print sc.ost()
# NOTE: epipolar error currently increases with resolution.
# At highest res expect error ~0.75
epierror = 0
n = 0
for l_img, r_img in zip(self.l[dim], self.r[dim]):
epierror_local = sc.epipolar_error_from_images(l_img, r_img)
if epierror_local:
epierror += epierror_local
n += 1
epierror /= n
self.assert_(epierror < epierrors[i],
'Epipolar error is %f for resolution i = %d' % (epierror, i))
self.assertAlmostEqual(sc.chessboard_size_from_images(self.l[dim][0], self.r[dim][0]), .108, 2)
#print sc.as_message()
img = self.l[dim][0]
flat = sc.l.remap(img)
self.assertEqual(img.shape, flat.shape)
flat = sc.r.remap(img)
self.assertEqual(img.shape, flat.shape)
sc2 = StereoCalibrator([board])
sc2.from_message(sc.as_message())
# sc2.set_alpha(1.0)
#sc2.report()
self.assert_(len(sc2.ost()) > 0)
class CalibrationNode:
def __init__(self, boards, service_check = True, synchronizer = message_filters.TimeSynchronizer, flags = 0,
pattern=Patterns.Chessboard, camera_name='', detection='cv2', output='yaml', checkerboard_flags = 0,
min_good_enough = 40):
if service_check:
# assume any non-default service names have been set. Wait for the service to become ready
for svcname in ["camera", "left_camera", "right_camera"]:
remapped = rospy.remap_name(svcname)
if remapped != svcname:
fullservicename = "%s/set_camera_info" % remapped
print("Waiting for service", fullservicename, "...")
try:
rospy.wait_for_service(fullservicename, 5)
print("OK")
except rospy.ROSException:
print("Service not found")
rospy.signal_shutdown('Quit')
self._boards = boards
self._detection = detection
self._output = output
self._calib_flags = flags
self._checkerboard_flags = checkerboard_flags
self._pattern = pattern
self._camera_name = camera_name
self._min_good_enough = min_good_enough
rospack = rospkg.RosPack()
pkg_path = rospack.get_path('autoware_camera_lidar_calibrator')
self._autoware_image = cv2.imread( path.join(pkg_path, 'docs/autoware_logo.jpg'), cv2.IMREAD_UNCHANGED)
lsub = message_filters.Subscriber('left', sensor_msgs.msg.Image)
rsub = message_filters.Subscriber('right', sensor_msgs.msg.Image)
ts = synchronizer([lsub, rsub], 4)
ts.registerCallback(self.queue_stereo)
msub = message_filters.Subscriber('image', sensor_msgs.msg.Image)
msub.registerCallback(self.queue_monocular)
self.set_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("camera"),
sensor_msgs.srv.SetCameraInfo)
self.set_left_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("left_camera"),
sensor_msgs.srv.SetCameraInfo)
self.set_right_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("right_camera"),
sensor_msgs.srv.SetCameraInfo)
self.q_mono = deque([], 1)
self.q_stereo = deque([], 1)
self.c = None
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()
def redraw_stereo(self, *args):
pass
def redraw_monocular(self, *args):
pass
def queue_monocular(self, msg):
self.q_mono.append(msg)
def queue_stereo(self, lmsg, rmsg):
self.q_stereo.append((lmsg, rmsg))
def handle_monocular(self, msg):
if self.c == None:
if self._camera_name:
self.c = MonoCalibrator(self._boards, self._calib_flags, self._pattern, name=self._camera_name,
detection=self._detection, output = self._output,
checkerboard_flags=self._checkerboard_flags,
min_good_enough = self._min_good_enough)
else:
self.c = MonoCalibrator(self._boards, self._calib_flags, self._pattern, detection=self._detection,
output=self._output, checkerboard_flags=self.checkerboard_flags,
min_good_enough = self._min_good_enough)
# This should just call the MonoCalibrator
drawable = self.c.handle_msg(msg)
self.displaywidth = drawable.scrib.shape[1]
self.redraw_monocular(drawable)
def handle_stereo(self, msg):
if self.c == None:
if self._camera_name:
self.c = StereoCalibrator(self._boards, self._calib_flags, self._pattern, name=self._camera_name,
detection=self._detection, output = self._output,
checkerboard_flags=self._checkerboard_flags,
min_good_enough = self._min_good_enough)
else:
self.c = StereoCalibrator(self._boards, self._calib_flags, self._pattern,detection=self._detection,
output=self._output, checkerboard_flags=self.checkerboard_flags,
min_good_enough = self._min_good_enough)
drawable = self.c.handle_msg(msg)
self.displaywidth = drawable.lscrib.shape[1] + drawable.rscrib.shape[1]
self.redraw_stereo(drawable)
def check_set_camera_info(self, response):
if response.success:
return True
for i in range(10):
print("!" * 80)
print()
print("Attempt to set camera info failed: " + response.status_message)
print()
for i in range(10):
print("!" * 80)
print()
rospy.logerr('Unable to set camera info for calibration. Failure message: %s' % response.status_message)
return False
def do_upload(self):
self.c.report()
print(self.c.ost())
info = self.c.as_message()
rv = True
if self.c.is_mono:
response = self.set_camera_info_service(info)
rv = self.check_set_camera_info(response)
else:
response = self.set_left_camera_info_service(info[0])
rv = rv and self.check_set_camera_info(response)
response = self.set_right_camera_info_service(info[1])
rv = rv and self.check_set_camera_info(response)
return rv