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 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:
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 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 __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 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.cols + drawable.rscrib.cols
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)
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 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 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, distortion_model=self._distortion_model, checkerboard_flags=self._checkerboard_flags)
else:
self.c = MonoCalibrator(self._boards, self._calib_flags, self._pattern, distortion_model=self._distortion_model, checkerboard_flags=self._checkerboard_flags)
# This should just call the MonoCalibrator
drawable = self.c.handle_msg(msg)
self.displaywidth = drawable.scrib.shape[1]
self.redraw_monocular(drawable)
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)
else:
self.c = MonoCalibrator(self._boards, self._calib_flags, self._pattern)
# This should just call the MonoCalibrator
drawable = self.c.handle_msg(msg)
self.displaywidth = drawable.scrib.cols
self.redraw_monocular(drawable)
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 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._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)
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_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)
else:
self.c = MonoCalibrator(self._boards, self._calib_flags,
self._pattern)
# This should just call the MonoCalibrator
drawable = self.c.handle_msg(msg)
self.displaywidth = drawable.scrib.cols
self.redraw_monocular(drawable)
def handle_monocular(self, msg):
if self.c == None:
if self._camera_name:
self.c = MonoCalibrator(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 = MonoCalibrator(self._boards, self._calib_flags, self._fisheye_calib_flags, self._pattern,
checkerboard_flags=self.checkerboard_flags,
max_chessboard_speed = self._max_chessboard_speed)
# This should just call the MonoCalibrator
drawable = self.c.handle_msg(msg)
self.displaywidth = drawable.scrib.shape[1]
self.redraw_monocular(drawable)
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 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_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, chess_size, dim):
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),
]
tsm = message_filters.TimeSynchronizer([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 = message_filters.TimeSynchronizer([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])
class CameraCheckerNode:
def __init__(self, chess_size, dim):
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),
]
tsm = message_filters.TimeSynchronizer([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 = message_filters.TimeSynchronizer([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 queue_monocular(self, msg, cmsg):
self.q_mono.put((msg, cmsg))
def queue_stereo(self, lmsg, lcmsg, rmsg, rcmsg):
self.q_stereo.put((lmsg, lcmsg, rmsg, rcmsg))
def mkgray(self, msg):
return self.mc.mkgray(msg)
def image_corners(self, im):
(ok, corners, b) = self.mc.get_corners(im)
if ok:
return corners
else:
return None
def handle_monocular(self, msg):
(image, camera) = msg
gray = self.mkgray(image)
C = self.image_corners(gray)
if C:
linearity_rms = self.mc.linear_error(C, self.board)
# Add in reprojection check
image_points = C
object_points = self.mc.mk_object_points([self.board], use_board_size=True)[0]
dist_coeffs = numpy.zeros((4, 1))
camera_matrix = numpy.array( [ [ camera.P[0], camera.P[1], camera.P[2] ],
[ camera.P[4], camera.P[5], camera.P[6] ],
[ camera.P[8], camera.P[9], camera.P[10] ] ] )
ok, rot, trans = cv2.solvePnP(object_points, image_points, camera_matrix, dist_coeffs)
# Convert rotation into a 3x3 Rotation Matrix
rot3x3, _ = cv2.Rodrigues(rot)
# Reproject model points into image
object_points_world = numpy.asmatrix(rot3x3) * (numpy.asmatrix(object_points.squeeze().T) + numpy.asmatrix(trans))
reprojected_h = camera_matrix * object_points_world
reprojected = (reprojected_h[0:2, :] / reprojected_h[2, :])
reprojection_errors = image_points.squeeze().T - reprojected.T
reprojection_rms = numpy.sqrt(numpy.sum(numpy.array(reprojection_errors) ** 2) / numpy.product(reprojection_errors.shape))
# Print the results
print("Linearity RMS Error: %.3f Pixels Reprojection RMS Error: %.3f Pixels" % (linearity_rms, reprojection_rms))
else:
print('no chessboard')
def handle_stereo(self, msg):
(lmsg, lcmsg, rmsg, rcmsg) = msg
lgray = self.mkgray(lmsg)
rgray = self.mkgray(rmsg)
L = self.image_corners(lgray)
R = self.image_corners(rgray)
if L and R:
epipolar = self.sc.epipolar_error(L, R)
dimension = self.sc.chessboard_size(L, R, [self.board], msg=(lcmsg, rcmsg))
print("epipolar error: %f pixels dimension: %f m" % (epipolar, dimension))
else:
print("no chessboard")
class CameraCheckerNode:
def __init__(self, chess_size, dim):
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),
]
tsm = message_filters.TimeSynchronizer([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 = message_filters.TimeSynchronizer([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 queue_monocular(self, msg, cmsg):
self.q_mono.put((msg, cmsg))
def queue_stereo(self, lmsg, lcmsg, rmsg, rcmsg):
self.q_stereo.put((lmsg, lcmsg, rmsg, rcmsg))
def mkgray(self, msg):
return self.mc.mkgray(msg)
def image_corners(self, im):
(ok, corners, b) = self.mc.get_corners(im)
if ok:
return corners
else:
return None
def handle_monocular(self, msg):
(image, camera) = msg
gray = self.mkgray(image)
C = self.image_corners(gray)
if C is not None:
linearity_rms = self.mc.linear_error(C, self.board)
# Add in reprojection check
image_points = C
object_points = self.mc.mk_object_points([self.board], use_board_size=True)[0]
dist_coeffs = numpy.zeros((4, 1))
camera_matrix = numpy.array( [ [ camera.P[0], camera.P[1], camera.P[2] ],
[ camera.P[4], camera.P[5], camera.P[6] ],
[ camera.P[8], camera.P[9], camera.P[10] ] ] )
ok, rot, trans = cv2.solvePnP(object_points, image_points, camera_matrix, dist_coeffs)
# Convert rotation into a 3x3 Rotation Matrix
rot3x3, _ = cv2.Rodrigues(rot)
# Reproject model points into image
object_points_world = numpy.asmatrix(rot3x3) * numpy.asmatrix(object_points.squeeze().T) + numpy.asmatrix(trans)
reprojected_h = camera_matrix * object_points_world
reprojected = (reprojected_h[0:2, :] / reprojected_h[2, :])
reprojection_errors = image_points.squeeze().T - reprojected
reprojection_rms = numpy.sqrt(numpy.sum(numpy.array(reprojection_errors) ** 2) / numpy.product(reprojection_errors.shape))
# Print the results
print("Linearity RMS Error: %.3f Pixels Reprojection RMS Error: %.3f Pixels" % (linearity_rms, reprojection_rms))
else:
print('no chessboard')
def handle_stereo(self, msg):
(lmsg, lcmsg, rmsg, rcmsg) = msg
lgray = self.mkgray(lmsg)
rgray = self.mkgray(rmsg)
L = self.image_corners(lgray)
R = self.image_corners(rgray)
if L and R:
epipolar = self.sc.epipolar_error(L, R)
dimension = self.sc.chessboard_size(L, R, [self.board], msg=(lcmsg, rcmsg))
print("epipolar error: %f pixels dimension: %f m" % (epipolar, dimension))
else:
print("no chessboard")
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('calibration')
self._autoware_image = cv2.imread(
path.join(pkg_path, 'docs/smartcar_logo.png'),
cv2.IMREAD_UNCHANGED)
self._autoware_image = cv2.resize(self._autoware_image, (100, 25))
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
class CalibrationNode:
def __init__(self,
boards,
service_check=True,
synchronizer=message_filters.TimeSynchronizer,
flags=0,
pattern=Patterns.Chessboard,
camera_name='',
checkerboard_flags=0):
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._calib_flags = flags
self._checkerboard_flags = checkerboard_flags
self._pattern = pattern
self._camera_name = camera_name
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
# add publisher when received the corner information TODO:
self.left_pub = rospy.Publisher('/left_corners',
Float32MultiArray,
queue_size=100)
self.right_pub = rospy.Publisher('/right_corners',
Float32MultiArray,
queue_size=100)
self.lcorner_size_pub = rospy.Publisher('/get_corner_size_l',
Int32,
queue_size=100)
self.rcorner_size_pub = rospy.Publisher('/get_corner_size_r',
Int32,
queue_size=100)
# self.intrinsic_pub = rospy.Publisher('/camera_intrinsics', intrinsic_param, queue_size = 100)
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 convert_point2f_to_list(
self, input_corners
): # change cv::Point2f(numpy.ndarray in python) to list
corner_Xs = input_corners[:, :, 0]
corner_Ys = input_corners[:, :, 1]
new_corner = zip(corner_Xs, corner_Ys)
corner_tuple = tuple(new_corner)
return corner_tuple
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,
checkerboard_flags=self._checkerboard_flags)
else:
self.c = MonoCalibrator(
self._boards,
self._calib_flags,
self._pattern,
checkerboard_flags=self.checkerboard_flags)
# 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,
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)
# end of the handle_strero
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
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)
class CalibrationNode:
def __init__(self, boards, service_check = True, synchronizer = message_filters.TimeSynchronizer, flags = 0,
pattern=Patterns.Chessboard, camera_name='', checkerboard_flags = 0):
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._calib_flags = flags
self._checkerboard_flags = checkerboard_flags
self._pattern = pattern
self._camera_name = camera_name
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,
checkerboard_flags=self._checkerboard_flags)
else:
self.c = MonoCalibrator(self._boards, self._calib_flags, self._pattern,
checkerboard_flags=self.checkerboard_flags)
# 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,
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)
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
def test_stereo(self):
epierrors = [0.1, 0.2, 0.4, 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(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 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 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 __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])
class CalibrationNode:
def __init__(self, boards, service_check = True, synchronizer = message_filters.TimeSynchronizer, flags = 0,
fisheye_flags = 0, pattern=Patterns.Chessboard, camera_name='', checkerboard_flags = 0,
max_chessboard_speed = -1, queue_size = 1):
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._calib_flags = flags
self._fisheye_calib_flags = fisheye_flags
self._checkerboard_flags = checkerboard_flags
self._pattern = pattern
self._camera_name = camera_name
self._max_chessboard_speed = max_chessboard_speed
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 = BufferQueue(queue_size)
self.q_stereo = BufferQueue(queue_size)
self.c = None
self._last_display = 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.put(msg)
def queue_stereo(self, lmsg, rmsg):
self.q_stereo.put((lmsg, rmsg))
def handle_monocular(self, msg):
if self.c == None:
if self._camera_name:
self.c = MonoCalibrator(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 = MonoCalibrator(self._boards, self._calib_flags, self._fisheye_calib_flags, self._pattern,
checkerboard_flags=self.checkerboard_flags,
max_chessboard_speed = self._max_chessboard_speed)
# 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._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 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
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)
class CalibrationNode(Node):
def __init__(self,
name,
boards,
service_check=True,
synchronizer=message_filters.TimeSynchronizer,
flags=0,
pattern=Patterns.Chessboard,
camera_name='',
checkerboard_flags=0):
super().__init__(name)
self.set_camera_info_service = self.create_client(
sensor_msgs.srv.SetCameraInfo, "camera/set_camera_info")
self.set_left_camera_info_service = self.create_client(
sensor_msgs.srv.SetCameraInfo, "left_camera/set_camera_info")
self.set_right_camera_info_service = self.create_client(
sensor_msgs.srv.SetCameraInfo, "right_camera/set_camera_info")
if service_check:
# assume any non-default service names have been set. Wait for the service to become ready
for cli in [
self.set_camera_info_service,
self.set_left_camera_info_service,
self.set_right_camera_info_service
]:
#remapped = rclpy.remap_name(svcname)
#if remapped != svcname:
#fullservicename = "%s/set_camera_info" % remapped
print("Waiting for service", cli.srv_name, "...")
# check all services so they are ready.
try:
cli.wait_for_service(timeout_sec=5)
print("OK")
except Exception as e:
print("Service not found: %s".format(e))
rclpy.shutdown()
self._boards = boards
self._calib_flags = flags
self._checkerboard_flags = checkerboard_flags
self._pattern = pattern
self._camera_name = camera_name
lsub = message_filters.Subscriber(self, sensor_msgs.msg.Image, 'left')
rsub = message_filters.Subscriber(self, sensor_msgs.msg.Image, 'right')
ts = synchronizer([lsub, rsub], 4)
ts.registerCallback(self.queue_stereo)
msub = message_filters.Subscriber(self, sensor_msgs.msg.Image, 'image')
msub.registerCallback(self.queue_monocular)
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,
checkerboard_flags=self._checkerboard_flags)
else:
self.c = MonoCalibrator(
self._boards,
self._calib_flags,
self._pattern,
checkerboard_flags=self.checkerboard_flags)
# 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,
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)
def check_set_camera_info(self, response):
if response.done():
if response.result() is not None:
if response.result().success:
return True
for i in range(10):
print("!" * 80)
print()
print("Attempt to set camera info failed: " +
response.result() if response.result(
) is not None else "Not available")
print()
for i in range(10):
print("!" * 80)
print()
self.get_logger().error(
'Unable to set camera info for calibration. Failure message: %s' %
response.result() if response.result(
) is not None else "Not available")
return False
def do_upload(self):
self.c.report()
print(self.c.ost())
info = self.c.as_message()
req = sensor_msgs.srv.SetCameraInfo.Request()
rv = True
if self.c.is_mono:
req.camera_info = info
response = self.set_camera_info_service.call_async(req)
rv = self.check_set_camera_info(response)
else:
req.camera_info = info[0]
response = self.set_left_camera_info_service.call_async(req)
rv = rv and self.check_set_camera_info(response)
req.camera_info = info[1]
response = self.set_right_camera_info_service.call_async(req)
rv = rv and self.check_set_camera_info(response)
return rv
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)
class CalibrationNode(Node):
def __init__(self,
boards,
service_check=False,
synchronizer=message_filters.TimeSynchronizer,
flags=0,
pattern=Patterns.Chessboard,
camera_name='camera',
camera='camera',
left_camera='left_camera',
right_camera='right_camera',
image='image',
checkerboard_flags=0):
# TODO will enable this function as soon as set_camera_info enabled
# 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 = svcname
# if remapped != svcname:
# fullservicename = "%s/set_camera_info" % remapped
# print("Waiting for service", fullservicename, "...")
# try:
# rclpy.wait_for_service(fullservicename, 5)
# print("OK")
# except:
# print("Service not found")
# rclpy.shutdown()
super().__init__(self._node_name)
self._boards = boards
self._calib_flags = flags
self._checkerboard_flags = checkerboard_flags
self._pattern = pattern
self._camera_name = camera_name
lsub = message_filters.Subscriber(self, sensor_msgs.msg.Image,
left_camera)
rsub = message_filters.Subscriber(self, sensor_msgs.msg.Image,
right_camera)
ts = synchronizer([lsub, rsub], 4)
ts.registerCallback(self.queue_stereo)
msub = message_filters.Subscriber(self, sensor_msgs.msg.Image, image)
msub.registerCallback(self.queue_monocular)
self.set_camera_info_cli = self.create_client(
sensor_msgs.srv.SetCameraInfo, "%s/set_camera_info" % camera)
self.set_left_camera_cli = self.create_client(
sensor_msgs.srv.SetCameraInfo, "%s/set_camera_info" % left_camera)
self.set_right_camera_cli = self.create_client(
sensor_msgs.srv.SetCameraInfo, "%s/set_camera_info" % right_camera)
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,
checkerboard_flags=self._checkerboard_flags)
else:
self.c = MonoCalibrator(
self._boards,
self._calib_flags,
self._pattern,
checkerboard_flags=self.checkerboard_flags)
# 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,
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)
def check_set_camera_info(self, response):
if response.result().success:
return True
for i in range(10):
print("!" * 80)
print()
print("Attempt to set camera info failed: " +
response.result().status_message)
print()
for i in range(10):
print("!" * 80)
print()
self.get_logger().error(
'Unable to set camera info for calibration. Failure message: %s' %
response.result().status_message)
return False
def do_upload(self):
self.c.report()
print(self.c.ost())
info = self.c.as_message()
req = sensor_msgs.srv.SetCameraInfo.Request()
rv = True
if self.c.is_mono:
req.camera_info = info
response = self.set_camera_info_cli.call_async(req)
rv = self.check_set_camera_info(response)
else:
req.camera_info = info[0]
response = self.set_left_camera_info_service(req)
rv = rv and self.check_set_camera_info(response)
req.camera_info = info[1]
response = self.set_right_camera_info_service(req)
rv = rv and self.check_set_camera_info(response)
return rv