java - 找不到如何转换Tango点云

Question

我现在使用 Google Tango，到目前为止一切正常，但我偶然发现了一些奇怪的事情。

Tango Examples 中有一个 ScenePoseCalculator 类。并且有一个方法“toOpenGlCameraPose”。然而，使用此功能时，OpenGL 相机设置不正确。当我向前移动时，摄像机向后移动。左右也交换了。

但最困难的是正确变换点云。我执行以下操作:

创建 Vector3 数组:

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

在 TangoListener 的点云回调中，我执行以下操作:

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

为了转换点云，我这样做:

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

但无论我做什么。我已经尝试了一切。但看起来不太对劲。点云相互钉在一起，或者看起来像是毫无意义地放置在一起。

希望有人知道更多...

Answer 1

对于仍在寻找如何执行此操作的每个人，我只是在 jMOnkeyEngine3 中进行了管理。 secret 是，必须使用 Tango 提供的旋转四元数的反转。

好的，这是它的工作原理:

通过回调获取深度数据:

public void onXyzIjAvailable(TangoXyzIjData xyzIj)

从 xyz 数组创建顶点数组:

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

使用 Rajawali3d 中的 ScenePoseCalculator:

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

必须在此之前订购设备内在函数，但请记住在 TRY/CATCH 封装中执行此操作。

之后，从 Tango Pose 中获取 Rotation Quaternion 和 Pose Vector3:

Pose cloudPose = this.rajawaliPoseToJMEPoseCLOUD(p);

该方法在这里定义:

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

然后将顶点放入节点中，并使用相应 XYZIJ-Data 的姿势数据转换该节点:

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

希望它对某人有帮助。我花了 4 天才弄清楚 x(

干杯