Ask Your Question
1

Converting Pixel Location in Camera Frame to World Frame

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

jonlwowski012 gravatar image

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

2 Answers

Sort by ยป oldest newest most voted
1

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

jonlwowski012 gravatar image

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 more
1

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

NEngelhard gravatar image

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 more

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.

jonlwowski012 gravatar imagejonlwowski012 ( 2016-08-12 13:10:26 -0500 )edit

In this case, set it to (0,0,0,1) if you just want to use the functions.

NEngelhard gravatar imageNEngelhard ( 2016-08-12 13:57:57 -0500 )edit

I added a edit. It looks better now but it needs to be scaled larger.

jonlwowski012 gravatar imagejonlwowski012 ( 2016-08-12 14:15:23 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

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

Seen: 935 times

Last updated: Aug 19 '16