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