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