項目探戈點雲翻譯和旋轉

Question

我想使用Tango設備捕獲點雲圖像並將它們保存到文件中。從那裏我可以將單個的點雲拼湊在一起，形成建築物內部的完整3D圖像。 我已經修改了PointCloudJava應用程序（google tango的官方github頁面）以包含一個按鈕，其中每次按下它時，當前可查看的點雲都會以二進制格式保存到文件以及其姿態數據中。 外部應用程序將二進制點雲轉換爲人可讀的.PTS格式。以下是這一個片段.PTS文件...

… 
3.695209841412947 -0.369136531 -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 
… 

這裏是我的Pose文件的一個片段：

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是我的鏈接姿勢數據點雲的方法;而Quaternion格式爲WXYZ。

一般情況下會糾正個別點雲我想以後我換點雲我看到這每個點的Y和Z軸...

原圖

點雲圖像（加載在雲比較）

這是我所期望的。

當我嘗試將多臺PC拼接在一起形成完整的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)); 
} 

但正如我所說，這似乎並沒有工作的權利？

我的翻譯方法可以嗎？

如何在姿勢文件中使用四元數來旋轉每臺PC？一些代碼示例會很好。

我應用這些變換的順序是什麼？

在正確的方向推動將不勝感激。

Answer 1

發生了什麼事是您沒有在正確的參照系中轉換點雲。在這篇文章中使用解決方案：Convert device pose to camera pose

在答案它假設OpenGL的約定，但你可以使用類似的技術來轉換點任何約定。