3D Mapping with an RGB-D Camera（RGBD SLAM V2 ）论文笔记
这篇文章即是Felix Endres等人12年完成的RGB-D SLAM V2，是最早的为kinect风格传感器设计的SLAM系统之一
没看到有相关的论文解析
在Github上可找到开源代码，工程配置与运行参考http://www.cnblogs.com/voyagee/p/6898278.html
系统流程
系统分为前后端。前端就是视觉里程记。从每一帧的RGB图像提取特征，计算描述符，RANSAC+ICP计算两帧之间的motion estimation，
并提出了一个EMM（Environment Measurement Model）模型来判断estimate motion是否可接受。后端回环检测，基于g2o优化库的位姿图（pose graph）优化，
得到优化后的轨迹，用于建立地 图。建图采用八叉树地图octomap的方式。
特征提取
在源码中可选择 SIFT \ SURF \ORB 特征，其中SIFT要在GPU上运行，ORB和SURF都在CPU上用Opencv实现
不同特征比较：
（ 其中，ATE为Absolute Trajectory Error，轨迹的误差，就是系统估计的轨迹和真实轨迹（GroundTruth）之间的欧式距离，RMSE就是均方根）
可以看出，有GPU时SIFT综合表现最好。 综合实时性、硬件成本和准确率来看，ORB较好。
运动估计
三对特征快速计算RANSAC的初始值，在每一轮迭代中，最小二乘correspondences之间的马氏距离 。
马氏距离与欧式距离的差别：http://www.cnblogs.com/likai198981/p/3167928.html，简单说就是计算距离时考虑到了各项异性，多乘了一个协方差矩阵
EMM：Environment Measurement Model
一个传统的判断motion estimate是否可接受的方法就是看inlier的比例，小于设定阈值就reject motion estimate。
然而，motion blur(运动模糊)，缺少纹理信息的环境都很容易出现inlier较少的情况。
并且有一些点，在一帧中可以看到，另一帧可能就被其他点挡住了。 作者提出使用这个EMM来更鲁棒的判断是否reject estimate
先看一个假设：实施transformation之后，空间上对应的深度测量值应该来自于同一个表面位置之下:
after applying transformation estimate,spatially corresponding depth measurement stem from the same underlying surface location.
论文中作者证明了观测yi和yj（不同与下图的yi,yj）之间的差满足高斯分布，方差为表示噪音的协方差矩阵（计算方法由论文 Accuracy and Resolution of Kinect Depth Data给出）
这样，就可以用概率学里的p值检验来判断是否reject estimate，然而，发现p值检验 有点神经衰弱 ，对于微小的误差太过敏感，
因此使用另一种方法
将相机A观测到的点投影到相机B，在observed points中找出inlier,outlier,occluded：
上图中yi和yj应该是同一个点，算作inlier。yk不再A的视场内，所以被忽视，不看作“observed points”。
投影之后的yq在相机B 的市场中被yk挡住，看不到，因此算作occluded。
至于yp,落在了yq和相机A光心的连线上，算作outlier（注意，yp与yk不同，yp在相机A的视场内，但是相机A在这里观测到的是yq的深度）
因此，在上图中，inlier有俩，outlier一个，occluded一个
而在代码中（misc.cpp 913行之后有两个函数，其中一个是用于p值检验方法的），作者是这么计算inlier,outlier,occluded，并判断是否reject的
observedPoints=inliers + outliers + occluded
I=inlier数量，O=outlier数量，C=occluded数量，如果I/（I+O+C）<25%,直接reject
否则，对I/（I+O）设一个阈值，小于阈值就reject
回环检测
现在的回环检测大多数是基于Bag-of-Words，深度学习兴起后也有用deep learning 的，或者用语义信息
此文回环是基于 最小生成树 ，随机森林的随机回环
使用描述符之间的距离（直接相减），生成一棵深度图像（limited depth）的最小生成树，去掉n个时间上最近的（避免重复）
再随机从树上选k帧(偏向于更早的帧)来找闭环
图优化
只优化位姿图，不优化三维点。
使用g2o图优化库。g2o的使用http://www.cnblogs.com/gaoxiang12/p/3776107.html
没有被EMM拒绝的motion，将两帧相机位姿作为优化顶点，motion作为约束，加入优化边
检测到的回环，也加入优化顶点和边
g2o优化边的误差函数：
这里的xi,xj为优化变量（顶点位姿的estimate），Zij是约束，也就是xi和xj之间的变换。e（）是how well xi and xj satisfy the constraint ，
中间的Ω是约束（优化边）的信息矩阵（协方差矩阵的逆）， 表示对边的精度的估计。在代码中，是这么计算的：
odom_edge.informationMatrix = Eigen::Matrix<double,6,6>::Identity() / time_delta_sec;//e-9;
单位矩阵除以两帧之间的时间间隔
看到这里，依然没有明白这个误差究竟咋算的～
查看g2o::SE3edge.cpp, 这个e其实是这么算的：
_inverseMeasurement * from->estimate().inverse() * to->estimate()
解释一下这个误差函数：
我们认为 帧2（ from）的位姿态 Tj 是帧1（to）的位姿 Ti 经过 Tij 得到的 ，也就是
Ti * Tij = Tj
也就是
Tij = Ti -1 * Tj
即一个位姿的逆乘另一个位姿可以表示两个位姿之间的变化，那么，我们想表达边的measurement与Tij之间的差距，也可以这么玩儿
delta ： Δ = measurement-1 * Ti-1 * Tj ,
这就得到g2o代码里的式子了， 后面的toVectorMQT是啥我也不清楚。。应该就是矩阵转化为向量
注意，Ti其实完整写法是Twi, w代表世界坐标系
这里看不懂的可以参考高翔大大《视觉SLAM十四讲》11.1.2
这个样子的回环检测，难免会出现错误的回环，这种回环约束加入到图优化中去，势必会把地图拉的扭曲
所以，在图优化第一次收敛之后，作者就把错误回环对应的优化边 从图中删除。
怎么判断回环是错误的呢？ ----- 给上面说的误差函数设一个阈值（又又又又是阈值。。） ，误差大于这个阈值的就是错误的
octoMap:
http://www.cnblogs.com/gaoxiang12/p/5041142.html
把一个立方体平均分成八个小立方体，小立方体又可以再分，子子孙孙无穷尽也
每个小立方体对应八叉树的一个节点，因此成为八叉树地图
每个小立方体存储 被占据的概率的logist回归，被观测到占据次数越多，概率越大，而如果没有一次观测到被占据，则不用展开该节点
OctoMap的优点： 利于导航 ; 易于更新 ;存储方式比较省空间
欢迎交流，欢迎补充，欢迎纠错
