菜鸟专学：从头到尾创建自己的SLAM系统（转载，我觉这种自己写一个SLAM很好）

RobotSlamApplication项目二： 小型SLAM系统

研究背景： 因为之前比较浮躁，总是喜欢研究别人的库然后测试跑通，效果好就拿来修修改改、然后测试测试就用，效果不好就抛弃。自己没有潜心下来真正写过一个系统，这次从头到尾写一个SLAM系统，写的过程真的不容易，写完就感觉舒畅多了。。

一、前言

RGB-D 相机的优点和缺点（结构光深度相机）

优点:

1. 适合在光照不足，缺乏纹理的场景中使用；
2. 在一定范围内可以达到较高的测量精度；
3. 技术成熟，深度图像可以做到较高的分辨率；

缺点:

1. 室外环境中基本不能使用；
2. 测量距离较近；
3. 容易受到光滑平面反光的影响。

二、相机的位姿估计

基于ORB特征点法获取不同帧图像之间的变换（这个方法原理很多大牛都讲过，这里就不多说）， 主要涉及以下部分：

1. 特征点的提取和匹配:

在实时获取图像数据的过程中，这一环节需要注意图像帧的获取，这里的帧包括当前帧和参考帧（也就是上一时刻的图像帧），当我们计算图像的匹配时，需要区分不同时刻的帧，以计算相邻帧之间图像变换。

即，在初始时刻，将当前帧赋给参考帧，然后在相机运动跟踪过程中，执行完每一次的图像匹配和跟踪之后，都要将当前帧进行转换之后赋给参考帧，以实现"帧"的更新。

		case INITIALIZING:
	    {
			state_ = OK;
			curr_frame = ref_frame = frame;
			....
			break;
	    }
	    case OK:
	    {
			curr_frame = frame;
			....
			if ( CameraPoses is OK ... )
			{
			    curr_frame->T_c_w_ = T * ref_frame->T_c_w_; 
			    ref_frame = curr_frame;
				....
			}
			else 
			{
				.....
			}
		}

2. 相机位姿估算；

相机位姿的估算也有很多种方式：2d-2d(本质矩阵、单应矩阵计算求解), 3d-2d（PnP求解）, 3d-3d（SVD+BA求解）。
在这些方法中使用比较多的就是PnP方法，而PnP求解也比较简单，可以直接调用OpenCV的方法。

	bool solvePnPRansac( InputArray objectPoints, InputArray imagePoints,
		                          InputArray cameraMatrix, InputArray distCoeffs,
		                          OutputArray rvec, OutputArray tvec,
		                          bool useExtrinsicGuess = false, int iterationsCount = 100,
		                          float reprojectionError = 8.0, double confidence = 0.99,
		                          OutputArray inliers = noArray(), int flags = SOLVEPNP_ITERATIVE );
		                          
	bool solvePnP( InputArray objectPoints, InputArray imagePoints,
		                    InputArray cameraMatrix, InputArray distCoeffs,
		                    OutputArray rvec, OutputArray tvec,
		                    bool useExtrinsicGuess = false, int flags = SOLVEPNP_ITERATIVE );

我自己测试tum的测试集获得相机运动轨迹（视觉里程计：VO）如下图：

三、地图创建1

在完成视觉里程计模块之后，接下来就是地图的创建和更新，由于时间有限，我又想完整整个SLAM系统所以对于地图的优化部分就没有深入调试，目前只完成了初步地图的创建和更新。第一步，构建点云地图如下：

看到这个样子的地图是不是要疯了。。。

不用着急，在经过仔细的对比调试之后， 发现其实是由于当前帧和参考帧之间的变换矩阵没有对应好，导致地图点的投影出现错误，在经过修改变换矩阵后得到如下：

这样是不是好看多了～～～。

四、地图创建2

在经过上述的步骤，就已经完成了基本的地图实时更新功能了，但是这个还没有对地图进行优化以及相应的回环检测部分，这个在后续会继续完善，但是在此之前，我还想尝试一下octomap地图的实时创建。。恩，想到就开始做，于是得到了如下的otomap地图：

OK， 顺利完成小型SLAM的编写测试。

五、心得体会

在完成上述系统的编写测试之后，才能清晰无误把握SLAM过程中的每个环节，这对于刚开始入门SLAM的同学来说非常重要。不仅如此对我自己而言理一理SLAM的整体过程也能收获很多～， 接下来将会重点放在后端优化部分，回环检测很重要呀。

而且，在过一遍SLAM过程之后，就开始从这几个重要部分开始进军深度学习在SLAM中的应用和提升啦，继续加油～～

参考文献

[1]. 深度相机原理