def test_checkerboard_object_points_4x4(self): c = Checkerboard((4,4), 1) points = c.get_pattern_points() points_ = [[0, 0, 0], [1, 0, 0], [2, 0, 0], [3, 0, 0], [0, 1, 0], [1, 1, 0], [2, 1, 0], [3, 1, 0], [0, 2, 0], [1, 2, 0], [2, 2, 0], [3, 2, 0], [0, 3, 0], [1, 3, 0], [2, 3, 0], [3, 3, 0]] self.assertTrue(np.allclose(points, points_))
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 test_checkerboard_object_points_4x4(self): c = Checkerboard((4, 4), 1) points = c.get_pattern_points() points_ = [ [0, 0, 0], [1, 0, 0], [2, 0, 0], [3, 0, 0], [0, 1, 0], [1, 1, 0], [2, 1, 0], [3, 1, 0], [0, 2, 0], [1, 2, 0], [2, 2, 0], [3, 2, 0], [0, 3, 0], [1, 3, 0], [2, 3, 0], [3, 3, 0], ] self.assertTrue(np.allclose(points, points_))
def test_checkerboard_object_points_2x2_scale(self): c = Checkerboard((2, 2), 1.5) points = c.get_pattern_points() points_ = [[0, 0, 0], [1.5, 0, 0], [0, 1.5, 0], [1.5, 1.5, 0]] self.assertTrue(np.allclose(points, points_))
def test_checkerboard_object_points_2x2_scale(self): c = Checkerboard((2,2), 1.5) points = c.get_pattern_points() points_ = [[0, 0, 0], [1.5, 0, 0], [0, 1.5, 0], [1.5, 1.5, 0]] self.assertTrue(np.allclose(points, points_))