def test_stereo(self):
lmsg = sensor_msgs.msg.CameraInfo()
rmsg = sensor_msgs.msg.CameraInfo()
for m in (lmsg, rmsg):
m.width = 640
m.height = 480
# These parameters taken from a real camera calibration
lmsg.D = [-0.363528858080088, 0.16117037733986861, -8.1109585007538829e-05, -0.00044776712298447841, 0.0]
lmsg.K = [430.15433020105519, 0.0, 311.71339830549732, 0.0, 430.60920415473657, 221.06824942698509, 0.0, 0.0, 1.0]
lmsg.R = [0.99806560714807102, 0.0068562422224214027, 0.061790256276695904, -0.0067522959054715113, 0.99997541519165112, -0.0018909025066874664, -0.061801701660692349, 0.0014700186639396652, 0.99808736527268516]
lmsg.P = [295.53402059708782, 0.0, 285.55760765075684, 0.0, 0.0, 295.53402059708782, 223.29617881774902, 0.0, 0.0, 0.0, 1.0, 0.0]
rmsg.D = [-0.3560641041112021, 0.15647260261553159, -0.00016442960757099968, -0.00093175810713916221]
rmsg.K = [428.38163131344191, 0.0, 327.95553847249192, 0.0, 428.85728580588329, 217.54828640915309, 0.0, 0.0, 1.0]
rmsg.R = [0.9982082576219119, 0.0067433328293516528, 0.059454199832973849, -0.0068433268864187356, 0.99997549128605434, 0.0014784127772287513, -0.059442773257581252, -0.0018826283666309878, 0.99822993965212292]
rmsg.P = [295.53402059708782, 0.0, 285.55760765075684, -26.507895206214123, 0.0, 295.53402059708782, 223.29617881774902, 0.0, 0.0, 0.0, 1.0, 0.0]
cam = StereoCameraModel()
cam.fromCameraInfo(lmsg, rmsg)
for x in (16, 320, m.width - 16):
for y in (16, 240, m.height - 16):
for d in range(1, 10):
pt3d = cam.projectPixelTo3d((x, y), d)
((lx, ly), (rx, ry)) = cam.project3dToPixel(pt3d)
self.assertAlmostEqual(y, ly, 3)
self.assertAlmostEqual(y, ry, 3)
self.assertAlmostEqual(x, lx, 3)
self.assertAlmostEqual(x, rx + d, 3)
u = 100.0
v = 200.0
du = 17.0
dv = 23.0
Z = 2.0
xyz0 = cam.left.projectPixelTo3dRay((u, v))
xyz0 = (xyz0[0] * (Z / xyz0[2]), xyz0[1] * (Z / xyz0[2]), Z)
xyz1 = cam.left.projectPixelTo3dRay((u + du, v + dv))
xyz1 = (xyz1[0] * (Z / xyz1[2]), xyz1[1] * (Z / xyz1[2]), Z)
self.assertAlmostEqual(cam.left.getDeltaU(xyz1[0] - xyz0[0], Z), du, 3)
self.assertAlmostEqual(cam.left.getDeltaV(xyz1[1] - xyz0[1], Z), dv, 3)
self.assertAlmostEqual(cam.left.getDeltaX(du, Z), xyz1[0] - xyz0[0], 3)
self.assertAlmostEqual(cam.left.getDeltaY(dv, Z), xyz1[1] - xyz0[1], 3)
def test_stereo(self):
lmsg = sensor_msgs.msg.CameraInfo()
rmsg = sensor_msgs.msg.CameraInfo()
for m in (lmsg, rmsg):
m.width = 640
m.height = 480
# These parameters taken from a real camera calibration
lmsg.D = [-0.363528858080088, 0.16117037733986861, -8.1109585007538829e-05, -0.00044776712298447841, 0.0]
lmsg.K = [430.15433020105519, 0.0, 311.71339830549732, 0.0, 430.60920415473657, 221.06824942698509, 0.0, 0.0, 1.0]
lmsg.R = [0.99806560714807102, 0.0068562422224214027, 0.061790256276695904, -0.0067522959054715113, 0.99997541519165112, -0.0018909025066874664, -0.061801701660692349, 0.0014700186639396652, 0.99808736527268516]
lmsg.P = [295.53402059708782, 0.0, 285.55760765075684, 0.0, 0.0, 295.53402059708782, 223.29617881774902, 0.0, 0.0, 0.0, 1.0, 0.0]
rmsg.D = [-0.3560641041112021, 0.15647260261553159, -0.00016442960757099968, -0.00093175810713916221]
rmsg.K = [428.38163131344191, 0.0, 327.95553847249192, 0.0, 428.85728580588329, 217.54828640915309, 0.0, 0.0, 1.0]
rmsg.R = [0.9982082576219119, 0.0067433328293516528, 0.059454199832973849, -0.0068433268864187356, 0.99997549128605434, 0.0014784127772287513, -0.059442773257581252, -0.0018826283666309878, 0.99822993965212292]
rmsg.P = [295.53402059708782, 0.0, 285.55760765075684, -26.507895206214123, 0.0, 295.53402059708782, 223.29617881774902, 0.0, 0.0, 0.0, 1.0, 0.0]
cam = StereoCameraModel()
cam.fromCameraInfo(lmsg, rmsg)
for x in (16, 320, m.width - 16):
for y in (16, 240, m.height - 16):
for d in range(1, 10):
pt3d = cam.projectPixelTo3d((x, y), d)
((lx, ly), (rx, ry)) = cam.project3dToPixel(pt3d)
self.assertAlmostEqual(y, ly, 3)
self.assertAlmostEqual(y, ry, 3)
self.assertAlmostEqual(x, lx, 3)
self.assertAlmostEqual(x, rx + d, 3)
u = 100.0
v = 200.0
du = 17.0
dv = 23.0
Z = 2.0
xyz0 = cam.left.projectPixelTo3dRay((u, v))
xyz0 = (xyz0[0] * (Z / xyz0[2]), xyz0[1] * (Z / xyz0[2]), Z)
xyz1 = cam.left.projectPixelTo3dRay((u + du, v + dv))
xyz1 = (xyz1[0] * (Z / xyz1[2]), xyz1[1] * (Z / xyz1[2]), Z)
self.assertAlmostEqual(cam.left.getDeltaU(xyz1[0] - xyz0[0], Z), du, 3)
self.assertAlmostEqual(cam.left.getDeltaV(xyz1[1] - xyz0[1], Z), dv, 3)
self.assertAlmostEqual(cam.left.getDeltaX(du, Z), xyz1[0] - xyz0[0], 3)
self.assertAlmostEqual(cam.left.getDeltaY(dv, Z), xyz1[1] - xyz0[1], 3)
class Curiosity():
def __init__(self):
saliency_map_sub = rospy.Subscriber("/saliency_map",
Float32MultiArray,
self.saliency_map_callback,
queue_size=1,
buff_size=2**24)
camera_info_left_sub = rospy.Subscriber("/camera_left/camera_info",
CameraInfo,
self.camera_info_left_callback,
queue_size=1,
buff_size=2**24)
camera_info_right_sub = rospy.Subscriber(
"/camera_right/camera_info",
CameraInfo,
self.camera_info_right_callback,
queue_size=1,
buff_size=2**24)
point_sub = rospy.Subscriber("/saccade_point",
PointStamped,
self.saccade_point_callback,
queue_size=1,
buff_size=2**24)
disparity_sub = rospy.Subscriber("/camera/disparity",
DisparityImage,
self.disparity_callback,
queue_size=1,
buff_size=2**24)
self.saliency_map_curiosity_pub = rospy.Publisher(
"/saliency_map_curiosity", Float32MultiArray, queue_size=1)
self.saliency_map_curiosity_image_pub = rospy.Publisher(
"/saliency_map_curiosity_image", Image, queue_size=1)
self.transform_proxy = rospy.ServiceProxy("/transform", Transform)
self.camera_info_left = None
self.camera_info_right = None
self.disparity_image = None
self.camera_model = StereoCameraModel()
self.targets = []
self.cv_bridge = CvBridge()
self.saliency_width = float(rospy.get_param('~saliency_width', '256'))
self.saliency_height = float(rospy.get_param('~saliency_height',
'192'))
self.min_disparity = rospy.get_param(
"/camera/stereo_image_proc/min_disparity", "-16")
self.tfBuffer = tf2_ros.Buffer(rospy.Duration(30))
self.listener = tf2_ros.TransformListener(self.tfBuffer)
def saliency_map_callback(self, saliency_map):
if self.camera_info_left is not None and self.camera_info_right is not None and self.disparity_image is not None:
# handle input
lo = saliency_map.layout
saliency_map = np.asarray(
saliency_map.data[lo.data_offset:]).reshape(
lo.dim[0].size, lo.dim[1].size)
# modify saliency map
self.camera_model.fromCameraInfo(self.camera_info_left,
self.camera_info_right)
disparity_image = self.cv_bridge.imgmsg_to_cv2(
self.disparity_image.image)
for point in self.targets:
point_new = geometry_msgs.msg.PointStamped()
point_new.header = point.header
point_new.point = point.point
transformed = self.transform_proxy(point_new).res
transformed_new = tf2_geometry_msgs.PointStamped()
transformed_new.header = transformed.header
transformed_new.point = transformed.point
transformed = transformed_new
point_torso = (-transformed.point.y, -transformed.point.z,
transformed.point.x)
pixel = self.camera_model.project3dToPixel(point_torso)
x = int(
pixel[0][0] *
(self.saliency_width / float(self.camera_info_left.width)))
y = int(pixel[0][1] * (self.saliency_height /
float(self.camera_info_left.height)))
disparity = self.camera_model.getDisparity(point_torso[2])
x = x + disparity
if x >= 0 and x < self.saliency_width and y >= 0 and y < self.saliency_height:
rr, cc = circle(y, x, 25,
(len(saliency_map), len(saliency_map[0])))
saliency_map[rr, cc] = 0.
self.saliency_map_curiosity_pub.publish(
Float32MultiArray(layout=lo, data=saliency_map.flatten()))
saliency_map_image = (saliency_map - saliency_map.min()) / (
saliency_map.max() - saliency_map.min()) * 255.
saliency_map_image = np.uint8(saliency_map_image)
try:
saliency_map_curiosity_image = self.cv_bridge.cv2_to_imgmsg(
saliency_map_image, "mono8")
except CvBridgeError as e:
print e
self.saliency_map_curiosity_image_pub.publish(
saliency_map_curiosity_image)
else:
rospy.loginfo(
"Received saliency map but camera information is missing")
def camera_info_left_callback(self, camera_info):
self.camera_info_left = camera_info
def camera_info_right_callback(self, camera_info):
self.camera_info_right = camera_info
def disparity_callback(self, disparity_image):
self.disparity_image = disparity_image
def saccade_point_callback(self, point):
self.targets.append(point)
