SLAM以及三维重建学习和实践记录

注：本文主要是实验的一些记录，通过对已有项目的运行和代码的了解来学习三维重建和SLAM相关的知识，意在打个基础。

三维重建和SLAM

总所周知，一个SLAM包含了很多流程，前端配准，后端优化。利用传感器的数据建图的问题也被包含在内。而对轮式机器人来说，在配置限制的情况下由于相机角度固定和2D雷达的原因三维重建并不能将稠密的三维模型整体建立出来，同时其对实时性也有要求，这和三维重建的其他应用场景差别较大，因此在机器人上更多的说法是SfM。

• SfM( Structure from Motion)：属于被动的三维重建的方法，是在已经知晓机器人的运动信息（相机姿态）和相机画面后进行匹配后建图
• VSLAM: 视觉slam即在搭载摄像头的机器人下进行实时建图和定位导航等，要求输入图像的系统是线性时不变的
• NerF (Neural Radiance Fields)：神经网络辐射场是近几年非常火的基于深度学习的主动式三维重建方法，重建的为Explcilt representation的三维图。按笔者的理解来说，主动的意思就是要在要针对重建的对象上拍几张照片；和implicit representation则是人类能直接看出来的模型，而不是计算机能用的。

3D Reconstruction

KinectFusion学习

KinectFusion是什么

来自Izadi大神团队，Richard A. Newcombe 等人在2011年提出的是一个基于RGBD相机的适用于任何环境光照的实时重建追踪和重建算法，以下是它的工作流程

代码实现

姿态估计

通过借助前一帧的表面信息{v, n}与当前帧的顶点和法线来进行alignment的计算，主要是通过使用point to plane ICP
计算表面向量，后进行SVD

    for (size_t i = 0; i < correspondence.size(); ++i ){
        auto match = correspondence[i];

        Eigen::Vector3d d_i = destPoints[ match.first ];
        Eigen::Vector3d n_i = destNormals[ match.first ];
        Eigen::Vector3d s_i = sourcePoints[ match.second ];

        Eigen::Matrix<double, 6, 1> A_i;
        A_i << s_i.cross(n_i) , n_i;
        A.row(i) = A_i;
        b(i) = n_i.dot(d_i) - n_i.dot(s_i);
    }

    Eigen::Matrix<double, 6,1> x = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);

Point2Point

void LinearSolver::solvePoint2Point(const std::vector<Eigen::Vector3d>& sourcePoints,
            const std::vector<Eigen::Vector3d>& destPoints,
            const std::vector<std::pair<size_t, size_t>>& correspondence){
    const size_t N = correspondence.size();
    Eigen::MatrixXd A( N*3 , 6);
    Eigen::MatrixXd b( N*3 , 1);
    for (size_t i = 0; i < correspondence.size(); ++i ){
        auto match = correspondence[i];

        Eigen::Vector3d d_i = destPoints[ match.first ];
        Eigen::Vector3d s_i = sourcePoints[ match.second ];

        Eigen::Matrix<double, 3, 6> A_i;
        A_i << 0,    s_i.z(), -s_i.y(), 1, 0, 0,
           -s_i.z(),   0,      s_i.x(), 0, 1, 0,
            s_i.y(), -s_i.x(), 0, 0, 0, 1;

        A.row(i*3) = A_i.row(0);
        A.row(i*3+1) = A_i.row(1);
        A.row(i*3+2) = A_i.row(2);

        b(i*3)     = d_i.x() - s_i.x();
        b(i*3 + 1) = d_i.y() - s_i.y();
        b(i*3 + 2) = d_i.z() - s_i.z();
    }

    Eigen::Matrix<double, 6,1> x = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
    solution = x;
}

表面探测

计算三维光线方向，用于表面预测

Eigen::Vector3d Raycast::calculateRayDirection(size_t x, size_t y, const Eigen::Matrix3d& rotation,
                                               const Eigen::Matrix3d& intrinsics) {
    double fovX = intrinsics(0, 0);
    double fovY = intrinsics(1, 1);
    double cX = intrinsics(0, 2);
    double cY = intrinsics(1, 2);

    Eigen::Vector3d cameraPoint = Eigen::Vector3d((x - cX) / fovX , (y - cY) / fovY ,1.0);
    Eigen::Vector3d rayDirection = rotation * cameraPoint;
    rayDirection.normalize();

    return rayDirection;
}

表面预测本体算法部分

 for( size_t v =0;v<height;v++){
        for(size_t u=0;u< width;u++) {
            vertices[u+ v*width] = Eigen::Vector3d(MINF, MINF, MINF);
            //calculate Normalized Direction
            auto direction = calculateRayDirection(u, v, rotationMatrix, currentFrame->getIntrinsics());

            //calculate rayLength
            float rayLength (0.f);

            Ray ray (translation, direction);
            if ( ! (volume->intersects( ray, rayLength))) continue;

            rayLength += voxelScale;

            Eigen::Vector3d currentPoint;
            if(! calculatePointOnRay(currentPoint, volume, translation,
                                     direction,rayLength))
                continue;

            double currentTSDF = volume->getTSDF(currentPoint);

            const double maxSearchLength = rayLength + volumeRange.norm();

            Eigen::Vector3d previousPoint;

            for (; rayLength < maxSearchLength; rayLength += truncationDistance * 0.5f) {

                Eigen::Vector3d previousPoint = currentPoint;
                const double previousTSDF = currentTSDF;

                if (!calculatePointOnRay(currentPoint, volume, translation,
                                         direction,rayLength+truncationDistance * 0.5f))
                    continue;

                currentTSDF = volume->getTSDF(currentPoint);
                if (previousTSDF < 0. && currentTSDF > 0.)break;
                if (previousTSDF > 0. && currentTSDF < 0.) {
                    Eigen::Vector3d globalVertex = getVertexAtZeroCrossing(previousPoint, currentPoint, previousTSDF, currentTSDF);
                    vertices[u+ v*width] = globalVertex;

                    Eigen::Vector3d gridVertex = (globalVertex - volume->getOrigin())/ voxelScale;
                    if (gridVertex.x()-1 < 1 || gridVertex.x()+1 >= volumeSize.x() - 1 ||
                        gridVertex.y()-1 < 1 || gridVertex.y()+1 >= volumeSize.y() - 1 ||
                        gridVertex.z()-1 < 1 || gridVertex.z()+1 >= volumeSize.z() - 1)
                        break;
                    Vector4uc color;

                    if(std::abs(previousTSDF) < std::abs(currentTSDF)){
                        color = volume->getColor(previousPoint);
                    }
                    else{
                        color = volume->getColor(currentPoint);
                    }
                    currentFrame->setGlobalPoint(globalVertex,u,v);
                    currentFrame->setColor(color,u,v);
                    colors.emplace_back(color);

                }

姿势预测

实验

代码从github下载
Kinect Github

实验流程

首先，根据README.md中的流程，安装requirements

sudo apt-get install libfreeimage3 libfreeimage-dev
sudo apt-get install libeigen3-dev
sudo apt-get install libgoogle-glog-dev
sudo apt-get install libceres-dev

然后发现\Fusionlib\CMakeLists.txt有 CMakelist.txt，直接进行Complie

cmake . && make

发现报错，eigen3找不到了

CMake Error at extern/CMakeLists.txt:17 (message):
  Problems with submodule Eigen3.
-- Configuring incomplete, errors occurred!
See also "/home/agilex/Downloads/KinectFusion-master/CMakeFiles/CMakeOutput.log".

将Eigen3 加入

find_package(Eigen3 REQUIRED)
include_directories(${EIGEN3_INCLUDE_DIR})

然后在所有src中替换PROJECT_DIR为自己的模拟文件路径
以main.cpp line69 为例

std::string filenameIn = PROJECT_DATA_DIR + std::string("/recording/"); \\修改PROJECT_DATA_DIR 

然后下载自己的模拟数据集，应该就可以运行了

SLAM 框架

ORB-SLAM系列

ORB（Orinted FAST and BRIEF) -SLAM是比较著名的SLAM框架,加入了循环回路的检测和闭合机制, 以消除误差累积。系统采用与重定位相同的方法来检测回路(匹配回路两侧关键帧上的公共点), 通过方位图 (Pose Graph) 优化来闭合回路.

• ORBSLAM1
  最初版本，只能使用单目相机
• ORBSLAM2
  是基于单目，双目和 RGB-D 相机的一套完整的 SLAM 方案。它能够实现地图重用，回环检测和重新定位的功能。
• ORBSLAM3
  基于ORBSLAM2 加上了惯性导航，提供了多地图的存储和拼接功能，在跟踪丢失后可以尝试将现有地图和历史地图进行匹配、融合，并更新当前的活跃地图（Active Map）

ORBSLAM2 实验

首先，将项目拷贝到机器人的主控上，可以使用

git clone https://github.com/raulmur/ORB_SLAM2.git

如果无法访问则可以使用在自己的电脑上下载，然后通过远程连接软件或者scp传输。
然后cd 进入项目目录，默认所有基础依赖已经安装，处理SLAM可视化的Pangolin需要安装

cd Pangolin
mkdir build
cd build
cmake ..
make -j
sudo make instal

编译好后进入项目目录，其实build.sh里也是以上几句，build是编译本体（数据集运行），build_ros 则是可以在ros中实时运行（生成ORB2_SLAM的package）。

cd ORB_SLAM2
chmod +x build.sh
chmod +x build_ros.sh
./build.sh
./build_ros.sh 

笔者在build的时候遇到了死机，如果编译死机则需要将 CMakeList.txt里make前面的 -j删掉，一般来说还会有报错，观察报错log，如果是usleep的报错，在.cc头文件中加入#include<unistd.h>,详见
ORB_SLAM2报错

然后就可以运行了，首先启动摄像头节点和roscore，如果是包的话直接roslaunch，笔者使用的是奥比中光的Dabai RGB-D

roslaunch astra_camera dabai_u3.launch

然后开启RGB-D的节点，再开启一个终端

rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/Asus.yaml

ORB_SLAM2后面跟的分别是 所用相机种类、词袋模型相对路径、相机标定参数（如果没有则使用如上相对路径），如果是双目的话，还需要加修正的参数

实验结果

有时还会段错误，尝试搜索有很多说法但原因未知。估计是内存爆了。运行的时候机器人移动需要慢一些，否则容易lost，找不到关键帧和特征点

参考Reference

[1] https://zhuanlan.zhihu.com/p/24873528
[2] Newcombe, R. A., Fitzgibbon, A., Izadi, S., Hilliges, O., Molyneaux, D., Kim, D., Davison, A. J., Kohi, P., Shotton, J., & Hodges, S. (2011, 2011/10). KinectFusion: Real-time dense surface mapping and tracking 2011 10th IEEE International Symposium on Mixed and Augmented Reality.
[3] https://zhuanlan.zhihu.com/p/92642910
[4] https://blog.51cto.com/u_14411234/3099497
[5] https://zhuanlan.zhihu.com/p/206952146