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")
