Can't find out how to transform Tango Point Cloud

Question

I work with the Google Tango right now and so far everything worked fine, but I stumbled upon a few strange things.

There is a class ScenePoseCalculator in the Tango Examples. And there is a method "toOpenGlCameraPose". When using this, the OpenGL camera however is not set up correctly. When I move forward, the camera moves backwards. Left and right are swapped as well.

But the most difficult thing is to transform the PointCloud correctly. I do the following:

Create an Vector3-Array:

Vector3f [] vertices = new Vector3f[mPointCloud.getGeometry().getVertices().capacity()];
    for(int i=0; i<mPointCloud.getGeometry().getVertices().capacity(); i++) {
        vertices[i] = new Vector3f(
            mPointCloud.getGeometry().getVertices().get(i*3), 
            mPointCloud.getGeometry().getVertices().get(i*3+1),
            mPointCloud.getGeometry().getVertices().get(i*3+2));
    }

In the PointCloud Callback of the TangoListener I do:

private void updateXYZIJ() {

    try {
        if(pcm.getLatestXyzIj().xyzCount<10) return;

        TangoCoordinateFramePair fp = new TangoCoordinateFramePair(
            TangoPoseData.COORDINATE_FRAME_START_OF_SERVICE,
            TangoPoseData.COORDINATE_FRAME_DEVICE);

        TangoXyzIjData xyzIj = pcm.getLatestXyzIj();
        depthArray = new float[xyzIj.xyzCount * 3];
        xyzIj.xyz.get(depthArray);

        com.projecttango.rajawali.Pose p = 
            ScenePoseCalculator.toDepthCameraOpenGlPose(
                tangoProvider.getTango().getPoseAtTime( 
                    xyzIj.timestamp, fp), this.setupExtrinsics());

        Pose cloudPose = this.rajawaliPoseToJMEPoseCLOUD(p);

        currentPointCloudData = new PointCloudUpdate(
            new Date(),
            depthArray,
            xyzIj.xyzCount,
            xyzIj.ijRows,
            xyzIj.ijCols,
            0,
            cloudPose,
            null
        );

        // inform listeners
        for (PointCloudListener listener : this.pointsListeners) {
            listener.acceptMessage(tangoProvider, this.currentPointCloudData);
        }

    } catch (Exception e) {
        e.printStackTrace();
    }
}

To transform the PointCloud I do:

this.transformation = new Matrix4f();
    this.transformation.setRotationQuaternion(
        this.referencePose.orientation);
    this.transformation.setTranslation(this.referencePose.place);

absVertices = new Vector3f[vertices.length];
    for(int i=0; i<vertices.length; i++) {

        // Create new Vertex
        absVertices[i]=new Vector3f(
            vertices[i].x,
            vertices[i].y,
            vertices[i].z
        );

        Vector3f v = absVertices[i];

        transformation.rotateVect(absVertices[i]);
        transformation.translateVect(absVertices[i]);
    }

But whatever I do. I've tried everything. But it won't look right. The PointClouds get stapled over each other or look like they where placed without any sense.

Hope someone knows more...

Answer 1

For everyone still looking to find out how to do this, I just managed it in jMOnkeyEngine3. The secret is, one has to use the INVERSE of the rotation Quaternion which the Tango delivers.

Okay, here is how it works:

Getting depth data with the Callback:

public void onXyzIjAvailable(TangoXyzIjData xyzIj)

Create an Array of vertices from the xyz-Array:

// Create new array, big enough to hold all vertices
        depthArray = new float[xyzIj.xyzCount * 3];
        xyzIj.xyz.get(depthArray);

        // Create Vertices
        Vector3f[] vertices = new Vector3f[xyzIj.xyzCount];
        for(int i=0; i<xyzIj.xyzCount; i++) {
            vertices[i] = new Vector3f(
                depthArray[i*3],
                depthArray[i*3+1], 
                depthArray[i*3+2]);
        }

Use the ScenePoseCalculator from Rajawali3d:

com.projecttango.rajawali.Pose p = ScenePoseCalculator
            .toDepthCameraOpenGlPose(tangoProvider.getTango().getPoseAtTime( 
                  xyzIj.timestamp, framePair_SS_D), this.setupExtrinsics());

Device intrinsics must be ordered before that, but remember to do it in an TRY/CATCH enclosure.

After that, get Rotation Quaternion and Pose Vector3 from the Tango Pose:

Pose cloudPose = this.rajawaliPoseToJMEPoseCLOUD(p);

That method is defined here:

private Pose rajawaliPoseToJMEPoseCLOUD(com.projecttango.rajawali.Pose p) {
    Pose pose = new Pose(
        new Vector3f(
            (float)p.getPosition().x, 
            (float)p.getPosition().y, 
            (float)p.getPosition().z),
        new Quaternion(
            (float)p.getOrientation().x,
            (float)p.getOrientation().y,
            (float)p.getOrientation().z,
            (float)p.getOrientation().w
        ));

    return pose;
}

Then Put the vertices in a node and transform that node with the Pose Data of the according XYZIJ-Data:

meshNode.setLocalRotation(pcu.referencePose.orientation.inverse());
    meshNode.setLocalTranslation(pcu.referencePose.place);

Hope it helps someone. Took me 4 days to figure it out x(

Cheers