Convert device pose to camera pose

Question

I'm using the camera intrinsic (fx, fy, cx, cy, width, hight) to store a depth image of TangoXyzIjData.xyz buffer. Therefore I calculate for each point of xyz the corresponding image point and store its z value

x' = (fx * x) / z + cx
y' = (fy * y) / z + cy
depthImage[x'][y'] = z

Now I would like to store the corresponding pose data as well. I'm using the timestamp of TangoXyzIjData.timestamp and the following function

getPoseAtTime(double timestamp, TangoCoordinateFramePair framePair)

with framepair

new TangoCoordinateFramePair(TangoPoseData.COORDINATE_FRAME_START_OF_SERVICE, TangoPoseData.COORDINATE_FRAME_DEVICE)

The problem is that the pose is the device frame wrt start of service frame. And the depth image get's its points from the depth camera frame. How can I match them?

There is a way to convert the depth camera points to the device frame by:

1. depth2IMU = depth camera frame wrt IMU frame
2. device2IMU = device frame wrt IMU frame
3. device2IMU^-1 = invert device frame wrt IMU frame
4. camera2Device = device2IMU^-1 * depth2IMU

Now I could multiply each point of the point cloud with camera2Device. But that's the transformation to the device frame.

Is there any way to convert the device pose to a camera pose?

Answer 1

The equation you put together is correct! But it's not finished.

---

Format

To formalize the terminalogy, let's use a_T_b as a transformation matrix, where a represents the base frame, b represents the target frame. a_T_b is a frame with respect to b frame.

---

Compute the matrix

Based on your question, the matrices we known are:

start_service_T_device, imu_T_device , imu_T_depth

The matrix we want to get is:

start_service_T_depth

We can just use a "matrix chain" to get the result:

start_service_T_depth = start_service_T_device * 
                        inverse(imu_T_device) * 
                        imu_T_depth;

Now, let's say we have a point P_depth in depth frame. To apply the pose for this point and convert it to start_service frame, we could use:

P_ss = start_service_T_depth * P_depth;

---

Put it in OpenGL frame

In most of the cases, you might want to convert it to some coordate frame that is easy for graphic library to render out. Let's take OpenGL for example, we can transform this point to the OpenGL world coordinate frame as follow:

Note that start_service_T_opengl_world is a constant matrix that you could compute by hand. Here is a link to the matrix, quoted from Project Tango c++ example.

P_gl = opengl_world_T_start_service * P_ss;

We can expand everything we just wrote and put it in a single equation:

P_gl = opengl_world_T_start_service * 
       start_service_T_device * 
       inverse(imu_T_device) * 
       imu_T_depth * 
       P_depth;

---

Sample code from Project Tango

Also, in the project tango examples, the point cloud example has a pretty good explanation of these conversions, here are the links(c++, java, unity).