Converting Pixel Location in Camera Frame to World Frame

asked 2016-08-12 12:00:49 -0500

jonlwowski012
29 ●2 ●3 ●8

updated 2016-08-12 14:14:45 -0500

I am trying convert the location in pixels of a target in my camera frame into the x and y coordinates in the world frame. I am currently doing it using the simple pinhole camera model and using TF. The results seem ok but not great. My code is posted below. Let me know if this seems correct or if I am missing something.

self.cam_model.fromCameraInfo(self.info_msg)
cam_model_point = self.cam_model.projectPixelTo3dRay(self.cam_model.rectifyPoint((X_pixel,Y_pixel)))
point_msg.pose.position.x=cam_model_point[0]
point_msg.pose.position.y=cam_model_point[1]
point_msg.pose.position.z=camera.position.z
point_msg.pose.orientation.x=camera.orientation.x
point_msg.pose.orientation.y=camera.orientation.y
point_msg.pose.orientation.z=camera.orientation.z
point_msg.pose.orientation.w=camera.orientation.w
point_msg.header.stamp = rospy.Time.now()
point_msg.header.frame_id = self.cam_model.tfFrame()
    try:
    self.listener.waitForTransform(self.cam_model.tfFrame(), "/world", rospy.Time.now(), rospy.Duration(10.0))
    tf_point = self.listener.transformPose("world", point_msg)

Edit 1: I fixed it slightly and the results look slightly better. I get the correct pattern but the pattern is too small and needs to be scaled up. Below is the new code.

self.cam_model.fromCameraInfo(self.info_msg)
cam_model_point = self.cam_model.projectPixelTo3dRay(self.cam_model.rectifyPoint((X_pixel,Y_pixel)))
point_msg.pose.position.x=cam_model_point[0] + camera.position.x
point_msg.pose.position.y=cam_model_point[1] + camera.position.y
point_msg.pose.position.z=cam_model_point[2] + camera.position.z
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()
    try:
    self.listener.waitForTransform(self.cam_model.tfFrame(), "/world", rospy.Time.now(), rospy.Duration(10.0))
    tf_point = self.listener.transformPose("world", point_msg)

edit retag flag offensive close merge delete

add a comment

1

answered 2016-08-19 10:05:19 -0500

jonlwowski012
29 ●2 ●3 ●8

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)

edit flag offensive delete link

add a comment

1

answered 2016-08-12 12:53:20 -0500

NEngelhard
3519 ●24 ●58 ●89

updated 2016-08-12 15:10:11 -0500

point_msg.pose.position.x=cam_model_point[0] point_msg.pose.position.y=cam_model_point[1] point_msg.pose.position.z=camera.position.z

Why should the z-coordinate of your point depend on the position of the camera? It just depends on the projected pixel and has nothing to do with your camera pose as it is relative to your camera. z should be 1.

And why do you assign a orientation to your point? It's just a point and not a pose.

Edit: Your edit does not make sense. The point is relative to the camera so it's position should not(!) depend on the pose of the camera.

edit flag offensive delete link

Comments

You are correct about the Z position. As for the orientation, I wanted my data to be in a pose message just for convenience so I assigned the orientation as the camera's orientation just to fill out the pose message knowing I am not going to use it.