def __init__(self): ''' Configures the calibration node Reads configuration from parameter server or uses default values ''' rospy.init_node(NODE) print "==> started " + NODE # get parameter from parameter server or set defaults self.pattern_size = rospy.get_param('~pattern_size', "9x6") self.square_size = rospy.get_param('~square_size', 0.03) self.alpha = rospy.get_param('~alpha', 0.0) self.verbose = rospy.get_param('~verbose', True) self.save_result = rospy.get_param('~save_result', False) # split pattern_size string into tuple, e.g '9x6' -> tuple(9,6) self.pattern_size = tuple((int(self.pattern_size.split("x")[0]), int(self.pattern_size.split("x")[1]))) self.camera_info = CameraInfo() self.camera_info_received = False self.latest_image = Image() self.bridge = CvBridge() # set up Checkerboard and CheckerboardDetector self.board = Checkerboard(self.pattern_size, self.square_size) self.detector = CheckerboardDetector(self.board) self.sss = simple_script_server() self.ts = TorsoState()
def __init__(self): ''' Configures the calibration node Reads configuration from parameter server or uses default values ''' # get parameter from parameter server or set defaults self.pattern_size = rospy.get_param('~pattern_size', "9x6") self.square_size = rospy.get_param('~square_size', 0.03) self.alpha = rospy.get_param('~alpha', 0.0) self.verbose = rospy.get_param('~verbose', True) self.save_result = rospy.get_param('~save_result', False) # split pattern_size string into tuple, e.g '9x6' -> tuple(9,6) self.pattern_size = tuple((int(self.pattern_size.split("x")[0]), int(self.pattern_size.split("x")[1]))) self.camera_info = CameraInfo() self.camera_info_received = False self.latest_image = Image() self.bridge = CvBridge() # set up Checkerboard and CheckerboardDetector self.board = Checkerboard(self.pattern_size, self.square_size) self.detector = CheckerboardDetector(self.board) self.sss = simple_script_server() topic_name = '/stereo/left/' # subscribe to /cam3d/rgb/camera_info for camera_matrix and distortion coefficients rospy.Subscriber(topic_name + 'camera_info', CameraInfo, self.__camera_info_callback__) # subscribe to /cam3d/rgb/image_raw for image data rospy.Subscriber(topic_name + 'image_color', Image, self.__image_raw_callback__) # wait until camera informations are recieved. start_time = rospy.Time.now() while not (self.camera_info_received or rospy.is_shutdown()): rospy.sleep(0.05) if start_time + rospy.Duration(2.0) < rospy.Time.now(): # print warning every 2 seconds if the message is stil missing print "--> still waiting for /cam3d/rgb/camera_info" start_time = rospy.Time.now() # convert camera matrix an distortion coefficients and store them camera_matrix = self.camera_info.K cm = np.asarray(camera_matrix) self.cm = np.reshape(cm, (3, 3)) dist_coeffs = self.camera_info.D self.dc = np.asarray(dist_coeffs) self.frame = self.camera_info.header.frame_id
def test_detect_image_points_color_image(self): # create Checkerboard instance and read image c = Checkerboard((9, 6), 0.03) image = cv2.imread(CHECKERBOARD_IMAGE, 0) # create CheckerboardDetector instance and detect image points cd = CheckerboardDetector(c) points = cd.detect_image_points(image, True) #dump = {'points': points.flatten().tolist()} #yaml.dump(dump, open(CHECKERBOARD_IMAGE_EXPECTED_POINTS_YAML, "w")) # load expected points points_expected = yaml.load(file(CHECKERBOARD_IMAGE_EXPECTED_POINTS_YAML))['points'] self.assertTrue(np.allclose(points.flatten().tolist(), points_expected))
def __init__(self): rospy.init_node(NODE) pattern = rospy.get_param('~calibration_pattern') self.cameras = rospy.get_param('~cameras') pattern_string = pattern["pattern_size"].split("x") pattern_size = (int(pattern_string[0]), int(pattern_string[1])) cb = Checkerboard(pattern_size, pattern["square_size"]) self.detector = CheckerboardDetector(cb) self.bridge = CvBridge() self.pub = rospy.Publisher("cb_detections", MarkerArray) self.topics = [self.cameras["reference"]["topic"]] for camera in self.cameras["further"]: self.topics.append(camera["topic"]) self.info_topics = [ t.replace("image_raw", "camera_info") for t in self.topics ] self.info_topics = [ t.replace("image_color", "camera_info") for t in self.info_topics ] print self.topics print self.info_topics self.images = dict.fromkeys(self.topics) self.camera_infos = dict.fromkeys(self.topics) self.colors = [ColorRGBA() for i in range(len(self.topics))] hue_values = np.linspace(0, 1, len(self.topics) + 1) for c, hue, topic in zip(self.colors, hue_values, self.topics): c.a = 1 (c.r, c.g, c.b) = hsv_to_rgb(hue, 1, 1) print "Listening to: %s - Color RGB: (%d, %d, %d)" % ( topic, c.r * 255, c.g * 255, c.b * 255) self.points = [] for point in cb.get_pattern_points(): p = Point() p.x = point[0] p.y = point[1] p.z = point[2] self.points.append(p) for topic, info_topic in zip(self.topics, self.info_topics): rospy.Subscriber(topic, Image, self.callback_image, topic) rospy.Subscriber(info_topic, CameraInfo, self.callback_info, topic) rospy.sleep(3)
def run_mono_calibration(self): ''' Runs the calibration ''' print "==> starting monocular calibration" # set up Checkerboard, CheckerboardDetector and MonoCalibrator board = Checkerboard(self.pattern_size, self.square_size) detector = CheckerboardDetector(board) calibrator = MonoCalibrator(board, detector, self.folder, self.image_prefix) # run calibration (rms, camera_matrix, projection_matrix, dist_coeffs, (h, w), _, _) = calibrator.calibrate_monocular_camera(self.alpha) print "==> successfully calibrated, reprojection RMS (in pixels):", rms # create CalibrationData object with results camera_info = CalibrationData(self.camera_name, self.frame_id, w, h) camera_info.camera_matrix = camera_matrix camera_info.distortion_coefficients = dist_coeffs camera_info.projection_matrix = projection_matrix camera_info.dist_coeffs = dist_coeffs # save results camera_info.save_camera_yaml_file(self.output_file) print "==> saved results to:", self.output_file # verbose mode if self.verbose: print "--> results:" np.set_printoptions(suppress=1) print "camera matrix:\n", camera_matrix print "distortion coefficients:\n", dist_coeffs print "projection matrix:\n", projection_matrix
def __init__(self): ''' Initializes storage, gets parameters from parameter server and logs to rosinfo ''' self.bridge = CvBridge() # set up Checkerboard and CheckerboardDetector self.board = Checkerboard((9, 6), 0.03) self.detector = CheckerboardDetector(self.board) rospy.init_node(NODE) self.counter = 0 # Get params from ros parameter server or use default self.cams = rospy.get_param("~cameras") self.camera = [self.cams["reference"]["topic"]] for cam in self.cams["further"]: self.camera.append(cam["topic"]) self.numCams = len(self.camera) # Init images self.image = [] self.image_received = [False] * self.numCams for id in range(self.numCams): self.image.append(Image()) # Subscribe to images self.imageSub = [] for id in range(self.numCams): self.imageSub.append(rospy.Subscriber( self.camera[id], Image, self._imageCallback, id)) # Wait for image messages for id in range(self.numCams): rospy.wait_for_message(self.camera[id], Image, 5)
def __init__(self): ''' Configures the calibration node Reads configuration from parameter server or uses default values ''' rospy.init_node(NODE) print "==> started " + NODE # get parameter from parameter server or set defaults self.pattern_size = rospy.get_param('~pattern_size', "9x6") self.square_size = rospy.get_param('~square_size', 0.03) self.alpha = rospy.get_param('~alpha', 0.0) self.verbose = rospy.get_param('~verbose', True) self.save_result = rospy.get_param('~save_result', False) # split pattern_size string into tuple, e.g '9x6' -> tuple(9,6) self.pattern_size = tuple((int(self.pattern_size.split( "x")[0]), int(self.pattern_size.split("x")[1]))) self.camera_info = CameraInfo() self.camera_info_received = False self.latest_image = Image() self.bridge = CvBridge() # set up Checkerboard and CheckerboardDetector self.board = Checkerboard(self.pattern_size, self.square_size) self.detector = CheckerboardDetector(self.board) self.sss = simple_script_server() self.ts = TorsoState()
def get_corners(self): # -------------------- # receive images # ----------------- for v in self._images_received: self._images_received[v] = False start_time = rospy.Time.now() while (not all(self._images_received.values()) or not all(self.transformations.values())): rospy.sleep(0.005) # print warning every 2 seconds if one of the messages is still # missing... if start_time + rospy.Duration(2.0) < rospy.Time.now(): for name, v in self._images_received.iteritems(): if not v: print "--> still waiting for sample from %s" % name for name, v in self._transformations_received.iteritems(): if not v: print "--> still waiting for sample from %s" % name start_time = rospy.Time.now() # set up checkerboard and checkerboard detector checkerboard = Checkerboard(pattern_size, square_size) checkerboard_detector = CheckerboardDetector(checkerboard) # detect cb for camera, image in self._images.iteritems(): image = image["image"] cvImage = self.bridge.imgmsg_to_cv2(image, "mono8") #imagecv = cv2util.cvmat2np(cvImage) self._images[camera]['corners'] = None try: corners = checkerboard_detector.detect_image_points( cvImage, is_grayscale=True) except: rospy.logwarn("No calibration pattern found for: '%s'" % camera) else: print "cb found: %s" % camera #img_points = [] #for (x, y) in corners.reshape(-1, 2): # img_points.append(ImagePoint(x, y)) self._images[camera]['corners'] = corners return self._images
def test_detect_image_points_color_image(self): # create Checkerboard instance and read image c = Checkerboard((9, 6), 0.03) image = cv2.imread(CHECKERBOARD_IMAGE, 0) # create CheckerboardDetector instance and detect image points cd = CheckerboardDetector(c) points = cd.detect_image_points(image, True) #dump = {'points': points.flatten().tolist()} #yaml.dump(dump, open(CHECKERBOARD_IMAGE_EXPECTED_POINTS_YAML, "w")) # load expected points points_expected = yaml.load( file(CHECKERBOARD_IMAGE_EXPECTED_POINTS_YAML))['points'] self.assertTrue(np.allclose(points.flatten().tolist(), points_expected))
def __init__(self): ''' Initializes storage, gets parameters from parameter server and logs to rosinfo ''' self.bridge = CvBridge() # set up Checkerboard and CheckerboardDetector self.board = Checkerboard( (9, 6), 0.03) ### Take the parameters from the parameter server self.detector = CheckerboardDetector(self.board) rospy.init_node(NODE) self.counter = 0 # Get params from ros parameter server or use default self.cams = rospy.get_param("~cameras") self.camera = [self.cams["reference"]["topic"]] if self.cams.has_key("further"): rospy.loginfo("FURTHER cameras specified") for cam in self.cams["further"]: self.camera.append(cam["topic"]) else: rospy.logwarn("No FURTHER cameras specified") self.numCams = len(self.camera) # Init images self.image = [] self.image_received = [False] * self.numCams for id in range(self.numCams): self.image.append( Image() ) # wait for a message from each camera this ensures that all cameras are working # Subscribe to images self.imageSub = [] for id in range(self.numCams): self.imageSub.append( rospy.Subscriber(self.camera[id], Image, self._imageCallback, id)) # Wait for image messages for id in range(self.numCams): print "now checking for incoming frames on the %s topic" % self.camera[ id] # pdb.set_trace() rospy.wait_for_message(self.camera[id], Image, 5)
def test_calculate_object_pose(self): # create Checkerboard instance and read image c = Checkerboard((9, 6), 0.03) image = cv2.imread(CHECKERBOARD_IMAGE, 0) # create CheckerboardDetector instance and detect object pose # (only dummy matrices, does not correspond to real camera used) cd = CheckerboardDetector(c) camera_matrix = np.matrix([1200, 0, 600, 0, 1200, 600, 0, 0, 1], dtype=np.float32).reshape((3,3)) dist_coeffs = np.matrix([0, 0, 0, 0, 0], dtype=np.float32).reshape((1,5)) (rvec, tvec) = cd.calculate_object_pose(image, camera_matrix, dist_coeffs, True) rvec_expected = np.matrix([[0.99905897, 0.035207, -0.02533079], [-0.0208742, 0.90224111, 0.43072642], [ 0.03801906, -0.42979234, 0.90212699]]) tvec_expected = np.matrix([[-0.03181351], [-0.13593217], [ 0.7021291]]) self.assertTrue(np.allclose(rvec_expected, rvec)) self.assertTrue(np.allclose(tvec_expected, tvec))
def __init__(self): ''' Configures the calibration node Reads configuration from parameter server or uses default values ''' # get parameter from parameter server or set defaults self.pattern_size = rospy.get_param('~pattern_size', "9x6") self.square_size = rospy.get_param('~square_size', 0.03) self.alpha = rospy.get_param('~alpha', 0.0) self.verbose = rospy.get_param('~verbose', True) self.save_result = rospy.get_param('~save_result', False) # split pattern_size string into tuple, e.g '9x6' -> tuple(9,6) self.pattern_size = tuple((int(self.pattern_size.split( "x")[0]), int(self.pattern_size.split("x")[1]))) self.camera_info = CameraInfo() self.camera_info_received = False self.latest_image = Image() self.bridge = CvBridge() # set up Checkerboard and CheckerboardDetector self.board = Checkerboard(self.pattern_size, self.square_size) self.detector = CheckerboardDetector(self.board) self.sss = simple_script_server() topic_name = '/stereo/left/' # subscribe to /cam3d/rgb/camera_info for camera_matrix and distortion coefficients rospy.Subscriber(topic_name + 'camera_info', CameraInfo, self.__camera_info_callback__) # subscribe to /cam3d/rgb/image_raw for image data rospy.Subscriber( topic_name + 'image_color', Image, self.__image_raw_callback__) # wait until camera informations are recieved. start_time = rospy.Time.now() while not (self.camera_info_received or rospy.is_shutdown()): rospy.sleep(0.05) if start_time + rospy.Duration(2.0) < rospy.Time.now(): # print warning every 2 seconds if the message is stil missing print "--> still waiting for /cam3d/rgb/camera_info" start_time = rospy.Time.now() # convert camera matrix an distortion coefficients and store them camera_matrix = self.camera_info.K cm = np.asarray(camera_matrix) self.cm = np.reshape(cm, (3, 3)) dist_coeffs = self.camera_info.D self.dc = np.asarray(dist_coeffs) self.frame = self.camera_info.header.frame_id
def test_calculate_object_pose(self): # create Checkerboard instance and read image c = Checkerboard((9, 6), 0.03) image = cv2.imread(CHECKERBOARD_IMAGE, 0) # create CheckerboardDetector instance and detect object pose # (only dummy matrices, does not correspond to real camera used) cd = CheckerboardDetector(c) camera_matrix = np.matrix([1200, 0, 600, 0, 1200, 600, 0, 0, 1], dtype=np.float32).reshape((3, 3)) dist_coeffs = np.matrix([0, 0, 0, 0, 0], dtype=np.float32).reshape( (1, 5)) (rvec, tvec) = cd.calculate_object_pose(image, camera_matrix, dist_coeffs, True) rvec_expected = np.matrix([[0.99905897, 0.035207, -0.02533079], [-0.0208742, 0.90224111, 0.43072642], [0.03801906, -0.42979234, 0.90212699]]) tvec_expected = np.matrix([[-0.03181351], [-0.13593217], [0.7021291]]) self.assertTrue(np.allclose(rvec_expected, rvec)) self.assertTrue(np.allclose(tvec_expected, tvec))
def run_stereo_calibration(self): ''' Runs the calibration ''' # set up Checkerboard, CheckerboardDetector and MonoCalibrator board = Checkerboard(self.pattern_size, self.square_size) detector = CheckerboardDetector(board) calibrator = StereoCalibrator(board, detector, self.folder, self.image_prefix_l, self.image_prefix_r) # run calibration ((rms_l, rms_r, rms_stereo), camera_matrix_l, dist_coeffs_l, rectification_matrix_l, projection_matrix_l, camera_matrix_r, dist_coeffs_r, rectification_matrix_r, projection_matrix_r, (h, w), R, T) = calibrator.calibrate_stereo_camera(self.alpha) print "==> successfully calibrated, stereo reprojection RMS (in pixels):", rms_stereo # create CalibrationData object with results camera_info_l = CalibrationData(self.camera_name_l, self.frame_id_l, w, h) camera_info_l.camera_matrix = camera_matrix_l camera_info_l.rectification_matrix = rectification_matrix_l camera_info_l.projection_matrix = projection_matrix_l camera_info_l.distortion_coefficients = dist_coeffs_l camera_info_r = CalibrationData(self.camera_name_r, self.frame_id_r, w, h) camera_info_r.camera_matrix = camera_matrix_r camera_info_r.rectification_matrix = rectification_matrix_r camera_info_r.projection_matrix = projection_matrix_r camera_info_r.distortion_coefficients = dist_coeffs_r # save results camera_info_l.save_camera_yaml_file(self.output_file_l) print "==> saved left results to:", self.output_file_l camera_info_r.save_camera_yaml_file(self.output_file_r) print "==> saved right results to:", self.output_file_r # convert baseline (invert transfrom as T and R bring right frame into left # and we need transform from left to right for urdf!) M = np.matrix(np.vstack((np.hstack( (R, T)), [0.0, 0.0, 0.0, 1.0]))) # 4x4 homogeneous matrix M_inv = M.I T_inv = np.array(M_inv[:3, 3]).flatten().tolist() # T as list (x, y, z) R_inv = list(tf.transformations.euler_from_matrix( M_inv[:3, :3])) # convert R to (roll, pitch, yaw) # save baseline if (self.calibration_urdf_in != "" and self.calibration_urdf_out != ""): attributes2update = { self.baseline_prop_prefix + 'x': T_inv[0], self.baseline_prop_prefix + 'y': T_inv[1], self.baseline_prop_prefix + 'z': T_inv[2], self.baseline_prop_prefix + 'roll': R_inv[0], self.baseline_prop_prefix + 'pitch': R_inv[1], self.baseline_prop_prefix + 'yaw': R_inv[2] } urdf_updater = CalibrationUrdfUpdater(self.calibration_urdf_in, self.calibration_urdf_out, self.verbose) urdf_updater.update(attributes2update) print "==> updated baseline in:", self.calibration_urdf_out else: print "==> NOT saving baseline to urdf file! Parameters 'calibration_urdf_in' and/or 'calibration_urdf_out' are empty..." # verbose mode if self.verbose: print "--> results:" np.set_printoptions(suppress=1) print "left\n----" print "rms left monocular calibration:", rms_l print "camera matrix:\n", camera_matrix_l print "distortion coefficients:\n", dist_coeffs_l print "rectification matrix:\n", rectification_matrix_l print "projection matrix:\n", projection_matrix_l print print "right\n-----" print "rms right monocular calibration:", rms_r print "camera matrix:\n", camera_matrix_r print "distortion coefficients:\n", dist_coeffs_r print "rectification matrix:\n", rectification_matrix_r print "projection matrix:\n", projection_matrix_r print print "baseline (transform from left to right camera)\n--------" print "R (in rpy):\n", R_inv print "T (in xyz):\n", T_inv
class Cb_marker_publish(): def __init__(self): rospy.init_node(NODE) pattern = rospy.get_param('~calibration_pattern') self.cameras = rospy.get_param('~cameras') pattern_string = pattern["pattern_size"].split("x") pattern_size = (int(pattern_string[0]), int(pattern_string[1])) cb = Checkerboard(pattern_size, pattern["square_size"]) self.detector = CheckerboardDetector(cb) self.bridge = CvBridge() self.pub = rospy.Publisher("cb_detections", MarkerArray) self.topics = [self.cameras["reference"]["topic"]] for camera in self.cameras["further"]: self.topics.append(camera["topic"]) self.info_topics = [ t.replace("image_raw", "camera_info") for t in self.topics ] self.info_topics = [ t.replace("image_color", "camera_info") for t in self.info_topics ] print self.topics print self.info_topics self.images = dict.fromkeys(self.topics) self.camera_infos = dict.fromkeys(self.topics) self.colors = [ColorRGBA() for i in range(len(self.topics))] hue_values = np.linspace(0, 1, len(self.topics) + 1) for c, hue, topic in zip(self.colors, hue_values, self.topics): c.a = 1 (c.r, c.g, c.b) = hsv_to_rgb(hue, 1, 1) print "Listening to: %s - Color RGB: (%d, %d, %d)" % ( topic, c.r * 255, c.g * 255, c.b * 255) self.points = [] for point in cb.get_pattern_points(): p = Point() p.x = point[0] p.y = point[1] p.z = point[2] self.points.append(p) for topic, info_topic in zip(self.topics, self.info_topics): rospy.Subscriber(topic, Image, self.callback_image, topic) rospy.Subscriber(info_topic, CameraInfo, self.callback_info, topic) rospy.sleep(3) def callback_image(self, data, topic): self.images[topic] = data def callback_info(self, data, topic): self.camera_infos[topic] = data def get_pose(self, topic): image = self.images[topic] cvImage = self.bridge.imgmsg_to_cv(image, 'rgb8') img_raw = cv2util.cvmat2np(cvImage) camera_matrix = np.matrix( np.reshape(self.camera_infos[topic].K, (3, 3))) dist_coeffs = np.matrix(self.camera_infos[topic].D) return self.detector.calculate_object_pose(img_raw, camera_matrix, dist_coeffs, False) def run(self): rate = rospy.Rate(10) br = tf.TransformBroadcaster() print "Everything up and running" while not rospy.is_shutdown(): # put code here msgs = MarkerArray() for topic in self.topics: # pick color as defined before c = self.colors[self.topics.index(topic)] if self.images[topic] is None: continue try: # compute pose of detected pattern p = self.get_pose(topic) except CheckerboardDetector.NoPatternFoundException: rospy.logwarn("No pattern found on topic: '%s'" % topic) continue else: rmat = np.array(p[0]) rmat = np.append(rmat, [[0, 0, 0]], 0) rmat = np.append(rmat, [[0], [0], [0], [1]], 1) q = tf.transformations.quaternion_from_matrix(rmat) msg = Marker() msg.type = Marker.SPHERE_LIST msg.header.frame_id = self.camera_infos[ topic].header.frame_id msg.header.stamp = rospy.Time() msg.id = self.topics.index(topic) msg.pose.position.x = p[1][0] msg.pose.position.y = p[1][1] msg.pose.position.z = p[1][2] msg.pose.orientation.x = q[0] msg.pose.orientation.y = q[1] msg.pose.orientation.z = q[2] msg.pose.orientation.w = q[3] msg.scale.x = 0.01 msg.scale.y = 0.01 msg.scale.z = 0.01 msg.color = c msg.points = self.points msgs.markers.append(msg) br.sendTransform(p[1], q, rospy.Time.now(), "cb", msg.header.frame_id) self.pub.publish(msgs) rate.sleep()
class VisibilityCheckerNode(): ''' @summary: Captures Images from one or more cameras (Image message topics) to files. Number of cameras, output folder and file names are configurable via ROS parameters. After startup call "~capture_images" ROS service to save images of all cameras to output folder. ''' def __init__(self): ''' Initializes storage, gets parameters from parameter server and logs to rosinfo ''' self.bridge = CvBridge() # set up Checkerboard and CheckerboardDetector self.board = Checkerboard((9, 6), 0.03) self.detector = CheckerboardDetector(self.board) rospy.init_node(NODE) self.counter = 0 # Get params from ros parameter server or use default self.cams = rospy.get_param("~cameras") self.camera = [self.cams["reference"]["topic"]] for cam in self.cams["further"]: self.camera.append(cam["topic"]) self.numCams = len(self.camera) # Init images self.image = [] self.image_received = [False] * self.numCams for id in range(self.numCams): self.image.append(Image()) # Subscribe to images self.imageSub = [] for id in range(self.numCams): self.imageSub.append(rospy.Subscriber( self.camera[id], Image, self._imageCallback, id)) # Wait for image messages for id in range(self.numCams): rospy.wait_for_message(self.camera[id], Image, 5) def _imageCallback(self, data, id): ''' Copy image message to local storage @param data: Currently received image message @type data: ROS Image() message @param id: Id of camera from which the image was received @type id: integer ''' #print "cb executed" self.image[id] = data self.image_received[id] = True def _checkHandle(self, req): ''' Service handle for "Capture" Service Grabs images, converts them and saves them to files @param req: service request @type req: CaptureRequest() message @return: CaptureResponse() message ''' self.image_received = [False] * self.numCams visible = [] while not all(self.image_received): print "waiting for images" rospy.sleep(1) if all(self.image_received): print "=== ALL IMAGES RECEIVED ===" self.images_received = [False] * self.numCams for id in range(self.numCams): image = self.image[id] cvImage = self.bridge.imgmsg_to_cv(image, 'rgb8') img_raw = cv2util.cvmat2np(cvImage) visible.append(self.detector.detect_image_points( img_raw, False, True) is not None) # grab image messages #print '%s checkerboards found --> return %s'%(sum(visible),all(visible)) response = VisibleResponse() response.every = False response.master = False if all(visible): response.every = True response.master = True elif visible[0]: response.master = True return response def run(self): # Start service srv = rospy.Service( '/image_capture/visibility_check', Visible, self._checkHandle) rospy.loginfo("service '/image_capture/visibility_check' started, waiting for requests...") rospy.spin()
class VisibilityCheckerNode(): ''' @summary: Captures Images from one or more cameras (Image message topics) to files. Number of cameras, output folder and file names are configurable via ROS parameters. After startup call "~capture_images" ROS service to save images of all cameras to output folder. ''' def __init__(self): ''' Initializes storage, gets parameters from parameter server and logs to rosinfo ''' self.bridge = CvBridge() # set up Checkerboard and CheckerboardDetector self.board = Checkerboard((9, 6), 0.03) self.detector = CheckerboardDetector(self.board) rospy.init_node(NODE) self.counter = 0 # Get params from ros parameter server or use default self.cams = rospy.get_param("~cameras") self.camera = [self.cams["reference"]["topic"]] if self.cams.has_key("further"): rospy.loginfo("FURTHER cameras specified") for cam in self.cams["further"]: self.camera.append(cam["topic"]) else: rospy.logwarn("No FURTHER cameras specified") self.numCams = len(self.camera) # Init images self.image = [] self.image_received = [False] * self.numCams for id in range(self.numCams): self.image.append(Image()) # Subscribe to images self.imageSub = [] for id in range(self.numCams): self.imageSub.append(rospy.Subscriber( self.camera[id], Image, self._imageCallback, id)) # Wait for image messages for id in range(self.numCams): rospy.wait_for_message(self.camera[id], Image, 5) def _imageCallback(self, data, id): ''' Copy image message to local storage @param data: Currently received image message @type data: ROS Image() message @param id: Id of camera from which the image was received @type id: integer ''' #print "cb executed" self.image[id] = data self.image_received[id] = True def _checkHandle(self, req): ''' Service handle for "Capture" Service Grabs images, converts them and saves them to files @param req: service request @type req: CaptureRequest() message @return: CaptureResponse() message ''' self.image_received = [False] * self.numCams visible = [] while not all(self.image_received): print "waiting for images" rospy.sleep(1) if all(self.image_received): print "=== ALL IMAGES RECEIVED ===" self.images_received = [False] * self.numCams for id in range(self.numCams): image = self.image[id] cvImage = self.bridge.imgmsg_to_cv(image, 'rgb8') img_raw = cv2util.cvmat2np(cvImage) try: self.detector.detect_image_points( img_raw, False, True) visible.append(True) except self.detector.NoPatternFoundException: rospy.logwarn("No cb found") visible.append(False) # grab image messages #print '%s checkerboards found --> return %s'%(sum(visible),all(visible)) response = VisibleResponse() response.every = False response.master = False if all(visible): response.every = True response.master = True #elif visible[0]: # response.master = True return response def run(self): # Start service srv = rospy.Service( '/image_capture/visibility_check', Visible, self._checkHandle) rospy.loginfo("service '/image_capture/visibility_check' started, waiting for requests...") rospy.spin()
def _capture_and_pub(self, sample_id, target_id, chain_id, pattern_size, square_size): ''' Main capturing function. Gets a set of recent messages for all needed topics. Processes messages and creates RobotMeasuerment message which is published. @param sample_id: Sample identifier (e.g. sample01) @type sample_id: string @param target_id: Name of checkerboard (e.g. cb_9x6) @type target_id: string @param chain_id: Name of dh chain to which checkerboard is attached (e.g. arm_chain) @type chain_id: string @param pattern_size: Size of checkerboard pattern as defined by opencv (e.g. (9, 6)) @type pattern_size: tuple(x, y) @param square_size: Size of checkerboard sqaures (im m) @type square_size: float ''' # capture measurements # -------------------- self._left_received = False self._right_received = False self._kinect_rgb_received = False start_time = rospy.Time.now() while (not self._left_received or not self._right_received or not self._kinect_rgb_received): rospy.sleep(0.005) # print warning every 2 seconds if one of the messages is still missing... if start_time + rospy.Duration(2.0) < rospy.Time.now(): if not self._left_received: print "--> still waiting for sample from left" if not self._right_received: print "--> still waiting for sample from right" if not self._kinect_rgb_received: print "--> still waiting for sample from kinect" start_time = rospy.Time.now() latest_left = self._left latest_right = self._right latest_kinect_rgb = self._kinect_rgb self._torso_joint_msg_received = False self._arm_joint_msg_received = False start_time = rospy.Time.now() while (not self._torso_joint_msg_received or not self._arm_joint_msg_received): rospy.sleep(0.005) # print warning every 2 seconds if one of the messages is still missing... if start_time + rospy.Duration(2.0) < rospy.Time.now(): if not self._torso_joint_msg_received: print "--> still waiting for torso joint states" if not self._arm_joint_msg_received: print "--> still waiting for srm joint states" start_time = rospy.Time.now() latest_torso = self._torso_joint_msg latest_arm = self._arm_joint_msg # set up checkerboard and checkerboard detector # --------------------------------------------- checkerboard = Checkerboard(pattern_size, square_size) checkerboard_detector = CheckerboardDetector(checkerboard) # detect cb left # -------------- cvImage = self.bridge.imgmsg_to_cv(latest_left["image_rect"], "mono8") image = cv2util.cvmat2np(cvImage) corners = checkerboard_detector.detect_image_points(image, is_grayscale=True) if corners != None: print "cb found: left" img_points_left = [] for (x, y) in corners.reshape(-1, 2): img_points_left.append(ImagePoint(x, y)) else: # cb not found return False # create camera msg left # ---------------------- cam_msg_left = CameraMeasurement() cam_msg_left.camera_id = "left" cam_msg_left.header.stamp = latest_left["camera_info"].header.stamp cam_msg_left.cam_info = latest_left["camera_info"] cam_msg_left.image_points = img_points_left cam_msg_left.verbose = False #cam_ms_leftg.image = latest_left["image_color"] #cam_msg_left.image_rect = latest_left["image_rect"] #cam_msg_left.features = # Not implemented here # detect cb right # -------------- cvImage = self.bridge.imgmsg_to_cv(latest_right["image_rect"], "mono8") image = cv2util.cvmat2np(cvImage) corners = checkerboard_detector.detect_image_points(image, is_grayscale=True) if corners != None: print "cb found: right" img_points_right = [] for (x, y) in corners.reshape(-1, 2): img_points_right.append(ImagePoint(x, y)) else: # cb not found return False # create camera msg right # ----------------------- cam_msg_right = CameraMeasurement() cam_msg_right.camera_id = "right" cam_msg_right.header.stamp = latest_right["camera_info"].header.stamp cam_msg_right.cam_info = latest_right["camera_info"] cam_msg_right.image_points = img_points_right cam_msg_right.verbose = False #cam_msg_right.image = latest_right["image_color"] #cam_msg_right.image_rect = latest_right["image_rect"] #cam_msg_right.features = # Not implemented here # detect cb kinect_rgb # -------------------- cvImage = self.bridge.imgmsg_to_cv(latest_kinect_rgb["image_color"], "mono8") image = cv2util.cvmat2np(cvImage) corners = checkerboard_detector.detect_image_points(image, is_grayscale=True) if corners != None: print "cb found: kinect_rgb" img_points_kinect_rgb = [] for (x, y) in corners.reshape(-1, 2): img_points_kinect_rgb.append(ImagePoint(x, y)) else: # cb not found return False # create camera msg kinect_rgb # ---------------------------- cam_msg_kinect_rgb = CameraMeasurement() cam_msg_kinect_rgb.camera_id = "kinect_rgb" cam_msg_kinect_rgb.header.stamp = latest_kinect_rgb[ "camera_info"].header.stamp cam_msg_kinect_rgb.cam_info = latest_kinect_rgb["camera_info"] cam_msg_kinect_rgb.image_points = img_points_kinect_rgb cam_msg_kinect_rgb.verbose = False #cam_ms_kinect_rgbg.image = latest_kinect_rgb["image_color"] #cam_msg_kinect_rgb.image_rect = latest_kinect_rgb["image_rect"] #cam_msg_kinect_rgb.features = # Not implemented here # create torso_chain msg # ---------------------- torso_chain_msg = ChainMeasurement() torso_chain_msg.header = latest_torso.header torso_chain_msg.chain_id = "torso_chain" torso_chain_msg.chain_state = latest_torso # create arm_chain msg # -------------------- arm_chain_msg = ChainMeasurement() arm_chain_msg.header = latest_arm.header arm_chain_msg.chain_id = "arm_chain" arm_chain_msg.chain_state = latest_arm # DEBUG publish pic # ----------------- self._image_pub_left.publish(latest_left["image_color"]) self._image_pub_right.publish(latest_right["image_color"]) self._image_pub_kinect_rgb.publish(latest_kinect_rgb["image_color"]) # create robot measurement msg and publish # ----------------- robot_msg = RobotMeasurement() robot_msg.sample_id = sample_id robot_msg.target_id = target_id robot_msg.chain_id = chain_id robot_msg.M_cam = [cam_msg_left, cam_msg_right, cam_msg_kinect_rgb] robot_msg.M_chain = [torso_chain_msg, arm_chain_msg] self._robot_measurement_pub.publish(robot_msg) return True
class Detection(): def __init__(self): ''' Configures the calibration node Reads configuration from parameter server or uses default values ''' # get parameter from parameter server or set defaults self.pattern_size = rospy.get_param('~pattern_size', "9x6") self.square_size = rospy.get_param('~square_size', 0.03) self.alpha = rospy.get_param('~alpha', 0.0) self.verbose = rospy.get_param('~verbose', True) self.save_result = rospy.get_param('~save_result', False) # split pattern_size string into tuple, e.g '9x6' -> tuple(9,6) self.pattern_size = tuple((int(self.pattern_size.split("x")[0]), int(self.pattern_size.split("x")[1]))) self.camera_info = CameraInfo() self.camera_info_received = False self.latest_image = Image() self.bridge = CvBridge() # set up Checkerboard and CheckerboardDetector self.board = Checkerboard(self.pattern_size, self.square_size) self.detector = CheckerboardDetector(self.board) self.sss = simple_script_server() topic_name = '/stereo/left/' # subscribe to /cam3d/rgb/camera_info for camera_matrix and distortion coefficients rospy.Subscriber(topic_name + 'camera_info', CameraInfo, self.__camera_info_callback__) # subscribe to /cam3d/rgb/image_raw for image data rospy.Subscriber(topic_name + 'image_color', Image, self.__image_raw_callback__) # wait until camera informations are recieved. start_time = rospy.Time.now() while not (self.camera_info_received or rospy.is_shutdown()): rospy.sleep(0.05) if start_time + rospy.Duration(2.0) < rospy.Time.now(): # print warning every 2 seconds if the message is stil missing print "--> still waiting for /cam3d/rgb/camera_info" start_time = rospy.Time.now() # convert camera matrix an distortion coefficients and store them camera_matrix = self.camera_info.K cm = np.asarray(camera_matrix) self.cm = np.reshape(cm, (3, 3)) dist_coeffs = self.camera_info.D self.dc = np.asarray(dist_coeffs) self.frame = self.camera_info.header.frame_id # initialize torso for movement def __camera_info_callback__(self, data): ''' executed on new message in /cam3d/rgb/camera_info stores received data ''' self.camera_info = data self.camera_info_received = True def __image_raw_callback__(self, data): ''' executed on new message in /cam3d/rgb/image_raw stores newest image ''' self.latest_image = data def get_position(self): ''' returns the Y value in subpixel accuracy for the center of the recognized checkerboard ''' r = rospy.Rate(10) # Hz while rospy.Time.now( ) - self.latest_image.header.stamp > rospy.Duration( 1) and not rospy.is_shutdown(): print 'no image received' print rospy.Time.now(), self.latest_image.header.stamp r.sleep() cvImage = self.bridge.imgmsg_to_cv(self.latest_image, "mono8") image_raw = cv2util.cvmat2np(cvImage) #image_processed = cv2.undistort(image_raw, self.cm, self.dc) #points=self.detector.detect_image_points(image_processed,True) (rmat, tvec) = self.detector.calculate_object_pose(image_raw, self.cm, self.dc, True) #print tvec[1] return (rmat, tvec), self.latest_image.header.frame_id
def _capture_and_pub(self, sample_id, target_id, chain_id, pattern_size, square_size): ''' Main capturing function. Gets a set of recent messages for all needed topics. Processes messages and creates RobotMeasuerment message which is published. @param sample_id: Sample identifier (e.g. sample01) @type sample_id: string @param target_id: Name of checkerboard (e.g. cb_9x6) @type target_id: string @param chain_id: Name of dh chain to which checkerboard is attached (e.g. arm_chain) @type chain_id: string @param pattern_size: Size of checkerboard pattern as defined by opencv (e.g. (9, 6)) @type pattern_size: tuple(x, y) @param square_size: Size of checkerboard sqaures (im m) @type square_size: float ''' # capture measurements # -------------------- self._left_received = False self._right_received = False self._kinect_rgb_received = False start_time = rospy.Time.now() while (not self._left_received or not self._right_received or not self._kinect_rgb_received): rospy.sleep(0.005) # print warning every 2 seconds if one of the messages is still missing... if start_time + rospy.Duration(2.0) < rospy.Time.now(): if not self._left_received: print "--> still waiting for sample from left" if not self._right_received: print "--> still waiting for sample from right" if not self._kinect_rgb_received: print "--> still waiting for sample from kinect" start_time = rospy.Time.now() latest_left = self._left latest_right = self._right latest_kinect_rgb = self._kinect_rgb self._torso_joint_msg_received = False self._arm_joint_msg_received = False start_time = rospy.Time.now() while (not self._torso_joint_msg_received or not self._arm_joint_msg_received): rospy.sleep(0.005) # print warning every 2 seconds if one of the messages is still missing... if start_time + rospy.Duration(2.0) < rospy.Time.now(): if not self._torso_joint_msg_received: print "--> still waiting for torso joint states" if not self._arm_joint_msg_received: print "--> still waiting for srm joint states" start_time = rospy.Time.now() latest_torso = self._torso_joint_msg latest_arm = self._arm_joint_msg # set up checkerboard and checkerboard detector # --------------------------------------------- checkerboard = Checkerboard(pattern_size, square_size) checkerboard_detector = CheckerboardDetector(checkerboard) # detect cb left # -------------- cvImage = self.bridge.imgmsg_to_cv(latest_left["image_rect"], "mono8") image = cv2util.cvmat2np(cvImage) corners = checkerboard_detector.detect_image_points(image, is_grayscale=True) if corners != None: print "cb found: left" img_points_left = [] for (x, y) in corners.reshape(-1, 2): img_points_left.append(ImagePoint(x, y)) else: # cb not found return False # create camera msg left # ---------------------- cam_msg_left = CameraMeasurement() cam_msg_left.camera_id = "left" cam_msg_left.header.stamp = latest_left["camera_info"].header.stamp cam_msg_left.cam_info = latest_left["camera_info"] cam_msg_left.image_points = img_points_left cam_msg_left.verbose = False #cam_ms_leftg.image = latest_left["image_color"] #cam_msg_left.image_rect = latest_left["image_rect"] #cam_msg_left.features = # Not implemented here # detect cb right # -------------- cvImage = self.bridge.imgmsg_to_cv(latest_right["image_rect"], "mono8") image = cv2util.cvmat2np(cvImage) corners = checkerboard_detector.detect_image_points(image, is_grayscale=True) if corners != None: print "cb found: right" img_points_right = [] for (x, y) in corners.reshape(-1, 2): img_points_right.append(ImagePoint(x, y)) else: # cb not found return False # create camera msg right # ----------------------- cam_msg_right = CameraMeasurement() cam_msg_right.camera_id = "right" cam_msg_right.header.stamp = latest_right["camera_info"].header.stamp cam_msg_right.cam_info = latest_right["camera_info"] cam_msg_right.image_points = img_points_right cam_msg_right.verbose = False #cam_msg_right.image = latest_right["image_color"] #cam_msg_right.image_rect = latest_right["image_rect"] #cam_msg_right.features = # Not implemented here # detect cb kinect_rgb # -------------------- cvImage = self.bridge.imgmsg_to_cv(latest_kinect_rgb["image_color"], "mono8") image = cv2util.cvmat2np(cvImage) corners = checkerboard_detector.detect_image_points(image, is_grayscale=True) if corners != None: print "cb found: kinect_rgb" img_points_kinect_rgb = [] for (x, y) in corners.reshape(-1, 2): img_points_kinect_rgb.append(ImagePoint(x, y)) else: # cb not found return False # create camera msg kinect_rgb # ---------------------------- cam_msg_kinect_rgb = CameraMeasurement() cam_msg_kinect_rgb.camera_id = "kinect_rgb" cam_msg_kinect_rgb.header.stamp = latest_kinect_rgb["camera_info"].header.stamp cam_msg_kinect_rgb.cam_info = latest_kinect_rgb["camera_info"] cam_msg_kinect_rgb.image_points = img_points_kinect_rgb cam_msg_kinect_rgb.verbose = False #cam_ms_kinect_rgbg.image = latest_kinect_rgb["image_color"] #cam_msg_kinect_rgb.image_rect = latest_kinect_rgb["image_rect"] #cam_msg_kinect_rgb.features = # Not implemented here # create torso_chain msg # ---------------------- torso_chain_msg = ChainMeasurement() torso_chain_msg.header = latest_torso.header torso_chain_msg.chain_id = "torso_chain" torso_chain_msg.chain_state = latest_torso # create arm_chain msg # -------------------- arm_chain_msg = ChainMeasurement() arm_chain_msg.header = latest_arm.header arm_chain_msg.chain_id = "arm_chain" arm_chain_msg.chain_state = latest_arm # DEBUG publish pic # ----------------- self._image_pub_left.publish(latest_left["image_color"]) self._image_pub_right.publish(latest_right["image_color"]) self._image_pub_kinect_rgb.publish(latest_kinect_rgb["image_color"]) # create robot measurement msg and publish # ----------------- robot_msg = RobotMeasurement() robot_msg.sample_id = sample_id robot_msg.target_id = target_id robot_msg.chain_id = chain_id robot_msg.M_cam = [cam_msg_left, cam_msg_right, cam_msg_kinect_rgb] robot_msg.M_chain = [torso_chain_msg, arm_chain_msg] self._robot_measurement_pub.publish(robot_msg) return True
def _capture_and_pub(self, sample_id, target_id, chain_id, pattern_size, square_size): ''' Main capturing function. Gets a set of recent messages for all needed topics. Processes messages and creates RobotMeasuerment message which is published. @param sample_id: Sample identifier (e.g. sample01) @type sample_id: string @param target_id: Name of checkerboard (e.g. cb_9x6) @type target_id: string @param chain_id: Name of dh chain to which checkerboard is attached (e.g. arm_chain) @type chain_id: string @param pattern_size: Size of checkerboard pattern as defined by opencv (e.g. (9, 6)) @type pattern_size: tuple(x, y) @param square_size: Size of checkerboard sqaures (im m) @type square_size: float ''' # capture measurements # -------------------- self._left_received = False self._right_received = False self._kinect_rgb_received = False start_time = rospy.Time.now() while (not self._left_received): rospy.sleep(0.005) # print warning every 2 seconds if one of the messages is still missing... if start_time + rospy.Duration(2.0) < rospy.Time.now(): if not self._left_received: print "--> still waiting for sample from left" start_time = rospy.Time.now() print "got sample" latest_left = self._left self._torso_joint_msg_received = False self._arm_joint_msg_received = False # set up checkerboard and checkerboard detector # --------------------------------------------- checkerboard = Checkerboard(pattern_size, square_size) checkerboard_detector = CheckerboardDetector(checkerboard) # detect cb left # -------------- cvImage = self.bridge.imgmsg_to_cv(latest_left["image"], "mono8") image = cv2util.cvmat2np(cvImage) corners = checkerboard_detector.detect_image_points( image, is_grayscale=True) if corners is not None: print "cb found: left" img_points_left = [] for (x, y) in corners.reshape(-1, 2): img_points_left.append(ImagePoint(x, y)) else: # cb not found return False # create camera msg left # ---------------------- cam_msg_left = CameraMeasurement() cam_msg_left.camera_id = "left" cam_msg_left.header.stamp = latest_left["image"].header.stamp #cam_msg_left.cam_info = latest_left["camera_info"] cam_msg_left.image_points = img_points_left cam_msg_left.verbose = False #cam_ms_leftg.image = latest_left["image_color"] #cam_msg_left.image_rect = latest_left["image_rect"] #cam_msg_left.features = # Not implemented here # create chain msgs # ---------------------- transformations = [] print self.transformations for (key, links) in self.transformations.iteritems(): chain_msg = ChainMeasurement() chain_msg.chain_id = key while not rospy.is_shutdown(): try: ( trans, rot) = self.listener.lookupTransform(links[0], links[1], rospy.Time(0)) break except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): print "error looking up transformation from ", links[0], " to ", links[1] rospy.sleep(0.5) chain_msg.translation = trans chain_msg.rotation = rot transformations.append(chain_msg) # DEBUG publish pic # ----------------- self._image_pub_left.publish(latest_left["image"]) # create robot measurement msg and publish # ----------------- robot_msg = RobotMeasurement() robot_msg.sample_id = sample_id robot_msg.target_id = target_id robot_msg.chain_id = chain_id robot_msg.M_cam = [cam_msg_left] robot_msg.M_chain = transformations self._robot_measurement_pub.publish(robot_msg) return True
class Detection(): def __init__(self): ''' Configures the calibration node Reads configuration from parameter server or uses default values ''' # get parameter from parameter server or set defaults self.pattern_size = rospy.get_param('~pattern_size', "9x6") self.square_size = rospy.get_param('~square_size', 0.03) self.alpha = rospy.get_param('~alpha', 0.0) self.verbose = rospy.get_param('~verbose', True) self.save_result = rospy.get_param('~save_result', False) # split pattern_size string into tuple, e.g '9x6' -> tuple(9,6) self.pattern_size = tuple((int(self.pattern_size.split( "x")[0]), int(self.pattern_size.split("x")[1]))) self.camera_info = CameraInfo() self.camera_info_received = False self.latest_image = Image() self.bridge = CvBridge() # set up Checkerboard and CheckerboardDetector self.board = Checkerboard(self.pattern_size, self.square_size) self.detector = CheckerboardDetector(self.board) self.sss = simple_script_server() topic_name = '/stereo/left/' # subscribe to /cam3d/rgb/camera_info for camera_matrix and distortion coefficients rospy.Subscriber(topic_name + 'camera_info', CameraInfo, self.__camera_info_callback__) # subscribe to /cam3d/rgb/image_raw for image data rospy.Subscriber( topic_name + 'image_color', Image, self.__image_raw_callback__) # wait until camera informations are recieved. start_time = rospy.Time.now() while not (self.camera_info_received or rospy.is_shutdown()): rospy.sleep(0.05) if start_time + rospy.Duration(2.0) < rospy.Time.now(): # print warning every 2 seconds if the message is stil missing print "--> still waiting for /cam3d/rgb/camera_info" start_time = rospy.Time.now() # convert camera matrix an distortion coefficients and store them camera_matrix = self.camera_info.K cm = np.asarray(camera_matrix) self.cm = np.reshape(cm, (3, 3)) dist_coeffs = self.camera_info.D self.dc = np.asarray(dist_coeffs) self.frame = self.camera_info.header.frame_id # initialize torso for movement def __camera_info_callback__(self, data): ''' executed on new message in /cam3d/rgb/camera_info stores received data ''' self.camera_info = data self.camera_info_received = True def __image_raw_callback__(self, data): ''' executed on new message in /cam3d/rgb/image_raw stores newest image ''' self.latest_image = data def get_position(self): ''' returns the Y value in subpixel accuracy for the center of the recognized checkerboard ''' r = rospy.Rate(10) # Hz while rospy.Time.now() - self.latest_image.header.stamp > rospy.Duration(1) and not rospy.is_shutdown(): print 'no image received' print rospy.Time.now(), self.latest_image.header.stamp r.sleep() cvImage = self.bridge.imgmsg_to_cv(self.latest_image, "mono8") image_raw = cv2util.cvmat2np(cvImage) #image_processed = cv2.undistort(image_raw, self.cm, self.dc) #points=self.detector.detect_image_points(image_processed,True) (rmat, tvec) = self.detector.calculate_object_pose(image_raw, self.cm, self.dc, True) #print tvec[1] return (rmat, tvec), self.latest_image.header.frame_id
def run_stereo_calibration(self): ''' Runs the calibration ''' # set up Checkerboard, CheckerboardDetector and MonoCalibrator board = Checkerboard(self.pattern_size, self.pattern["square_size"]) detector = CheckerboardDetector(board) attributes2update = {} output = "" camera_ref_saved = False; for image_prefix_dep,\ camera_name_dep,\ frame_id_dep,\ output_file_dep,\ baseline_prop_prefix,\ is_part_of_stereo_system in zip(self.image_prefixes_dep, self.camera_names_dep, self.frame_ids_dep, self.output_files_dep, self.baseline_prop_prefixes_dep, self.is_part_of_stereo_system): if image_prefix_dep is not None: print "Calibrating %s: \n\t Frame: %s \n\t Output File: %s \n\t Baseline Prefix: %s"%(camera_name_dep,frame_id_dep,output_file_dep, baseline_prop_prefix) output += self.camera_name_ref +" -> " + camera_name_dep + ": \n" calibrator = StereoCalibrator(board, detector, self.folder, self.image_prefix_ref, image_prefix_dep) # run calibration for stereo pair ( (rms_ref, rms_dep, rms_stereo), camera_matrix_ref, dist_coeffs_ref, rectification_matrix_ref, projection_matrix_ref, camera_matrix_dep, dist_coeffs_dep, rectification_matrix_dep, projection_matrix_dep, ((h_ref, w_ref), (h_dep, w_dep)), R, T) = calibrator.calibrate_stereo_camera(self.alpha) output += "==> successfully calibrated, stereo reprojection RMS (in pixels): " output += str(rms_stereo) output += "\n\n" # create CalibrationData object with results camera_info_ref = CalibrationData( self.camera_name_ref, self.frame_id_ref, w_ref, h_ref) camera_info_ref.camera_matrix = camera_matrix_ref camera_info_ref.rectification_matrix = rectification_matrix_ref camera_info_ref.projection_matrix = projection_matrix_ref camera_info_ref.distortion_coefficients = dist_coeffs_ref camera_info_dep = CalibrationData( camera_name_dep, frame_id_dep, w_dep, h_dep) camera_info_dep.camera_matrix = camera_matrix_dep camera_info_dep.rectification_matrix = rectification_matrix_dep camera_info_dep.projection_matrix = projection_matrix_dep camera_info_dep.distortion_coefficients = dist_coeffs_dep if output_file_dep is not None: if not is_part_of_stereo_system: # TODO: compute new projectionmatrix (projection_matrix, _) = cv2.getOptimalNewCameraMatrix( camera_matrix_dep, dist_coeffs_dep, (w_dep, h_dep),0 ) camera_info_dep.projection_matrix = np.array(np.hstack(( projection_matrix, np.matrix([0, 0, 0]).reshape(3, 1)))) camera_info_dep.rectification_matrix=[1,0,0, 0,1,0, 0,0,1] # save results if not camera_ref_saved: camera_info_ref.save_camera_yaml_file(self.output_file_ref) print "==> saved left results to:", self.output_file_ref camera_ref_saved = True #output += "==> saved left results to:", self.output_file_ref camera_info_dep.save_camera_yaml_file(output_file_dep) print "==> saved " + camera_name_dep + " results to:", output_file_dep #output += "==> saved " + camera_name_dep + " results to:", output_file_dep # convert baseline (invert transfrom as T and R bring right frame into left # and we need transform from left to right for urdf!) M = np.matrix(np.vstack((np.hstack( (R, T)), [0.0, 0.0, 0.0, 1.0]))) # 4x4 homogeneous matrix # TODO: tests with real hardware samples # compute transformation from reference to parent frame of camera # k_u = kinematic_utils() # k_u.get_parent(frame_id_dep) # M_parent = np.matrix(k_u.get_tf_to_parent(self.frame_id_ref)) # # resulting transformation M_inv = M.I # M_result = M_inv * M_parent # # urdf specifies origin -> inverse # M_inv = M_result.I T_inv = np.array( M_inv[:3, 3]).flatten().tolist() # T as list (x, y, z) R_inv = list(tf.transformations.euler_from_matrix( M_inv[:3, :3])) # convert R to (roll, pitch, yaw) baseline_prop_prefix = 'offset_' + baseline_prop_prefix attributes2update[baseline_prop_prefix + 'x'] = T_inv[0] attributes2update[baseline_prop_prefix + 'y'] = T_inv[1] attributes2update[baseline_prop_prefix + 'z'] = T_inv[2] attributes2update[baseline_prop_prefix + 'roll'] = R_inv[0] attributes2update[baseline_prop_prefix + 'pitch'] = R_inv[1] attributes2update[baseline_prop_prefix + 'yaw'] = R_inv[2] #verbose mode if self.verbose: print "--> results:" np.set_printoptions(suppress=1) print "left\n----" print "rms left monocular calibration:", rms_ref print "camera matrix:\n", camera_matrix_ref print "distortion coefficients:\n", dist_coeffs_ref print "rectification matrix:\n", rectification_matrix_ref print "projection matrix:\n", projection_matrix_ref print print "right\n-----" print "rms right monocular calibration:", rms_dep print "camera matrix:\n", camera_matrix_dep print "distortion coefficients:\n", dist_coeffs_dep print "rectification matrix:\n", rectification_matrix_dep print "projection matrix:\n", projection_matrix_dep print print "baseline (transform from left to right camera)\n--------" print "R (in rpy):\n", R_inv print "T (in xyz):\n", T_inv # save baseline if (self.calibration_offset_urdf != "" and self.calibration_default_urdf != ""): for p in attributes2update.iteritems(): print "%s: %s"%p urdf_updater = CalibrationUrdfUpdater(self.calibration_offset_urdf, self.calibration_offset_urdf, debug = self.verbose, urdf_default=self.calibration_default_urdf) urdf_updater.update(attributes2update) print "==> updated baseline in:", self.calibration_offset_urdf else: print "==> NOT saving baseline to urdf file! Parameters 'calibration_urdf_in' and/or 'calibration_urdf_out' are empty..." print output
class TorsoCalibration(): def __init__(self): ''' Configures the calibration node Reads configuration from parameter server or uses default values ''' rospy.init_node(NODE) print "==> started " + NODE # get parameter from parameter server or set defaults self.pattern_size = rospy.get_param('~pattern_size', "9x6") self.square_size = rospy.get_param('~square_size', 0.03) self.alpha = rospy.get_param('~alpha', 0.0) self.verbose = rospy.get_param('~verbose', True) self.save_result = rospy.get_param('~save_result', False) # split pattern_size string into tuple, e.g '9x6' -> tuple(9,6) self.pattern_size = tuple((int(self.pattern_size.split("x")[0]), int(self.pattern_size.split("x")[1]))) self.camera_info = CameraInfo() self.camera_info_received = False self.latest_image = Image() self.bridge = CvBridge() # set up Checkerboard and CheckerboardDetector self.board = Checkerboard(self.pattern_size, self.square_size) self.detector = CheckerboardDetector(self.board) self.sss = simple_script_server() self.ts = TorsoState() def __camera_info_callback__(self, data): ''' executed on new message in /cam3d/rgb/camera_info stores received data ''' self.camera_info = data self.camera_info_received = True def __image_raw_callback__(self, data): ''' executed on new message in /cam3d/rgb/image_raw stores newest image ''' self.latest_image = data def run(self): ''' Runs the calibration ''' print "==> starting torso pitch/height calibration" # subscribe to /cam3d/rgb/camera_info for camera_matrix and distortion coefficients rospy.Subscriber('/cam3d/rgb/camera_info', CameraInfo, self.__camera_info_callback__) # subscribe to /cam3d/rgb/image_raw for image data rospy.Subscriber('/cam3d/rgb/image_color', Image, self.__image_raw_callback__) # wait until camera informations are recieved. start_time = rospy.Time.now() while not (self.camera_info_received or rospy.is_shutdown()): rospy.sleep(0.05) if start_time + rospy.Duration(2.0) < rospy.Time.now(): # print warning every 2 seconds if the message is stil missing print "--> still waiting for /cam3d/rgb/camera_info" start_time = rospy.Time.now() # convert camera matrix an distortion coefficients and store them camera_matrix = self.camera_info.K cm = np.asarray(camera_matrix) self.cm = np.reshape(cm, (3, 3)) dist_coeffs = self.camera_info.D self.dc = np.asarray(dist_coeffs) # initialize torso for movement self.sss.init("torso") # get initial state of Torso (Upper Tilt Joint and Head Axis Joint horizontal) state = list(self.ts.calc_references()) print state # start minimization for getting the upright position of the torso start_time = rospy.Time.now() solution = self.minimize_yvalue(state) if self.verbose: print 'Elapsed time: ', (rospy.Time.now() - start_time).to_sec() print 'Upright forward Position found at %s' % solution print 'Seed was ', state if self.save_result: system( 'roslaunch cob_torso_calibration update_torso_calibration_urdf.launch' ) # def get_position(self): ''' returns the Y value in subpixel accuracy for the center of the recognized checkerboard ''' r = rospy.Rate(10) # Hz while rospy.Time.now( ) - self.latest_image.header.stamp > rospy.Duration( 1) and not rospy.is_shutdown(): print 'no image received' print rospy.Time.now(), self.latest_image.header.stamp r.sleep() cvImage = self.bridge.imgmsg_to_cv(self.latest_image, "mono8") image_raw = cv2util.cvmat2np(cvImage) #image_processed = cv2.undistort(image_raw, self.cm, self.dc) #points=self.detector.detect_image_points(image_processed,True) (rmat, tvec, image_points) = self.detector.calculate_object_pose_ransac( image_raw, self.cm, self.dc, True) #print tvec[1] return tvec[0][0], tvec[1][0], tvec[2][0] def get_average_position(self, nsec, frequency=15): ''' returns the average Y value for the last nsec seconds @param nsec: time for samples to be taken @type nsec: integer @param frequency: update frequency for image topic @type frequency: integer ''' R = rospy.Rate(frequency) position_list = [] for i in range(frequency * nsec): position_list.append(list(self.get_position())) R.sleep() #if self.verbose: #prettyprint(position_list) #print np.average(position_list,0) return np.average(position_list, 0) def minimize_yvalue(self, initial_state, eps=0.005): ''' Executive function. Calculates and applies forward rotation. Moves torso until the upright position is found @param initial_state: initial joint state of torso (usually "Home" position) with leveled head joint @type initial_state: list [joint state lower neck, joint state tilt, joint state upper neck] @param eps: minimum accuracy for upright position @type eps: float ''' initial_state = np.asarray(initial_state) states = [] step = 0.1 step_factor = [1, -1] state = initial_state self.sss.move("torso", [state.tolist()]) rospy.sleep(3) # calculate rotational reference and rotate to center states.append([self.get_average_position(10)[1], state]) while step > eps: for factor in step_factor: state = initial_state + np.asarray( [factor * step, 0, factor * -step]) self.sss.move("torso", [state.tolist()]) rospy.sleep(2.5) states.append([self.get_average_position(10)[1], state]) if self.verbose: prettyprint(states) states = sorted(states) prettyprint(states) step /= 2 states = [states[0]] initial_state = states[0][1] # initial state for next iteration if self.verbose: print '*' * 20 print(states) print initial_state self.sss.move("torso", [state.tolist()]) rospy.sleep(2) for i in range(3): position = self.get_average_position(10) rotational_error = math.atan(position[0] / position[2]) state -= np.asarray([0, rotational_error, 0]) self.sss.move("torso", [state.tolist()]) return state
def _capture_and_pub(self, sample_id, target_id, chain_id, pattern_size, square_size): ''' Main capturing function. Gets a set of recent messages for all needed topics. Processes messages and creates RobotMeasuerment message which is published. @param sample_id: Sample identifier (e.g. sample01) @type sample_id: string @param target_id: Name of checkerboard (e.g. cb_9x6) @type target_id: string @param chain_id: Name of dh chain to which checkerboard is attached (e.g. arm_chain) @type chain_id: string @param pattern_size: Size of checkerboard pattern as defined by opencv (e.g. (9, 6)) @type pattern_size: tuple(x, y) @param square_size: Size of checkerboard sqaures (im m) @type square_size: float ''' # capture measurements # -------------------- # create robot measurement msg and publish # ----------------- robot_msg = RobotMeasurement() robot_msg.sample_id = sample_id robot_msg.target_id = target_id robot_msg.chain_id = chain_id # -------------------- # receive images # ----------------- for v in self._images_received: self._images_received[v] = False for v in self.transformations_received: self.transformations_received[v] = False print self._images_received start_time = rospy.Time.now() while (not all(self._images_received.values()) or not all(self.transformations.values())): rospy.sleep(0.005) # print warning every 2 seconds if one of the messages is still # missing... if start_time + rospy.Duration(2.0) < rospy.Time.now(): for name, v in self._images_received.iteritems(): if not v: print "--> still waiting for sample from %s" % name for name, v in self._transformations_received.iteritems(): if not v: print "--> still waiting for sample from %s" % name start_time = rospy.Time.now() print "got sample" #latest_left = self._left # set up checkerboard and checkerboard detector # --------------------------------------------- checkerboard = Checkerboard(pattern_size, square_size) checkerboard_detector = CheckerboardDetector(checkerboard) # detect cb # -------------- for name, image in self._images.iteritems(): image = image["image"] #print image.header cvImage = self.bridge.imgmsg_to_cv(image, "mono8") imagecv = cv2util.cvmat2np(cvImage) try: corners = checkerboard_detector.detect_image_points( imagecv, is_grayscale=True) except: # cb not found rospy.logwarn("No calibration pattern found for: '%s'" % name) return False else: print "cb found: %s" % name img_points = [] for (x, y) in corners.reshape(-1, 2): img_points.append(ImagePoint(x, y)) # create camera msg left # ---------------------- cam_msg = CameraMeasurement() cam_msg.camera_id = name #print "crash before" cam_msg.header.stamp = image.header.stamp #print "crash after" #cam_msg.cam_info = image["camera_info"] cam_msg.image_points = img_points cam_msg.verbose = False robot_msg.M_cam.append(cam_msg) cam_msg.image = image # print cam_msg.camera_id #print cam_msg.header # cam_msg.image_rect = latest_left["image_rect"] # cam_msg.features = # Not implemented here #---------------------- #DEBUG publish pic #----------------- #self._image_pub_left.publish(latest_left["image"]) #---------------------- # Fill robot_msg #---------------------- robot_msg.M_chain = self.transformations.values() self._robot_measurement_pub.publish(robot_msg) return True
class TorsoCalibration(): def __init__(self): ''' Configures the calibration node Reads configuration from parameter server or uses default values ''' rospy.init_node(NODE) print "==> started " + NODE # get parameter from parameter server or set defaults self.pattern_size = rospy.get_param('~pattern_size', "9x6") self.square_size = rospy.get_param('~square_size', 0.03) self.alpha = rospy.get_param('~alpha', 0.0) self.verbose = rospy.get_param('~verbose', True) self.save_result = rospy.get_param('~save_result', False) # split pattern_size string into tuple, e.g '9x6' -> tuple(9,6) self.pattern_size = tuple((int(self.pattern_size.split( "x")[0]), int(self.pattern_size.split("x")[1]))) self.camera_info = CameraInfo() self.camera_info_received = False self.latest_image = Image() self.bridge = CvBridge() # set up Checkerboard and CheckerboardDetector self.board = Checkerboard(self.pattern_size, self.square_size) self.detector = CheckerboardDetector(self.board) self.sss = simple_script_server() self.ts = TorsoState() def __camera_info_callback__(self, data): ''' executed on new message in /cam3d/rgb/camera_info stores received data ''' self.camera_info = data self.camera_info_received = True def __image_raw_callback__(self, data): ''' executed on new message in /cam3d/rgb/image_raw stores newest image ''' self.latest_image = data def run(self): ''' Runs the calibration ''' print "==> starting torso pitch/height calibration" # subscribe to /cam3d/rgb/camera_info for camera_matrix and distortion coefficients rospy.Subscriber('/cam3d/rgb/camera_info', CameraInfo, self.__camera_info_callback__) # subscribe to /cam3d/rgb/image_raw for image data rospy.Subscriber( '/cam3d/rgb/image_color', Image, self.__image_raw_callback__) # wait until camera informations are recieved. start_time = rospy.Time.now() while not (self.camera_info_received or rospy.is_shutdown()): rospy.sleep(0.05) if start_time + rospy.Duration(2.0) < rospy.Time.now(): # print warning every 2 seconds if the message is stil missing print "--> still waiting for /cam3d/rgb/camera_info" start_time = rospy.Time.now() # convert camera matrix an distortion coefficients and store them camera_matrix = self.camera_info.K cm = np.asarray(camera_matrix) self.cm = np.reshape(cm, (3, 3)) dist_coeffs = self.camera_info.D self.dc = np.asarray(dist_coeffs) # initialize torso for movement self.sss.init("torso") # get initial state of Torso (Upper Tilt Joint and Head Axis Joint horizontal) state = list(self.ts.calc_references()) print state # start minimization for getting the upright position of the torso start_time = rospy.Time.now() solution = self.minimize_yvalue(state) if self.verbose: print 'Elapsed time: ', (rospy.Time.now() - start_time).to_sec() print 'Upright forward Position found at %s' % solution print 'Seed was ', state if self.save_result: system('roslaunch cob_torso_calibration update_torso_calibration_urdf.launch') # def get_position(self): ''' returns the Y value in subpixel accuracy for the center of the recognized checkerboard ''' r = rospy.Rate(10) # Hz while rospy.Time.now() - self.latest_image.header.stamp > rospy.Duration(1) and not rospy.is_shutdown(): print 'no image received' print rospy.Time.now(), self.latest_image.header.stamp r.sleep() cvImage = self.bridge.imgmsg_to_cv(self.latest_image, "mono8") image_raw = cv2util.cvmat2np(cvImage) #image_processed = cv2.undistort(image_raw, self.cm, self.dc) #points=self.detector.detect_image_points(image_processed,True) (rmat, tvec, image_points) = self.detector.calculate_object_pose_ransac(image_raw, self.cm, self.dc, True) #print tvec[1] return tvec[0][0], tvec[1][0], tvec[2][0] def get_average_position(self, nsec, frequency=15): ''' returns the average Y value for the last nsec seconds @param nsec: time for samples to be taken @type nsec: integer @param frequency: update frequency for image topic @type frequency: integer ''' R = rospy.Rate(frequency) position_list = [] for i in range(frequency * nsec): position_list.append(list(self.get_position())) R.sleep() #if self.verbose: #prettyprint(position_list) #print np.average(position_list,0) return np.average(position_list, 0) def minimize_yvalue(self, initial_state, eps=0.005): ''' Executive function. Calculates and applies forward rotation. Moves torso until the upright position is found @param initial_state: initial joint state of torso (usually "Home" position) with leveled head joint @type initial_state: list [joint state lower neck, joint state tilt, joint state upper neck] @param eps: minimum accuracy for upright position @type eps: float ''' initial_state = np.asarray(initial_state) states = [] step = 0.1 step_factor = [1, -1] state = initial_state self.sss.move("torso", [state.tolist()]) rospy.sleep(3) # calculate rotational reference and rotate to center states.append([self.get_average_position(10)[1], state]) while step > eps: for factor in step_factor: state = initial_state + np.asarray( [factor * step, 0, factor * -step]) self.sss.move("torso", [state.tolist()]) rospy.sleep(2.5) states.append([self.get_average_position(10)[1], state]) if self.verbose: prettyprint(states) states = sorted(states) prettyprint(states) step /= 2 states = [states[0]] initial_state = states[0][1] # initial state for next iteration if self.verbose: print '*' * 20 print(states) print initial_state self.sss.move("torso", [state.tolist()]) rospy.sleep(2) for i in range(3): position = self.get_average_position(10) rotational_error = math.atan(position[0] / position[2]) state -= np.asarray([0, rotational_error, 0]) self.sss.move("torso", [state.tolist()]) return state