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 test_stereo(self):
for dim in 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 = sc.epipolar_error_from_images(self.l[dim][0],
self.r[dim][0])
print "Epipolar error =", epierror
self.assert_(epierror < 0.8)
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(cv.GetSize(img), cv.GetSize(flat))
flat = sc.r.remap(img)
self.assertEqual(cv.GetSize(img), cv.GetSize(flat))
sc2 = StereoCalibrator([board])
sc2.from_message(sc.as_message())
# sc2.set_alpha(1.0)
#sc2.report()
self.assert_(len(sc2.ost()) > 0)
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)
else:
self.c = StereoCalibrator(self._boards, self._calib_flags, self._pattern)
drawable = self.c.handle_msg(msg)
self.displaywidth = drawable.lscrib.shape[1] + drawable.rscrib.shape[1]
self.redraw_stereo(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, distortion_model=self._distortion_model, checkerboard_flags=self._checkerboard_flags)
else:
self.c = StereoCalibrator(self._boards, self._calib_flags, self._pattern, distortion_model=self._distortion_model,
checkerboard_flags=self._checkerboard_flags)
drawable = self.c.handle_msg(msg)
self.displaywidth = drawable.lscrib.shape[1] + drawable.rscrib.shape[1]
self.redraw_stereo(drawable)
def handle_stereo(self, msg):
if self.c == None:
if self._camera_name:
self.c = StereoCalibrator(self._boards, self._calib_flags, self._fisheye_calib_flags, self._pattern, name=self._camera_name,
checkerboard_flags=self._checkerboard_flags,
max_chessboard_speed = self._max_chessboard_speed)
else:
self.c = StereoCalibrator(self._boards, self._calib_flags, self._fisheye_calib_flags, self._pattern,
checkerboard_flags=self._checkerboard_flags,
max_chessboard_speed = self._max_chessboard_speed)
drawable = self.c.handle_msg(msg)
self.displaywidth = drawable.lscrib.shape[1] + drawable.rscrib.shape[1]
self.redraw_stereo(drawable)
def handle_stereo(self, msg):
if self.c == None:
self.c = StereoCalibrator(self._boards, self._calib_flags)
drawable = self.c.handle_msg(msg)
self.displaywidth = drawable.lscrib.cols + drawable.rscrib.cols
self.redraw_stereo(drawable)
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 cal_from_tarfile(boards, tarname, mono = False, upload = False):
if mono:
calibrator = MonoCalibrator(boards)
else:
calibrator = StereoCalibrator(boards)
calibrator.do_tarfile_calibration(tarname)
print calibrator.ost()
if upload:
info = calibrator.as_message()
if mono:
set_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("camera"), sensor_msgs.srv.SetCameraInfo)
response = set_camera_info_service(info)
if not response.success:
raise RuntimeError("connected to set_camera_info service, but failed setting camera_info")
else:
set_left_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("left_camera"), sensor_msgs.srv.SetCameraInfo)
set_right_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("right_camera"), sensor_msgs.srv.SetCameraInfo)
response1 = set_left_camera_info_service(info[0])
response2 = set_right_camera_info_service(info[1])
if not (response1.success and response2.success):
raise RuntimeError("connected to set_camera_info service, but failed setting camera_info")
def handle_stereo(self, msg):
(lmsg, lcmsg, rmsg, rcmsg) = msg
lgray = self.mkgray(lmsg)
rgray = self.mkgray(rmsg)
sc = StereoCalibrator([self.board])
L = self.image_corners(lgray)
R = self.image_corners(rgray)
scm = image_geometry.StereoCameraModel()
scm.fromCameraInfo(lcmsg, rcmsg)
if L and R:
d = [(y0 - y1) for ((_, y0), (_, y1)) in zip(L, R)]
epipolar = math.sqrt(sum([i**2 for i in d]) / len(d))
disparities = [(x0 - x1) for ((x0, y0), (x1, y1)) in zip(L, R)]
pt3d = [scm.projectPixelTo3d((x, y), d) for ((x, y), d) in zip(L, disparities)]
def l2(p0, p1):
return math.sqrt(sum([(c0 - c1) ** 2 for (c0, c1) in zip(p0, p1)]))
# Compute the length from each horizontal and vertical lines, and return the mean
cc = self.board.n_cols
cr = self.board.n_rows
lengths = (
[l2(pt3d[cc * r + 0], pt3d[cc * r + (cc - 1)]) / (cc - 1) for r in range(cr)] +
[l2(pt3d[c + 0], pt3d[c + (cc * (cr - 1))]) / (cr - 1) for c in range(cc)])
dimension = sum(lengths) / len(lengths)
print "epipolar error: %f pixels dimension: %f m" % (epipolar, dimension)
else:
print "no chessboard"
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 cal_from_tarfile(boards, tarname, mono=False):
if mono:
calibrator = MonoCalibrator(boards)
else:
calibrator = StereoCalibrator(boards)
calibrator.do_tarfile_calibration(tarname)
print calibrator.ost()
def test_nochecker(self):
# Run with same images, but looking for an incorrect chessboard size (8, 7).
# Should raise an exception because of lack of input points.
new_board = copy.deepcopy(board)
new_board.n_cols = 8
new_board.n_rows = 7
sc = StereoCalibrator([new_board])
self.assertRaises(CalibrationException, lambda: sc.cal(self.limages, self.rimages))
mc = MonoCalibrator([new_board])
self.assertRaises(CalibrationException, lambda: mc.cal(self.limages))
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.assertTrue(
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.assertTrue(len(sc2.ost()) > 0)
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 cal_from_tarfile(boards,
tarname,
mono=False,
upload=False,
calib_flags=0,
visualize=False,
alpha=1.0):
if mono:
calibrator = MonoCalibrator(boards, calib_flags)
else:
calibrator = StereoCalibrator(boards, calib_flags)
calibrator.do_tarfile_calibration(tarname)
print(calibrator.ost())
if upload:
info = calibrator.as_message()
if mono:
set_camera_info_service = rospy.ServiceProxy(
"%s/set_camera_info" % rospy.remap_name("camera"),
sensor_msgs.srv.SetCameraInfo)
response = set_camera_info_service(info)
if not response.success:
raise RuntimeError(
"connected to set_camera_info service, but failed setting camera_info"
)
else:
set_left_camera_info_service = rospy.ServiceProxy(
"%s/set_camera_info" % rospy.remap_name("left_camera"),
sensor_msgs.srv.SetCameraInfo)
set_right_camera_info_service = rospy.ServiceProxy(
"%s/set_camera_info" % rospy.remap_name("right_camera"),
sensor_msgs.srv.SetCameraInfo)
response1 = set_left_camera_info_service(info[0])
response2 = set_right_camera_info_service(info[1])
if not (response1.success and response2.success):
raise RuntimeError(
"connected to set_camera_info service, but failed setting camera_info"
)
if visualize:
#Show rectified images
calibrator.set_alpha(alpha)
archive = tarfile.open(tarname, 'r')
if mono:
for f in archive.getnames():
if f.startswith('left') and (f.endswith('.pgm')
or f.endswith('png')):
filedata = archive.extractfile(f).read()
file_bytes = numpy.asarray(bytearray(filedata),
dtype=numpy.uint8)
im = cv2.imdecode(file_bytes, cv2.IMREAD_COLOR)
bridge = cv_bridge.CvBridge()
try:
msg = bridge.cv2_to_imgmsg(im, "bgr8")
except cv_bridge.CvBridgeError as e:
print(e)
#handle msg returns the recitifed image with corner detection once camera is calibrated.
drawable = calibrator.handle_msg(msg)
vis = numpy.asarray(drawable.scrib[:, :])
#Display. Name of window:f
display(f, vis)
else:
limages = [
f for f in archive.getnames() if (f.startswith('left') and (
f.endswith('pgm') or f.endswith('png')))
]
limages.sort()
rimages = [
f for f in archive.getnames() if (f.startswith('right') and (
f.endswith('pgm') or f.endswith('png')))
]
rimages.sort()
if not len(limages) == len(rimages):
raise RuntimeError(
"Left, right images don't match. %d left images, %d right"
% (len(limages), len(rimages)))
for i in range(len(limages)):
l = limages[i]
r = rimages[i]
if l.startswith('left') and (
l.endswith('.pgm')
or l.endswith('png')) and r.startswith('right') and (
r.endswith('.pgm') or r.endswith('png')):
# LEFT IMAGE
filedata = archive.extractfile(l).read()
file_bytes = numpy.asarray(bytearray(filedata),
dtype=numpy.uint8)
im_left = cv2.imdecode(file_bytes, cv2.IMREAD_COLOR)
bridge = cv_bridge.CvBridge()
try:
msg_left = bridge.cv2_to_imgmsg(im_left, "bgr8")
except cv_bridge.CvBridgeError as e:
print(e)
#RIGHT IMAGE
filedata = archive.extractfile(r).read()
file_bytes = numpy.asarray(bytearray(filedata),
dtype=numpy.uint8)
im_right = cv2.imdecode(file_bytes, cv2.IMREAD_COLOR)
try:
msg_right = bridge.cv2_to_imgmsg(im_right, "bgr8")
except cv_bridge.CvBridgeError as e:
print(e)
drawable = calibrator.handle_msg([msg_left, msg_right])
h, w = numpy.asarray(drawable.lscrib[:, :]).shape[:2]
vis = numpy.zeros((h, w * 2, 3), numpy.uint8)
vis[:h, :w, :] = numpy.asarray(drawable.lscrib[:, :])
vis[:h, w:w * 2, :] = numpy.asarray(drawable.rscrib[:, :])
display(l + " " + r, vis)
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 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,
checkerboard_flags=self._checkerboard_flags)
else:
self.c = StereoCalibrator(
self._boards,
self._calib_flags,
self._pattern,
checkerboard_flags=self._checkerboard_flags)
drawable = self.c.handle_msg(msg)
self.displaywidth = drawable.lscrib.shape[1] + drawable.rscrib.shape[1]
self.redraw_stereo(drawable)
# added lines TODO:
# left_temp = points() #get msg type from corners
# right_temp = points() #get msg type from corners
# corner_msgs = corners()
lcorner_size_msg = Int32()
rcorner_size_msg = Int32()
# The mat is giving the ready-to-publish corner coordinates, push everything in a mat TODO:
if drawable.lcorner is not None:
lcorner_size = len(drawable.lcorner)
# publish the coner size for computation
lcorner_size_msg.data = lcorner_size
self.lcorner_size_pub.publish(lcorner_size_msg)
left_mat = Float32MultiArray()
left_mat.layout.dim.append(MultiArrayDimension())
left_mat.layout.dim.append(MultiArrayDimension())
left_mat.layout.dim[0].label = "row"
left_mat.layout.dim[1].label = "col"
left_mat.layout.dim[
0].size = lcorner_size # if there are 9 points give 9 dimension
left_mat.layout.dim[1].size = lcorner_size
left_mat.layout.dim[0].stride = lcorner_size
left_mat.layout.dim[1].stride = lcorner_size
left_mat.layout.data_offset = 0
left_mat.data = [0] * lcorner_size * lcorner_size
dstride1 = left_mat.layout.dim[1].stride
offset = left_mat.layout.data_offset
print("left") # debug
print(drawable.lcorner) # debug
i_l = 0
for temp_left in drawable.lcorner:
temp_x = temp_left[0, 0]
temp_y = temp_left[0, 1]
left_mat.data[offset + i_l + dstride1 * 0] = temp_x
left_mat.data[offset + i_l + dstride1 * 1] = temp_y
i_l += 1
self.left_pub.publish(left_mat)
else:
print()
print("No LEFT corner coordinates")
print()
if drawable.rcorner is not None:
rcorner_size = len(drawable.rcorner)
rcorner_size_msg.data = rcorner_size
self.rcorner_size_pub.publish(rcorner_size_msg)
right_mat = Float32MultiArray()
right_mat.layout.dim.append(MultiArrayDimension())
right_mat.layout.dim.append(MultiArrayDimension())
right_mat.layout.dim[0].label = "row"
right_mat.layout.dim[1].label = "col"
right_mat.layout.dim[0].size = rcorner_size
right_mat.layout.dim[1].size = rcorner_size
right_mat.layout.dim[0].stride = rcorner_size
right_mat.layout.dim[1].stride = rcorner_size
right_mat.layout.data_offset = 0
right_mat.data = [0] * rcorner_size * rcorner_size
dstride1 = right_mat.layout.dim[1].stride
offset = right_mat.layout.data_offset
print("right") # debug
print(drawable.rcorner) #debug
i_r = 0
for temp_right in drawable.rcorner:
temp_x = temp_right[0, 0]
temp_y = temp_right[0, 1]
right_mat.data[offset + i_r + dstride1 * 0] = temp_x
right_mat.data[offset + i_r + dstride1 * 1] = temp_y
i_r += 1
self.right_pub.publish(right_mat)
else:
print()
print("No RIGHT corner coordinates")
print()
# tryp to publisht the intrinsic matrix for left and right camera
# if self.c.is_mono:
# pass
# else:
# stereo_cam_info = self.c.as_message()
# left_response = self.set_left_camera_info_service(stereo_cam_info[0])
# left_intrinsic = left_response.camera_info.K
# right_response = self.set_right_camera_info_service(stereo_cam_info[1])
# right_intrinsic = right_response.camera_info.K
# camera_intrinsics_msg.left_intirnsic = left_intrinsic
# camera_intrinsics_msg.right_intirnsic = right_intrinsic
# self.intrinsic_pub.publish(camera_intrinsics_msg)
rospy.sleep(0.1)
