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