如何使用RGB-D相机使用视觉里程计重建3D场景？

Question

我正在使用this示例并尝试向其添加点云可视化。

基本上我的代码是来自链接示例的修改代码：

PointTrackerTwoPass<GrayU8> tracker = FactoryPointTrackerTwoPass.klt(configKlt,
            new ConfigGeneralDetector(600, 3, 1), GrayU8.class, GrayS16.class);
visualOdometry = FactoryVisualOdometry.depthDepthPnP(-1, 120, 2, 150, 100, true, sparseDepth, tracker,
            GrayU8.class, GrayU16.class);
<...>
visualOdometry.process(grayScaleImage, depth));
Se3_F64 leftToWorld = visualOdometry.getCameraToWorld();
AccessPointTracks3D access = (AccessPointTracks3D) visualOdometry;
int N = access.getAllTracks().size();
for (int i = 0; i < N; i++) {
    if (access.isInlier(i)) {
        Point3D_F64 inlierWorld = new Point3D_F64();
        Point3D_F64 inlierCamera = access.getTrackLocation(i);
        georegression.transform.se.SePointOps_F64.transform(leftToWorld, inlierCamera, inlierWorld);
        if (tree.add(new My3DPoint(inlierWorld.x, inlierWorld.y, inlierWorld.z)))
          totalElementsInTree++;

它会产生一个点云，但它与我的场景并不相似。这是一个从上到下的视图，通过旋转kinect相机围绕它的向上矢量实现。图为一个房间的一角。如您所见，上面的代码不是以直角连接的两个直墙，而是产生一系列弧形或曲线。如果我将FactoryVisualOdometry.depthDepthPnP的第一个参数设置为正整数，则效果会更加明显。