Ich kann nicht herausfinden, wie man Tango Point Cloud transformiert

Question

Ich arbeite gerade mit dem Google Tango und bisher hat alles gut funktioniert, aber ich bin auf ein paar seltsame Dinge gestoßen.

Es gibt eine Klasse ScenePoseCalculator in den Tango-Beispielen. Und es gibt eine Methode "toOpenGlCameraPose". Wenn Sie dies verwenden, ist die OpenGL-Kamera jedoch nicht richtig eingerichtet. Wenn ich vorwärts gehe, bewegt sich die Kamera rückwärts. Links und rechts sind ebenfalls vertauscht.

Aber das Schwierigste ist, die PointCloud korrekt zu transformieren. Ich folgendes tun:

Eine 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)); 
    } 

Im Pointcloud Rückruf des TangoListener ich tun:

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(); 
    } 
} 

Um die Punktwolke zu verwandeln ich tun:

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]); 
    } 

Aber was auch immer ich mache. Ich habe alles versucht. Aber es wird nicht gut aussehen. Die PointClouds werden übereinander geheftet oder sehen aus, als wären sie ohne Sinn.

Hoffnung jemand weiß mehr ...

Answer 1

Für alle noch um herauszufinden suchen, wie dies zu tun, ich schaffte es gerade in jMOnkeyEngine3. Das Geheimnis ist, dass man den INVERSE der Rotation Quaternion, den der Tango liefert, verwenden muss.

Okay, hier ist, wie es funktioniert:

Erste Tiefendaten mit dem Rückruf:

public void onXyzIjAvailable(TangoXyzIjData xyzIj) 

ein Array von Eckpunkten erstellen aus dem 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]); 
     } 

Verwenden Sie die ScenePoseCalculator von Rajawali3d:

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

Geräteinnere müssen vorher bestellt werden, aber vergessen Sie nicht, dies in einem TRY/CATCH-Gehäuse zu tun.

Danach, Rotation Quaternion bekommen und Pose Vector3 vom Tango-Pose:

Pose cloudPose = this.rajawaliPoseToJMEPoseCLOUD(p); 

Diese Methode wird hier definiert:

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; 
} 

Dann legen Sie die Eckpunkte in einem Knoten und zu transformieren, dass der Knoten mit die Pose-Daten der entsprechenden XYZIJ-Daten:

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

Hoffe es hilft jemandem. Nahm mich 4 Tage, um es herauszufinden x (

Prost