def main(args): from optparse import OptionParser parser = OptionParser() parser.add_option("-s", "--size", default="8x6", help="specify chessboard size as nxm [default: %default]") parser.add_option("-q", "--square", default=".108", help="specify chessboard square size in meters [default: %default]") options, args = parser.parse_args() size = tuple([int(c) for c in options.size.split('x')]) dim = float(options.square) images = [] for fname in args: if os.path.isfile(fname): img = cv.LoadImage(fname) if img is None: print >> sys.stderr, "[WARN] Couldn't open image " + fname + "!" sys.exit(1) else: print "[INFO] Loaded " + fname + " (" + str(img.width) + "x" + str(img.height) + ")" images.append(img) cboard = ChessboardInfo() cboard.dim = dim cboard.n_cols = size[0] cboard.n_rows = size[1] mc = MonoCalibrator([cboard]) mc.cal(images) print mc.as_message()
def test_rational_polynomial_model(self): """Test that the distortion coefficients returned for a rational_polynomial model are not empty.""" 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_RATIONAL_MODEL, pattern=setup.pattern) mc.cal(self.limages[i]) self.assertEqual( len(mc.distortion.flat), 8, 'length of distortion coefficients is %d' % len(mc.distortion.flat)) self.assertTrue( all(mc.distortion.flat != 0), 'some distortion coefficients are zero: %s' % str(mc.distortion.flatten())) self.assertEqual(mc.as_message().distortion_model, 'rational_polynomial') self.assert_good_mono(mc, self.limages[i], setup.lin_err) yaml = mc.yaml() # Issue #278 self.assertIn('cols: 8', yaml)
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 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_camera_info_service(info[0]) response2 = set_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 test_monocular(self): # Run the calibrator, produce a calibration, check it mc = MonoCalibrator([board], flags=cv2.CALIB_FIX_K3) for dim in self.sizes: mc.cal(self.l[dim]) self.assert_good_mono(mc, dim) # 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)
def test_monocular(self): # Run the calibrator, produce a calibration, check it mc = MonoCalibrator([ board ], cv2.CALIB_FIX_K3) for dim in self.sizes: mc.cal(self.l[dim]) self.assert_good_mono(mc, dim) # 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)
def main(args): from optparse import OptionParser parser = OptionParser() parser.add_option( "-s", "--size", default="8x6", help="specify chessboard size as nxm [default: %default]") parser.add_option( "-q", "--square", default=".108", help="specify chessboard square size in meters [default: %default]") options, args = parser.parse_args() size = tuple([int(c) for c in options.size.split('x')]) dim = float(options.square) images = [] for fname in args: if os.path.isfile(fname): img = cv.LoadImage(fname) if img is None: print >> sys.stderr, "[WARN] Couldn't open image " + fname + "!" sys.exit(1) else: print "[INFO] Loaded " + fname + " (" + str( img.width) + "x" + str(img.height) + ")" images.append(img) cboard = ChessboardInfo() cboard.dim = dim cboard.n_cols = size[0] cboard.n_rows = size[1] mc = MonoCalibrator([cboard]) mc.cal(images) print mc.as_message()
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 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 find_intrinsics(visualize=False): fatal = False good = [] xys = [] if 1: with open('setup.json', mode='r') as fd: setup = json.loads(fd.read()) globber = '*-corners*.data' files = glob.glob(globber) files.sort() print 'all data files matching "%s"' % (globber, ) print files for f in files: this_fatal = False parts = f.split('-') board = None for p in parts: if p.startswith('board'): boardname = p with open(boardname + '.json', mode='r') as fd: buf = fd.read() boardd = json.loads(buf) board = ChessboardInfo() board.n_cols = boardd['n_cols'] board.n_rows = boardd['n_rows'] board.dim = boardd['dim'] assert board is not None xy = np.loadtxt(f) xys.append(xy) if len(xy) != board.n_cols * board.n_rows: rospy.logfatal('Error: %d points in %s. Expected %d.' % (len(xy), f, board.n_cols * board.n_rows)) this_fatal = True fatal = True if visualize: if 0: plt.figure() fmt = 'o-' label = f if this_fatal: fmt = 'kx:' label = 'BAD: ' + f plt.plot(xy[:, 0], xy[:, 1], fmt, mfc='none', label=label) if this_fatal: for i in range(len(xy)): plt.text(xy[i, 0], xy[i, 1], str(i)) corners = [(x, y) for (x, y) in xy] good.append((corners, board)) if visualize: plt.legend() plt.show() if fatal: sys.exit(1) mc = MonoCalibrator( [board] ) #, cv2.CALIB_FIX_K1 | cv2.CALIB_FIX_K2 | cv2.CALIB_FIX_K3 | cv2.CALIB_ZERO_TANGENT_DIST ) mc.size = tuple(setup['size']) mc.cal_fromcorners(good) msg = mc.as_message() cam = pymvg.CameraModel.from_ros_like(intrinsics=msg) cam_mirror = cam.get_mirror_camera() if 1: # The intrinsics are valid for the whole physical display and # each virtual display. names = setup['names'] for name in names: name = str( name) # remove unicode -- ROS bag fails on unicode topic name if 'mirror' in name: c = cam_mirror else: c = cam msg = c.get_intrinsics_as_msg() fname = 'display-intrinsic-cal-%s.bag' % (name.replace('/', '-'), ) bagout = rosbag.Bag(fname, 'w') topic = '/%s/camera_info' % name bagout.write(topic, msg) bagout.close() print 'saved to', fname print 'You can play this calibration with:\n' print 'rosbag play %s -l' % (fname, ) print
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.CV_LOAD_IMAGE_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.CV_LOAD_IMAGE_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.CV_LOAD_IMAGE_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)
import cv2 from camera_calibration.calibrator import MonoCalibrator, ChessboardInfo numImages = 30 images = [ cv2.imread('../Images/frame{:04d}.jpg'.format(i)) for i in range(numImages) ] board = ChessboardInfo() board.n_cols = 7 board.n_rows = 5 board.dim = 0.050 mc = MonoCalibrator([board], cv2.CALIB_FIX_K3) mc.cal(images) print(mc.as_message()) mc.do_save()
class CalibrationNode: def __init__(self, boards, service_check=True, synchronizer=message_filters.TimeSynchronizer, 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 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.q_mono = Queue.Queue() self.c = None mth = ConsumerThread(self.q_mono, self.handle_monocular) mth.setDaemon(True) mth.start() def redraw_monocular(self, *args): pass def queue_monocular(self, msg): self.q_mono.put(msg) def handle_monocular(self, msg): if self.c == None: self.c = MonoCalibrator(self._boards, self._calib_flags) #print 'BBBBBBBBOOOOOOOOOOOOOOARRRRRRDS',self._boards.gridsize # This should just call the MonoCalibrator drawable = self.c.handle_msg(msg) #print drawable.scrib.shape[1] self.displaywidth = drawable.scrib.shape[1] print(drawable) print(drawable.scrib) self.redraw_monocular(drawable) def check_set_camera_info(self, response): if response.success: return True for i in range(10): print("!" * 80) print("Attempt to set camera info failed: " + response.status_message) for i in range(10): print("!" * 80) 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 find_intrinsics(visualize=False): fatal = False good = [] xys = [] if 1: with open('setup.json',mode='r') as fd: setup = json.loads(fd.read()) globber = '*-corners*.data' files = glob.glob(globber) files.sort() print 'all data files matching "%s"'%(globber,) print files for f in files: this_fatal = False parts = f.split('-') board = None for p in parts: if p.startswith('board'): boardname = p with open(boardname+'.json',mode='r') as fd: buf = fd.read() boardd = json.loads(buf) board = ChessboardInfo() board.n_cols = boardd['n_cols'] board.n_rows = boardd['n_rows'] board.dim = boardd['dim'] assert board is not None xy = np.loadtxt(f) xys.append(xy) if len(xy) != board.n_cols * board.n_rows: rospy.logfatal('Error: %d points in %s. Expected %d.'%( len(xy), f, board.n_cols * board.n_rows)) this_fatal = True fatal = True if visualize: if 0: plt.figure() fmt = 'o-' label = f if this_fatal: fmt = 'kx:' label = 'BAD: '+ f plt.plot( xy[:,0], xy[:,1], fmt, mfc='none', label=label ) if this_fatal: for i in range(len(xy)): plt.text( xy[i,0], xy[i,1], str(i) ) corners = [ (x,y) for (x,y) in xy ] good.append( (corners, board) ) if visualize: plt.legend() plt.show() if fatal: sys.exit(1) mc = MonoCalibrator([ board ])#, cv2.CALIB_FIX_K1 | cv2.CALIB_FIX_K2 | cv2.CALIB_FIX_K3 | cv2.CALIB_ZERO_TANGENT_DIST ) mc.size = tuple(setup['size']) mc.cal_fromcorners(good) msg = mc.as_message() cam = pymvg.CameraModel.from_ros_like( intrinsics=msg ) cam_mirror = cam.get_mirror_camera() if 1: # The intrinsics are valid for the whole physical display and # each virtual display. names = setup['names'] for name in names: name = str(name) # remove unicode -- ROS bag fails on unicode topic name if 'mirror' in name: c = cam_mirror else: c = cam msg = c.get_intrinsics_as_msg() fname = 'display-intrinsic-cal-%s.bag'%( name.replace('/','-'), ) bagout = rosbag.Bag(fname, 'w') topic = '/%s/camera_info'%name bagout.write(topic, msg) bagout.close() print 'saved to',fname print 'You can play this calibration with:\n' print 'rosbag play %s -l'%(fname,) print