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