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)