【SLAM】使用单目相机进行地图构建

1.前言

经过我们连续超长时间的学习，我们终于可以到介绍建图问题了！

之前我们一致在介绍SLAM系统的前三大组成部分，前端估计、后端估计、回环检测，那么，第四大部分，建图问题，就从本文开始介绍，这也是SLAM系统中基础环节的最后一个问题。

试想一下，对于基于人工设计的特征点，它构建的地图是怎么样的呢？

由于特征点的稀疏性，这会导致SLAM地图构建的稀疏性，对于稀疏的地图，在一些场景下是可以使用的，比如大致的路线规划，但是，在大多数场景下，我们还是希望构建一个不是那么稀疏的地图，即稠密地图或者半稠密地图。

2.单目相机建图

对于建图过程，首先需要特征匹配，就是可以使用之前讲解的BA优化求解问题，对于特征匹配的算法，通常可以使用如下2种方法：

1. 极线搜索；
2. 块匹配。

对于不同的相机，要获得像素点的深度信息，要怎么办呢？

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-kzR2TIG8-1590475150591)(https://b03.ickimg.com/202005/f5acde71a37de3e7eebaf8819180dc5e.png)]

通常情况下，有如下几种方案：

1. 双目相机提供立体匹配得出深度图；
2. 单目相机通过三角化提供深度图；
3. 深度相机直接提供深度信息。

这里，我们使用单目相机构建地图为例进行讲解，当然，单目相机构建深度地图有着很多缺点，得出的深度图还不一定可靠，但是，通过这个例子，大家可以很容易的实现并且理解整个系统和方法。

三角化代码：

 // 方程
 2     // d_ref * f_ref = d_cur * ( R_RC * f_cur ) + t_RC
 3     // => [ f_ref^T f_ref, -f_ref^T f_cur ] [d_ref] = [f_ref^T t]
 4     //    [ f_cur^T f_ref, -f_cur^T f_cur ] [d_cur] = [f_cur^T t]
 5     // 二阶方程用克莱默法则求解并解之
 6     Vector3d t = T_R_C.translation();
 7     Vector3d f2 = T_R_C.rotation_matrix() * f_curr; 
 8     Vector2d b = Vector2d ( t.dot ( f_ref ), t.dot ( f2 ) );
 9     double A[4];
10     A[0] = f_ref.dot ( f_ref );
11     A[2] = f_ref.dot ( f2 );
12     A[1] = -A[2];
13     A[3] = - f2.dot ( f2 );
14     double d = A[0]*A[3]-A[1]*A[2];
15     Vector2d lambdavec = 
16         Vector2d (  A[3] * b ( 0,0 ) - A[1] * b ( 1,0 ),
17                     -A[2] * b ( 0,0 ) + A[0] * b ( 1,0 )) /d;
18     Vector3d xm = lambdavec ( 0,0 ) * f_ref;
19     Vector3d xn = t + lambdavec ( 1,0 ) * f2;
20     Vector3d d_esti = ( xm+xn ) / 2.0;  // 三角化算得的深度向量
21     double depth_estimation = d_esti.norm();   // 深度值

3.极线搜索与块匹配

如上图所示，我们之前讲解过这方面的概念，相信大家也很熟悉了，通过极限约束我们可以将一个搜索问题从二维变为一维，这会大大减少计算量。

简单的来讲，一个图像的特征点我们可以通过比较搜索另一个图像的相似特征点而不用特征匹配，就可以简单的比较光度误差就可以了，比如直接法就是直接比较光度误差，这样的话就比较快，但是，光度误差只有灰度值，单一的比较大小会存在很多误差，比如光照变化等等，如何让搜索的误差减少呢？

这就需要块匹配的方法。

何为块匹配呢，就是在极线搜索的时候不在使用一个像素点对比，而是使用一块像素块来对比。这避免了单一像素对比时出现的问题。

有如下几种计算差异的办法：

1. SAD
2. SSD
3. NCC

但是，在搜索的时候可能会出现非凸函数，这就需要一个滤波器或者非线性优化来求解。

4.滤波器

在衡量深度是否估计正确，需要一个不确定度的概率模型来衡量，不断优化整个不确定度，就得到了最优的深度值。

假设深度值服从高斯分布。

• a = p - t

• α = arccos<p, t="">

• β = arccos<a, -t="">

• δβ=arctan1/f

• β’= β-δβ

• γ=π-α -β

• σobs=||p||-||p’||

> 有δβ的像素误差，就会造成p和p‘之间的差异。两个深度向量的1范数之差定义为σobs，作为一个像素误差带来的深度不确定度。

5.实践

主函数：

int main(int argc, char **argv) {
    if (argc != 2) {
        cout << "Usage: dense_mapping path_to_test_dataset" << endl;
        return -1;
    }

    // 从数据集读取数据
    vector<string> color_image_files;
    vector<se3d> poses_TWC;
    Mat ref_depth;
    bool ret = readDatasetFiles(argv[1], color_image_files, poses_TWC, ref_depth);
    if (ret == false) {
        cout &lt;&lt; "Reading image files failed!" &lt;&lt; endl;
        return -1;
    }
    cout &lt;&lt; "read total " &lt;&lt; color_image_files.size() &lt;&lt; " files." &lt;&lt; endl;

    // 第一张图
    Mat ref = imread(color_image_files[0], 0);                // gray-scale image
    SE3d pose_ref_TWC = poses_TWC[0];
    double init_depth = 3.0;    // 深度初始值
    double init_cov2 = 3.0;     // 方差初始值
    Mat depth(height, width, CV_64F, init_depth);             // 深度图
    Mat depth_cov2(height, width, CV_64F, init_cov2);         // 深度图方差

    for (int index = 1; index &lt; color_image_files.size(); index++) {
        cout &lt;&lt; "*** loop " &lt;&lt; index &lt;&lt; " ***" &lt;&lt; endl;
        Mat curr = imread(color_image_files[index], 0);
        if (curr.data == nullptr) continue;
        SE3d pose_curr_TWC = poses_TWC[index];
        SE3d pose_T_C_R = pose_curr_TWC.inverse() * pose_ref_TWC;   // 坐标转换关系： T_C_W * T_W_R = T_C_R
        update(ref, curr, pose_T_C_R, depth, depth_cov2);
        evaludateDepth(ref_depth, depth);
        plotDepth(ref_depth, depth);
        imshow("image", curr);
        waitKey(1);
    }

    cout &lt;&lt; "estimation returns, saving depth map ..." &lt;&lt; endl;
    imwrite("depth.png", depth);
    cout &lt;&lt; "done." &lt;&lt; endl;

    return 0;
}

一些参数：

// parameters
const int boarder = 20;         // 边缘宽度
const int width = 640;          // 图像宽度
const int height = 480;         // 图像高度
const double fx = 481.2f;       // 相机内参
const double fy = -480.0f;
const double cx = 319.5f;
const double cy = 239.5f;
const int ncc_window_size = 3;    // NCC 取的窗口半宽度
const int ncc_area = (2 * ncc_window_size + 1) * (2 * ncc_window_size + 1); // NCC窗口面积
const double min_cov = 0.1;     // 收敛判定：最小方差
const double max_cov = 10;      // 发散判定：最大方差


/@@**
 * 根据新的图像更新深度估计
 * @param ref           参考图像
 * @param curr          当前图像
 * @param T_C_R         参考图像到当前图像的位姿
 * @param depth         深度
 * @param depth_cov     深度方差
 * @return              是否成功
 */
bool update(
    const Mat &ref,
    const Mat &curr,
    const SE3d &T_C_R,
    Mat &depth,
    Mat &depth_cov2
);

/@@**
 * 极线搜索
 * @param ref           参考图像
 * @param curr          当前图像
 * @param T_C_R         位姿
 * @param pt_ref        参考图像中点的位置
 * @param depth_mu      深度均值
 * @param depth_cov     深度方差
 * @param pt_curr       当前点
 * @param epipolar_direction  极线方向
 * @return              是否成功
 */
bool epipolarSearch(
    const Mat &ref,
    const Mat &curr,
    const SE3d &T_C_R,
    const Vector2d &pt_ref,
    const double &depth_mu,
    const double &depth_cov,
    Vector2d &pt_curr,
    Vector2d &epipolar_direction
);

/@@**
 * 更新深度滤波器
 * @param pt_ref    参考图像点
 * @param pt_curr   当前图像点
 * @param T_C_R     位姿
 * @param epipolar_direction 极线方向
 * @param depth     深度均值
 * @param depth_cov2    深度方向
 * @return          是否成功
 */
bool updateDepthFilter(
    const Vector2d &pt_ref,
    const Vector2d &pt_curr,
    const SE3d &T_C_R,
    const Vector2d &epipolar_direction,
    Mat &depth,
    Mat &depth_cov2
);

6.小结

这一章，我们学习了相关的单目建图的应用，这是一个简单的例子，在实际场景中，还有很多的复杂性，这也是如何减少这方面误差称为一个研究热点，也有很多方法的而提出，比如在滤波器优化时，有均值-高斯分布的假设，而不简简单单时高斯分布的假设。