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 __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_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_monocular(self):
# Run the calibrator, produce a calibration, check it
mc = MonoCalibrator([ board ], cv2.CALIB_FIX_K3)
max_errs = [0.1, 0.2, 0.4, 0.7]
for i, dim in enumerate(self.sizes):
mc.cal(self.l[dim])
self.assert_good_mono(mc, dim, max_errs[i])
# Make another calibration, import previous calibration as a message,
# and assert that the new one is good.
mc2 = MonoCalibrator([board])
mc2.from_message(mc.as_message())
self.assert_good_mono(mc2, dim, max_errs[i])
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))
class CameraCheckerNode:
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 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 is not None and R is not None:
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")
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 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)
class CameraCheckerNode:
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 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 is not None and R is not None:
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")