Project Tango: Converting between coordinate systems and merging point clouds

Question

I am trying to convert point clouds sampled and stored in XYZij data (which, according to the document, stores data in camera space) into a world coordinate system so that they can be merged. The frame pair I use for the Tango listener has COORDINATE_FRAME_START_OF_SERVICE as the base frame and COORDINATE_FRAME_DEVICE as the target frame.

This is the way I implement the transformation:

1. Retrieve the rotation quaternion from TangoPoseData.getRotationAsFloats() as q_r, and the point position from XYZij as p.

2. Apply the following rotation, where q_mult is a helper method computing the Hamilton product of two quaternions (I have verified this method against another math library):

   p_transformed = q_mult(q_mult(q_r, p), q_r_conjugated);

3. Add the translate retrieved from TangoPoseData.getTranslationAsFloats() to p_transformed.

But eventually, points at p_transformed always seem to end up in clutter of partly overlapped point clouds instead of an aligned, merged point cloud.

Am I missing anything here? Is there a conceptual mistake in the transformation?

Thanks in advance.

Answer 1

Ken & Vincenzo, thanks for the reply.

I somehow get better results by performing ICP registration using CloudCompare on individual point clouds after they are transformed into world coordinates using pose data alone. Below is a sample result from ~30 scans of a computer desk. Points on the farther end are still a bit off, but with carefully tuned parameters this might be improved. Also CloudCompare's command line interface makes it suitable for batch processing.

Besides the inevitable integration error that needs to be corrected, a mistake I made earlier was wrongly taking the camera space frame (the camera on the device), which is described here in the documentation, to be the same as the OpenGL camera frame, which is the same as the device frame as described here. But they are not.

Also, moving the camera slowly to get more overlap between two adjacent frames also helps registration. And a good visible lighting setup of the scene is important, since besides the motion sensors, Tango also relies on the fish eye camera on its back for motion tracking.

Hope the tips also work for more general cases other than mine.

Answer 2

There are two different "standard" forms of the quaternion notation. One has the rotation angle first, i.e. x i j k, and one has the rotation angle last, i.e. x y z w. The Tango API docs list the TangoPoseData::orientation as x y z w. The Wikipedia page on quaternions lists them as x i j k. You might want to check what notation is assumed in your product method.

Answer 3

Where is your pose data coming from? Are you getting the most recent pose after you are in the callback for the point cloud data or are you asking for the pose that corresponds to the timestamp in the XYZij struct? You should be asking for the pose at time "timestamp" from the XYZij struct.

I tried it, it does not work. I tried to queue the pose and get the nearest one to the XYZij.

Look at the blue wall

The real wall

Answer 4

we from roomplan.de created an opensource sample how to use pcl in project tango apps. It records pointclouds and transforms them into a common coordinate frame (the StartOf Service Frame). You can find the sample code here: https://github.com/roomplan/tango-examples-java/tree/master/PointCloudJava_with_PCL the specific funtion is in jni/jni_part.cpp function: Java_com_tangoproject_experiments_javapointcloud_PointCloudActivity_saveRotatedPointCloud

If you want the sample to compile, you need to clone the complete folder and integrate pcl into your project. A solution how this can be done can be found on our website.

sample pictures can be viewed at the demo app in the playstore. (Cant post them here yet) https://play.google.com/store/apps/details?id=com.tangoproject.experiments.javapointcloud&hl=en