使用单目相机进行地图构建 你学会了吗

1.前言
http://www.makeru.com.cn/video/2099_11457.html?s=144282

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

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

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

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

2.单目相机建图

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

极线搜索；
块匹配。
对于不同的相机，要获得像素点的深度信息，要怎么办呢？

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

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

三角化代码：

// 方程
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// 二阶方程用克莱默法则求解并解之
6Vector3d t = T_R_C.translation();
7Vector3d f2 = T_R_C.rotation_matrix()* f_curr;
8Vector2d b =Vector2d( t.dot ( f_ref ), t.