目录

基本情况

摘要

介绍

系统流程

特征提取

运动估计

EMM：Environment Measurement Model

回环检测

图优化

建图OctoMap

参考

---

基本情况

• 出处：Endres F, Hess J, Sturm J, et al. 3-D mapping with an RGB-D camera[J]. IEEE transactions on robotics, 2013, 30(1): 177-187.(其他名称:RGBD SLAM V2 ）
• 这篇文章是Felix Endres等人12年完成RGB-D SLAM V是最早的kinect风格传感器设计SLAM系统之一

摘要

本文介绍了一种新颖的建图系统，它使用RGB-D相机可靠地生成高度精确的3D地图。 我们的方法不需要任何进一步的传感器或里程表。 凭借Microsoft Kinect等低成本轻质RGB-D传感器，我们的方法适用于真空清洁器、四向直升机等小型家用机器人。 此外，我们的系统还可用于详细3D自由手动重建模型。 除系统本身外，我们还对公开可用的基准数据集进行了彻底的实验评估。 我们分析和讨论了特征描述符的选择、视觉特征的数量和验证方法等几个参数的影响。 实验结果表明，我们的系统可以稳定地处理具有挑战性的场景，例如快速相机运动和功能差异在线操作速度足够快的环境。 我们的系统作为开源完全可用，已被机器人社区广泛使用。

介绍

同时，定位和映射问题(SLAM)这是过去十年机器人界最积极的研究问题之一。 机器人工作区地图的可用性是独立执行定位、路径规划和导航等多项任务的重要要求。 特别是对于在复杂和动态环境中工作的移动机器人，如在工厂地板或医院执行运输任务，只能使用车载传感器快速生成（和维护）其工作空间的3D地图。 例如，操作机器人需要详细的工作空间模型，用于无碰撞运动规划，空中车辆需要详细的定位和导航地图。 虽然以前很多3D映射方法依赖于昂贵而重的激光扫描仪，但基于结构光RGB-D商业推出相机提供了吸引人的强大替代品。

我们描述了这项工作第一批利用RGB-D相机提供的密集的颜色和深度图像RGB-D SLAM系统之一。 与以前的工作相比，我们引入了几个扩展，进一步提高鲁棒性和准确性。 特别是，

• 提出使用环境测量模型(EMM)来验证最接近点的特征对应和迭代(ICP)算法估计的变换(相机位置)。

试验结果表明，在长轨迹和具有挑战性的环境中准确跟踪机器人的姿势。 为了允许其他研究人员使用我们的软件，复制结果并改进它，我们根据开源许可证发布了系统。 在线可以获得代码和详细的安装说明。

系统流程

系统分为前端和后端。视觉里程记录是前端。从每一帧的RGB图像提取特征，计算描述符，RANSAC ICP计算两帧之间的motion estimation，并提出了一个EMM（Environment Measurement Model）模型来判断estimate motion是否可接受。基于后端回环检测g2o优化库位置图（pose graph）用于建立地图的优化和优化轨迹。八叉树地图用于建图octomap的方式。

特征提取

可选择源码 SIFT \ SURF \ORB 特征，其中SIFT要在GPU上运行，ORB和SURF都在CPU上用Opencv实现

不同特征的error与耗时比较：

有GPU时SIFT综合表现最好。 在实时性、硬件成本和准确性方面，ORB较好。

运动估计

快速计算三对特征RANSAC的初始值，在每一轮迭代中，最小二乘correspondences马氏距离 。

马氏距离和欧式距离的区别，简单来说就是计算距离时考虑异性，乘以协方差矩阵

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值检查 有点神经衰弱 ，对小误差过于敏感，

所以用另一种方法

蓝色是Cam A红色是观测到的点Cam B观察点。将相机A观测到的点投影到相机B，在observed points中找出inlier,outlier,occluded：

1. 上图中yi和yj应该是同一点，算作inlier。yk不再是A的视场，所以被忽视，不被视为observed points”。

2. 投影之后的yq在相机B 的视角中被yk挡住，看不到，因此算作occluded。

3. 至于yp,落在了yq与相机A光心连接，算作outlier（注意，yp与yk不同，yp在相机A的视野中，但在这里观察到的是相机Ayq的深度）

而在代码中（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-Wors, 此文回环是基于 最小生成树 ，随机森林的随机回环：

使用描述符之间的距离（直接相减），生成一棵深度图像（limited depth）的最小生成树，去掉n个时间上最近的（避免重复），再随机从树上选k帧(偏向于更早的帧)来找闭环

图优化

只优化位姿图，不优化三维点。

使用g2o图优化库

没有被EMM拒绝的motion，将两帧相机位姿作为优化顶点，motion作为约束，加入优化边

检测到的回环，也加入优化顶点和边

g2o优化边的误差函数：

xi,xj为优化变量（顶点位姿的estimate），Zij是约束，也就是xi和xj之间的变换。e 是how well xi and xj satisfy the constraint ，中间的Ω是约束（优化边）的信息矩阵（协方差矩阵的逆）， 表示对边的精度的估计。

其中信息矩阵具体在代码中，是这么计算的：

odom_edge.informationMatrix = Eigen::Matrix::Identity() / time_delta_sec;//e-9;

即单位矩阵除以两帧之间的时间间隔

具体这个误差项 e 是如何构建的呢 ？在g2o::SE3edge.cpp中这么写：

解释一下这个误差函数：

我们认为 帧2（ from）的位姿态 Tj 是帧1（to）的位姿 Ti 经过 Tij 得到的 ，也就是

Ti * Tij = Tj

也就是

Tij = Ti -1 * Tj

即一个位姿的逆乘另一个位姿可以表示两个位姿之间的变化，那么，我们想表达边的measurement与Tij之间的差距，也可以这么玩儿

delta ： Δ = measurement-1 * Ti-1 * Tj

注意，Ti其实完整写法是Twi, w代表世界坐标系

这个样子的回环检测，难免会出现错误的回环，这种回环约束加入到图优化中去，势必会把地图拉的扭曲。所以，在图优化第一次收敛之后，作者就把错误回环对应的优化边 从图中删除。

怎么判断回环是错误的呢？ ----- 给上面说的误差函数设一个阈值（又是阈值，不太靠谱） ，误差大于这个阈值的就是错误的

建图OctoMap

SLAM拾萃(1)：octomap - 半闲居士 - 博客园​www.cnblogs.com/gaoxiang12/p/5041142.html正在上传…重新上传取消

把一个立方体平均分成八个小立方体，小立方体又可以再分，子子孙孙无穷尽也

每个小立方体对应八叉树的一个节点，因此成为八叉树地图

每个小立方体存储 被占据的概率的logist回归，被观测到占据次数越多，概率越大，而如果没有一次观测到被占据，则不用展开该节点

OctoMap的优点： 利于导航 ; 易于更新 ;存储方式比较省空间

参考

3D Mapping with an RGB-D Camera（RGBD SLAM V2 ）浅谈

SLAM论文粗译：3D Mapping with an RGB-D Camera