爱发呆de白菜头RobotSlamApplication项目二： 小型SLAM系统
研究背景： 因为之前比较浮躁，总是喜欢研究别人的库然后测试跑通，效果好就拿来修修改改、然后测试测试就用，效果不好就抛弃。自己没有潜心下来真正写过一个系统，这次从头到尾写一个SLAM系统，写的过程真的不容易，写完就感觉舒畅多了。。
一、前言
RGB-D 相机的优点和缺点（结构光深度相机）
优点:
- 适合在光照不足，缺乏纹理的场景中使用；
- 在一定范围内可以达到较高的测量精度；
- 技术成熟，深度图像可以做到较高的分辨率；
缺点:
- 室外环境中基本不能使用；
- 测量距离较近；
- 容易受到光滑平面反光的影响。
二、相机的位姿估计
基于ORB特征点法获取不同帧图像之间的变换（这个方法原理很多大牛都讲过，这里就不多说）， 主要涉及以下部分：
在实时获取图像数据的过程中，这一环节需要注意图像帧的获取，这里的帧包括当前帧和参考帧（也就是上一时刻的图像帧），当我们计算图像的匹配时，需要区分不同时刻的帧，以计算相邻帧之间图像变换。
即，在初始时刻，将当前帧赋给参考帧，然后在相机运动跟踪过程中，执行完每一次的图像匹配和跟踪之后，都要将当前帧进行转换之后赋给参考帧，以实现"帧"的更新。
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
{
.....
}
}
相机位姿的估算也有很多种方式：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]. 深度相机原理
RobotSlamApplication项目二： 小型SLAM系统
研究背景： 因为之前比较浮躁，总是喜欢研究别人的库然后测试跑通，效果好就拿来修修改改、然后测试测试就用，效果不好就抛弃。自己没有潜心下来真正写过一个系统，这次从头到尾写一个SLAM系统，写的过程真的不容易，写完就感觉舒畅多了。。
一、前言
RGB-D 相机的优点和缺点（结构光深度相机）
优点:
- 适合在光照不足，缺乏纹理的场景中使用；
- 在一定范围内可以达到较高的测量精度；
- 技术成熟，深度图像可以做到较高的分辨率；
缺点:
- 室外环境中基本不能使用；
- 测量距离较近；
- 容易受到光滑平面反光的影响。
二、相机的位姿估计
基于ORB特征点法获取不同帧图像之间的变换（这个方法原理很多大牛都讲过，这里就不多说）， 主要涉及以下部分：
在实时获取图像数据的过程中，这一环节需要注意图像帧的获取，这里的帧包括当前帧和参考帧（也就是上一时刻的图像帧），当我们计算图像的匹配时，需要区分不同时刻的帧，以计算相邻帧之间图像变换。
即，在初始时刻，将当前帧赋给参考帧，然后在相机运动跟踪过程中，执行完每一次的图像匹配和跟踪之后，都要将当前帧进行转换之后赋给参考帧，以实现"帧"的更新。
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
{
.....
}
}
相机位姿的估算也有很多种方式：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]. 深度相机原理