Java 项目Tango点云平移和旋转

我想使用Tango设备捕获点云图像并将其保存到文件中。从那里我可以把各个点云粘在一起，形成一个完整的建筑内部3D图像。 我已经从（google tango的官方github页面）修改了PointCloudJava应用程序，添加了一个按钮，每次按下该按钮，当前可查看的点云将以二进制格式与其姿势数据一起保存到一个文件中。 外部应用程序将二进制点云转换为可读的.PTS格式。下面是这个.PTS文件的一个片段

…
3.695209841412947 -0.473210369136531 -4.512059939636327
3.688643257302687 -0.479607720759112 -4.520564449562169
3.693362633866713 -0.489787657644946 -4.520674956573583
3.689930121583388 -0.445884992268283 -4.507568610443212
3.685492555779860 -0.462776307847697 -4.512719524635411
3.667657594365523 -0.480809484508235 -4.535319340957738
3.687827806157515 -0.496090938236911 -4.526998413337804
3.673954348248885 -0.508897741224963 -4.541318190826512
3.666625241918013 -0.498062555577952 -4.540336502327062
3.678675393742964 -0.474170316841799 -4.529156101478673
3.684755723161147 -0.485069070842463 -4.528201831115819
3.680454175156996 -0.519459594872195 -4.541313899292089
3.673863212747023 -0.492040087368685 -4.534148228897191
3.683874587697432 -0.502062504079539 -4.534162414802648
3.662268142384932 -0.486682941105563 -4.540854943527318
3.659110169095442 -0.521473725226123 -4.556032908691503
3.673068563622878 -0.526471559789378 -4.549449575676061
…

这是我的姿势文件的一个片段

Snapshot Event ID: 0
Translation: -2.828956167382643 3.18717159105692 0.4915560231473033
Quarternion: 0.7047427543062862 0.6239282881689626 0.23707690815727211 0.2405112909905187
Accuracy: 4.2E-45
Confidence: 1688685432

Snapshot Event ID: 1
Translation: -2.796160158125086 4.193282171082194 0.5346153464487627
Quarternion: 0.4973877606777205 0.4648489837872261 0.515537924119383 0.520328248891662
Accuracy: 4.2E-45
Confidence: 1687633528

Snapshot Event ID: 2
Translation: -3.504446692020165 3.1853833575251675 0.5117019018999412
Quarternion: 0.6256551950823549 0.5778181469917516 -0.3583361921431137 -0.3824616834061323
Accuracy: 4.2E-45
Confidence: 1687858272 

其中，快照ID是将姿势数据链接到点云的方法；四元数的格式是WXYZ

这通常适用于单个点云，我认为，在交换点云中每个点的Y轴和Z轴后，我看到

原始图像

点云图像（加载到云比较中）

这正是我所期望的

当我试图将多台电脑粘在一起，形成一座建筑物的完整3D内部时，问题就出现了

通过将相应姿势数据中的XY和Z坐标添加到点云中的每个XY和Z点，我已经转换了点云。不确定这样做是否正确。然后我像以前一样交换了Y和Z。如果我这样做的话，大多数点云都会相互叠加，但这是意料之中的，因为我还没有（也不知道如何）应用旋转

这是我做这件事的一般方法

for (File file : files) {
    if (FilenameUtils.getExtension(file.getAbsolutePath()).equals("dat")) {
        PointCloud cloud = PointCloud.newInstance(file.toURI().toURL());
        if (poseMap.containsKey(cloud.getEventID())) {
            Pose pose = poseMap.get(cloud.getEventID());
            //cloud = cloud.rotate(pose);
            cloud = cloud.translate(pose);
        }
        cloud = cloud.invertYandZ();
        //cloud = cloud.invertAllAxis();
        String absFilename = ptsFolderTxt.getText() + '\\' + FilenameUtils.getBaseName(cloud.getFilename()) + ".pts";
        cloud.write(absFilename);

正如你所看到的，我尝试了一种旋转方法，但它似乎不起作用，所以我把它注释掉了

旋转方法将此应用于每个点

ThreeDPoint rotatePoint(ThreeDPoint oldPoint)
{
    float ox = oldPoint.getxPoint().floatValue();
    float oy = oldPoint.getyPoint().floatValue();
    float oz = oldPoint.getzPoint().floatValue();

    float qx = quaternion.getX().floatValue();
    float qy = quaternion.getY().floatValue();
    float qz = quaternion.getZ().floatValue();
    float qw = quaternion.getW().floatValue();

    float newX = (float)((1-2*qy*qy-2*qz*qz)*ox+(2*qx*qy+2*qw*qz)*oy+(2*qx*qz-2*qw*qy)*oz);
    float newY = (float)((2*qx*qy-2*qw*qz)*ox+(1-2*qx*qx-2*qz*qz)*oy+(2*qy*qz+2*qw*qx)*oz);
    float newZ = (float)((2*qx*qz+2*qw*qy)*ox+(2*qy*qz-2*qw*qx)*oy+(1-2*qx*qx-2*qy*qy)*oz);

    return ThreeDPoint.newInstance(BigDecimal.valueOf(newX), BigDecimal.valueOf(newY), BigDecimal.valueOf(newZ));
}

但正如我所说，这似乎不起作用

我的翻译方法行吗

如何使用姿势文件中的四元数旋转每台电脑？一些代码示例将非常好

我应用这些变换的顺序是什么

---

向正确的方向推进将非常感激。

发生的事情是，您没有在正确的参考系中变换点云。使用本文中的解决方案：

---

答案是假设使用OpenGL约定，但您可以使用类似的技术将该点转换为任何约定。

谢谢，作为探戈新手，我想我还有很多东西要学。我正在查看您链接到的帖子中的Java链接，ScenePoseCalcuator类似乎包含了大多数有用的矩阵转换，可以完成我需要的工作。这是一个合适的地方吗？我不认为有一个函数可以做完全相同的事情，但是

toDepthCameraOpenGlPose

已经足够接近了

toDepthCameraOpenGlPose

现在返回一个姿势，但理想情况下，它应该返回一个矩阵来乘以pointcloud中的3D点。要使用此函数，我需要先设置设备外部，这只能在运行的设备应用程序中完成。对吗？我现在身上没有Tango（将在下周取回），所以我希望我拥有能够在我的外部桌面应用程序中执行转换所需的大部分数据。是的，Exterinsics只需要查询一次。