java - 无法找到如何转换探戈点云

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

探戈示例中有一个ScenePoseCalculator类。并且有一种方法“ toOpenGlCameraPose”。使用此功能时，但是OpenGL相机未正确设置。当我向前移动时，相机向后移动。左右互换。

但是最困难的事情是正确转换PointCloud。我执行以下操作：

创建一个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));
    }

在TangoListener的PointCloud回调中，我执行以下操作：

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

要转换PointCloud，我要做的是：

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

但是，无论我做什么。我已经尝试了一切。但这看起来不正确。 PointCloud相互装订在一起，或者看起来毫无意义地放置在它们之间。

希望有人知道更多...

最佳答案

对于仍然希望了解如何执行此操作的每个人，我只是在jMOnkeyEngine3中对其进行了管理。秘诀是，必须使用探戈提供的旋转四元数的INVERSE。

好的，这是它的工作方式：

通过回调获取深度数据：

public void onXyzIjAvailable(TangoXyzIjData xyzIj)

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

使用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（

干杯