Revision history [back]

I got it work. I realize I had to use a stereo camera in order to get a 3D point. The pinhole camera model only gives you a 3D ray. The code below is the correct way to do it.

self.cam_model.fromCameraInfo(self.info_msg_left,self.info_msg_right)
cam_model_point = self.cam_model.projectPixelTo3d((centres[i][0],centres[i][1]),self.Disparity[centres[i][0]][centres[i][1]])
point_msg.pose.position.x= cam_model_point[0]
point_msg.pose.position.y=cam_model_point[1]
point_msg.pose.position.z= cam_model_point[2]
point_msg.pose.orientation.x=0
point_msg.pose.orientation.y=0
point_msg.pose.orientation.z=0
point_msg.pose.orientation.w=1
point_msg.header.stamp = rospy.Time.now()
point_msg.header.frame_id = self.cam_model.tfFrame()
self.listener.waitForTransform(self.cam_model.tfFrame(), "world", rospy.Time.now(), rospy.Duration(1.0))
tf_point = self.listener.transformPose("world", point_msg)