SLAM 是 Simultaneous Localization And Mapping 的英文首字母组合，一般翻译为：同时定位与建图、同时定位与地图构建。SLAM 是三维视觉的核心技术，广泛应用于 AR 、自动驾驶、智能机器人、无人机等前沿热门领域。可以说凡是具有一定行动能力的智能体都拥有某种形式的 SLAM 系统。
- 从零开始学习 SLAM
- References
计算机视觉可以分为两大方向：基于学习的方法和基于几何的方法。其中基于学习的方法最火的就是深度学习，而基于几何方法最火的就是视觉 SLAM。
没有 SLAM，虚拟现实将永远停留在座椅上。
- 相机相关：单目、双目、RGB-D 等相机的物理参数意义、相机成像模型、相机的标定、去畸变等。双目的话还涉及到视差计算，RGB-D 的话涉及到 RGB 和 depth 图像的对齐等。
- 图像处理相关：特征点描述子、特征点提取、特征点匹配、图像梯度计算、边缘检测、直线检测等。
- 多视角几何相关：对极约束、本质矩阵、单应矩阵、光流估计、三角化等。
定位、建图、路径规划。
SLAM 是指当某种移动设备（如机器人、无人机、手机等）从一个未知环境里的未知地点出发，在运动过程中通过传感器（如激光雷达、摄像头等）观测定位自身位置、姿态、运动轨迹，再根据自身位置进行增量式的地图构建，从而达到同时定位和地图构建的目的。
- 更方便的列表初始化
- C++11 前只有数组能使用初始化列表。而 C++11 后大部分类型都可以初始化列表。
- 省心省力的自动类型推导
- auto，不影响编译速度
- 简洁舒服的循环体
- for(auto n: arr)
- 简洁的 lambda 表达式
- [函数对象参数]（操作符重载函数参数）->返回值类型{函数体}
- 随心所欲可变长的参数模板
- std::make_tuple(), std::tie()
- 能非常方便的表达点在直线或平面上
- 方便表达直线与直线，平面与平面的交点
- 能够区分一个向量和一个点
- 能够表达无穷远
- 更简洁的表达欧氏空间变换
刚体，是指本身不会在运动过程中产生形变的物体，如相机的运动就是刚体运动，运动过程中同一个向量的长度和夹角都不会发生变化。刚体变换也称为欧式变换。
-
旋转矩阵
- 旋转矩阵不是一般矩阵，它有比较强的约束条件。旋转矩阵 R 具有正交性，R 和 R 的转置的乘积是单位阵，且行列式值为 1。
- 旋转矩阵 R 的逆矩阵表示了一个和 R 相反的旋转。
- 旋转矩阵 R 通常和平移向量 t 一起组成齐次的变换矩阵 T，描述了欧氏坐标变换。引入齐次坐标是为了可以方便的描述连续的欧氏变换。
- 冗余。用9个元素表示3个自由度的旋转，比较冗余。
-
四元数
- 四元数由一个实部和三个虚部组成，是一种非常紧凑、没有奇异的表达方式。
- 注意四元素定义中实部虚部和打印系数的顺序不同，很容易出错！
- 单位四元素才能描述旋转，所以四元素使用前必须归一化。
- /res/bonus_gimbal_lock.pdf
-
旋转向量
- 用一个旋转轴 n 和旋转角 θ 来描述一个旋转，所以也称轴角。不过很明显，因为旋转角度有一定的周期性（360°一圈），所以这种表达方式具有奇异性。
- 从旋转向量到旋转矩阵的转换过程称为罗德里格斯公式。
- 旋转向量和旋转矩阵的转换关系，其实对应于李代数和李群的映射。
-
欧拉角
- 把一次旋转分解成 3 次绕不同坐标轴的旋转，比如航空领域经常使用的“偏航-俯仰-滚转”（yaw，pitch，roll）就是一种欧拉角。该表达方式最大的优势就是直观。
- 欧拉角在 SLAM 中用的很少，原因是它的一个致命缺点：万向锁。会丢失一个自由度，引起奇异性。事实上，想要表达三维旋转，至少需要4个变量。
- /res/quaternion.pdf
-
Eigen 是一个 C++ 开源线性代数库。
旋转矩阵对加法不封闭。李代数就是解决这个问题的。把大写 SE(3) 空间的 T 映射为一种叫做李代数的东西，映射后的李代数叫做小 se(3)。它是由向量组成的，我们知道向量是对加法封闭的。这样我们就可以通过对李代数求导来间接的对变换矩阵求导了。
群（group）就是一种集合加上一种运算的代数结构。群有几个运算性质：封闭性，结合律，幺元，逆元（凤姐咬你）。
旋转矩阵和乘法构成了旋转矩阵群（SO(3)，特殊正交群)，变换矩阵和乘法构成了变换矩阵群（SE(3)，特殊欧式群)。
李群的定义是指连续光滑的群，
李代数对应李群的正切空间，它描述了李群局部的导数。
旋转矩阵的微分是一个反对称矩阵左乘它本身。对于某个时刻的 R(t)（李群空间），存在一个三维向量 φ=（φ1，φ2，φ3）（李代数空间），用来描述 R 在 t 时刻的局部的导数。
'.jpg)
三维反对称矩阵只有3个自由度，这样就可以把一个三维向量和一个三维矩阵建立对应关系。
这个 φ 正是李群大 SO(3) 对应的李代数小 so(3)。
小 so(3) 的李代数空间就是由旋转向量组成的的空间，其物体意义就是旋转向量。而上面的指数映射关系就是罗德里格斯公式，他们在数学上本质是一样的。
旋转矩阵的导数可以由其对应的旋转向量指定，指导如何在旋转矩阵中进行微积分运算。
李代数求导分两种：一种是用李代数表示位姿，然后根据李代数加法来对李代数求导。结果中有复杂的雅克比公式，不是很方便。一般都用第二种，对李群进行左乘或者右乘微小的扰动，然后对该扰动求导。
把倒立的实像去掉，用正立的虚像代替。三维空间的点大 P 在成像平面上成的像就是小 p。大 P，小 p 还有相机中心 C 在同一条直线上。
- 世界坐标系(world coordinate system)：是用户定义的三维世界的坐标系，以某个点为原点，为了描述目标物在真实世界里的位置而被引入。单位为m。
- 相机坐标系(camera coordinate system)：是以相机为原点建立的坐标系，为了从相机的角度描述物体位置而定义，作为沟通世界坐标系和图像/像素坐标系的中间一环。单位为m。
- 图像坐标系(image coordinate system)：是为了描述成像过程中物体从相机坐标系到图像坐标系的投影透射关系而引入，是我们真正从相机内读取到的图像所在的坐标系。单位为像素。
其中 fx, fy 分别是 x, y 方向焦距，一般都是相等的，cx, cy 是光心位置，一般是长和宽的一半，都是内参，此外还有畸变系数也属于内参，他们都是相机固有参数。
畸变产生的原因是：透镜不能完全满足针孔模型假设。
相机透镜的畸变主要分为径向畸变和切向畸变，还有其他的畸变，但都没有径向和切向畸变影响显著，所以在这里只考虑径向和切向畸变。
径向畸变是由于相机透镜的形状导致的，且越向透镜边缘移动径向畸变越严重。分为桶形和枕形。
切向畸变是由于透镜和 CMOS 或者 CCD 的安装位置误差导致。如果存在切向畸变，一个矩形被投影到成像平面上时，很可能会变成一个梯形。不过随着相机制造工艺的大大提升，这种情况很少出现了，所以一般也不考虑切向的畸变。
C0-C1-P 组成的平面称为极平面（epipolar plane），和成像平面相交的直线称之为极线（epipolar line），两个光心 C0, C1 和成像平面的交点叫做极点（epipole）。
第一个优点就是可以表达物体的空间轮廓和具体位置。第二个优点就是，点云本身和视角无关，也就是你可以任意旋转，可以从不同角度和方向观察一个点云，而且不同的点云只要在同一个坐标系下就可以直接融合。
第一个缺点就是点云并不是稠密的表达，一般比较稀疏，在空间很多位置其实没有点云，这部分的信息是缺失的。第二个缺点就是点云的分辨率和离相机的距离有关。离近了看是看不清是什么的，只能拉的很远才能看个大概。
滤波原因：
- 点云数据密度不规则需要平滑
- 因为遮挡等问题造成离群点需要去除
- 大量数据需要下采样
- 噪声数据需要去除
滤波方案：
- 按照给定的规则限制过滤去除点
- 通过常用滤波算法修改点的部分属性
- 对数据进行下采样
pcl::ApproximateVoxelGrid< PointT >对输入的点云数据创建一个三维体素栅格，每个体素内用体素中所有点的重心来近似显示体素中其他点，这样该体素内所有点都用一个重心点最终表示。它的优点是可以在下采样的时候保存点云的形状特征。
离群点会使局部点云特征(如表面法线或曲率变化)的估计复杂化，从而导致错误的值，从而可能导致点云配准失败。而且这些离群点还会随着积累进行传导。
pcl::StatisticalOutlierRemoval< pcl::PointXYZ >对每个点的邻域进行统计分析，剔除不符合一定标准的邻域点。具体来说，对于每个点，我们计算它到所有相邻点的平均距离。假设得到的分布是高斯分布，可以计算出一个均值 μ 和一个标准差 σ，那么这个邻域点集中所有点与其邻域距离大于 μ + std_mul * σ 区间之外的点都可以被视为离群点，并可从点云数据中去除。std_mul 是标准差倍数的一个阈值。
pcl::RadiusOutlierRemoval< pcl::PointXYZ > 根据空间点半径范围临近点数量来滤波。在点云数据中，设定每个点一定半径范围内周围至少有足够多的近邻，不满足就会被删除。
用 RGB-D，激光扫描仪等设备扫描物体，尤其是比较小的物体时，往往会有测量误差。这些误差所造成的不规则数据如果直接拿来曲面重建的话，会使得重建的曲面不光滑或者有漏洞，而且这种不规则数据很难用前面提到过的统计分析等滤波方法消除，所以为了建立光滑完整的模型必须对物体表面进行平滑处理和漏洞修复。
点云重采样，实际上是通过“移动最小二乘”（MLS， Moving Least Squares ）法来实现的。
pcl::MovingLeastSquares< PointT, PointT >法线的用处：尤其是在三维建模中应用非常广泛，比如在计算机图形学（computer graphics）领域里，法线决定着曲面与光源（light source）的强弱处理（Flat Shading），对于每个点光源位置，其亮度取决于曲面法线的方向。
点云的法线计算一般有两种方法：
- 使用曲面重建方法，从点云数据中得到采样点对应的曲面，然后再用曲面模型计算其表面的法线；
- 直接使用近似值直接从点云数据集推断出曲面法线。
// 直接使用近似值直接从点云数据集推断出曲面法线
pcl::NormalEstimation< PointT, pcl::Normal > 网格主要用于计算机图形学中，有三角、四角网格等很多种。不过，计算机图形学中的网格处理绝大部分都是基于三角网格的。
三角形表示网格也叫三角剖分。它有如下几个优点：
- 稳定性强。
- 三角网格比较简单（主要原因），实际上三角网格是最简单的网格类型之一，可以非常方便并且快速生成，在非结构化网格中最常见。而且相对于一般多边形网格，许多操作对三角网格更容易。
- 有助于恢复模型的表面细节。
三角网格需要存储三类信息：
- 顶点。每个三角形都有三个顶点，各顶点都有可能和其他三角形共享。
- 边。连接两个顶点的边，每个三角形有三条边。
- 面。每个三角形对应一个面，我们可以用顶点或边列表表示面。
点云面临的问题：
- 点云噪声。 每个点云都会带有噪声，噪声有可能和物体表面光学性质、物体深度、传感器性能等都有关系。
- 点云匹配误差。三维重建中需要将不同帧得到的点云估计其在世界坐标系下的位姿，会引入一定的位置误差。
- 点云分布。分布的不均匀性体现在两个方面。一个是每个点云在不同的方向上分布是不均匀的另一个是不同的点云匹配后，不同位置的点云密度是不一样的。
- 缺失数据。 扫描中如果碰到不易成像的部位（比如不可见、反光等等），那么这部分的数据是缺失的，点云是不完整的。
网格生成算法的要求：
- 对点云噪声有一定的冗余度。
- 能够重建出曲率变化比较大的曲面。
- 能够处理大数据量，算法时间和空间复杂度不会太高。
- 重建出的网格中包含尽可能少的异常三角片，比如三角片交错在一起、表面法向量不连续或不一致、同一个位置附近出现多层三角片等。
常用网格生成算法：
- 插值法。重建的曲面都是通过原始的数据点得到的。
- 逼近法。用分片线性曲面或其他曲面来逼近原始数据点，得到的重建曲面是原始点集的一个逼近。
pcl::GreedyProjectionTriangulation< pcl::PointNormal >该算法的优点是可以用来处理来自一个或者多个设备扫描到得到、并且有多个连接处的散乱点云。但是也是有很大的局限性，它更适用于采样点云来自表面连续光滑的曲面，并且点云的密度变化比较均匀的情况。所以之前需要进行点云滤波和平滑。
贪心投影三角化的流程的：
- 先将点云通过法线投影到某一二维坐标平面内。
- 然后对投影得到的点云做平面内的三角化，从而得到各点的拓扑连接关系。平面三角化的过程中用到了基于 Delaunay 三角剖分的空间区域增长算法。
- 最后根据平面内投影点的拓扑连接关系确定各原始三维点间的拓扑连接，所得三角网格即为重建得到的曲面模型。
Delaunay 三角剖分的有两个优点：
- 最大化最小角，“最接近于规则化的“的三角网。
- 唯一性（任意四点不能共圆）。
Delaunay 条件：所有三角形的顶点都不在其他三角形的外接圆内。
Delaunay 三角剖分：它通过选取一个样本三角片作为初始曲面，不断扩张延伸曲面的边界，直到所有符合几何正确性和拓扑正确性的点都被连上，最后形成一张完整的三角网格曲面。
SLAM 的后端一般分为两种处理方法，一种是以扩展卡尔曼滤波（EKF）为代表的滤波方法，一种是以图优化为代表的非线性优化方法。目前 SLAM 研究的主流热点几乎都是基于图优化的。
代表性的就是2009年，《SBA：A software package for generic sparse bundle adjustment》，基于图优化的视觉 SLAM 可以实时了。
图优化里的图就是数据结构里的图，一个图由若干个顶点（vertex），以及连接这些顶点的边（edge）组成。
在 SLAM 里，图优化一般分解为两个任务：
- 构建图。相机位姿作为顶点，位姿间关系作为边。
- 优化图。调整相机的位姿（顶点）来尽量满足边的约束，使得误差最小。
ICP 全称 Iterative Closest Point，就是迭代最近点。ICP在点云配准领域应用的非常广泛，因此基于深度相机、激光雷达的算法使用ICP的频率比较高。
ICP 算法流程：
- 首先对于一幅点云中的每个点，在另一幅点云中计算匹配点（最近点）。
- 极小化匹配点间的匹配误差，计算位姿。
- 然后将计算的位姿作用于点云。
- 再重新计算匹配点。
- 如此迭代，直到迭代次数达到阈值，或者极小化的能量函数变化量小于设定阈值。
- 线性插值
- 球面线性插值
- 预处理
- 传感器数据预处理。
- 前端
- 又称为视觉里程计（visual odometry，简称VO）。
- 主要是研究如何根据相邻帧图像定量估算帧间相机的运动。
- 只计算相邻帧的运动，进行局部估计，这会不可避免的出现累积漂移，这是因为每次估计两个图像间的运动时都有一定的误差，经过相邻帧多次传递，前面的误差会逐渐累积，轨迹漂移（drift）的越来越厉害。
- 解决轨迹漂移的方法有两个：后端优化、回环检测。
- 后端
- 对前端的结果进行优化，得到最优的位姿估计。
- 主要有两种方法：基于滤波理论的优化（EKF（扩展卡尔曼滤波））；非线性优化（图优化）。
- 回环检测
- 主要目的是让相机能够认识自己曾经去过的地方，从而解决位置随时间漂移的问题。
- 当回环检测成功后，就会建立现在的图像和过去曾经见过图像的对应关系，后端优化算法可以根据这些信息来重新调整轨迹和地图，从而最大限度地消除累积误差。
- 建立地图
VO vs. Structure from Motion（SfM）：
视觉里程计概念来自车轮里程计。视觉里程计是通过移动物体（如车辆、人、机器人）上搭载的单个或多个相机拍摄的连续图像作为输入，从而增量式地估计物体自身的运动状态。
SfM 中文译为运动恢复结构。是指从一组相机图像（有序或者无序都可以）中恢复相机位置姿态和重建三维结构的一种通用方法。最终结构和相机位姿通过离线优化算法来进一步提升效果，它的计算时间随图像的数量增加而增长。SfM 的一个比较著名的项目就是《Building Rome in a Day》：该项目利用网上搜集到的以罗马为关键词的200万张图片，经过一天的计算时间实现了对罗马的三维重建。
根据 VO 是否需要提取特征点，VO 的具体实现方法可以分为基于特征点法的 VO 和直接法 VO。特征点法 VO 是以提取图像中的特征点为基础，学术界有长久的研究，运行比较稳定、对光照变化和动态场景鲁棒性强，是比较成熟的 VO 实现方案。而直接法 VO 是为了克服特征点法 VO 的部分缺点（如计算量大，不适用于纹理缺乏场景等）而出现的。
- 环境的光照强度比较合适，不能太暗或者太亮。
- 环境具有相对丰富的纹理。
- 需要捕捉连续的图像，保证相邻图像帧的内容有足够的重叠区域。
- 最好是静态场景下运行。
- 特征点检测
- 特征点的选择和设计非常重要。优秀的特征点应该具有以下特征：
- 高辨识度。能够和周围的像素进行明显的区分，具有极强的代表性。
- 可重复性强。相同的特征点可以在下一幅图像中被再次检测到，这样才能进行特征点匹配。
- 高鲁棒性。在光照变化、几何变化（旋转、缩放，透视变形）时能够保持没有明显变化，在图像中存在噪声，压缩损伤，模糊等情况下仍然能够检测到。
- 计算效率高。
- 常用特征点：SIFT，FAST，ORB
- 特征点的选择和设计非常重要。优秀的特征点应该具有以下特征：
- 特征匹配
- 暴力匹配，握手匹配，快速近似最近邻匹配（FLANN）
- 运动估计
- 2D-2D：对极约束(极线约束)
- 3D-3D：迭代最近点（iterative closest point, ICP）
- 3D-2D：PnP(perspective-n-point)
- 离群点移除
- 最常用的离群点去除方法就是随机采样一致算法（Random Sample Consensus，RANSAC）
- Bundle Adjustment 优化
- BA 的本质是一个优化模型，其目的是最小化重投影误差。
相机标定的目的是：建立相机成像几何模型并矫正透镜畸变。
- 世界坐标系(world coordinate system)：用户定义的三维世界的坐标系，为了描述目标物在真实世界里的位置而被引入。单位为m。
- 相机坐标系(camera coordinate system)：在相机上建立的坐标系，为了从相机的角度描述物体位置而定义，作为沟通世界坐标系和图像/像素坐标系的中间一环。单位为m。
- 图像坐标系(image coordinate system)：为了描述成像过程中物体从相机坐标系到图像坐标系的投影透射关系而引入，方便进一步得到像素坐标系下的坐标。 单位为m。
- 像素坐标系(pixel coordinate system)：为了描述物体成像后的像点在数字图像上（相片）的坐标而引入，是我们真正从相机内读取到的信息所在的坐标系。单位为个（像素数目）。
下图可以更清晰地表达这四个坐标系之间的关系：
世界坐标系：Xw、Yw、Zw。相机坐标系： Xc、Yc、Zc。图像坐标系：x、y。像素坐标系：u、v。
- 从世界坐标系到相机坐标系
- 从相机坐标系到理想图像坐标系（不考虑畸变）
- 从理想图像坐标系到实际图像坐标系（考虑畸变）
- 从实际图像坐标系到像素坐标系
总结从世界坐标系到像素坐标系（不考虑畸变）的转换关系：
单应性（Homography）变换。可以简单的理解为它用来描述物体在世界坐标系和像素坐标系之间的位置映射关系。对应的变换矩阵称为单应性矩阵。
单应性在计算机视觉中的应用：
- 图像校正
- 视角变换
- 图像拼接
- 增强显示（AR)
单应矩阵有8个自由度，至少需要4个点对来确定。在真实的应用场景中，我们计算的点对中都会包含噪声，为了使得计算更精确，一般都会使用远大于4 个点对来计算单应矩阵。另外采用直接线性解法通常很难得到最优解，所以实际使用中一般会用其他优化方法，如奇异值分解、Levenberg-Marquarat（LM）算法等进行求解。
OpenCV 中有现成的函数可以调用，用来计算单应矩阵：Mat findHomography(InputArray srcPoints, InputArray dstPoints, int method=0, double ransacReprojThreshold=3, OutputArray mask=noArray() )
H是内参矩阵和外参矩阵的混合体，其中，u、v表示像素坐标系中的坐标，s表示尺度因子，fx、fy、u0、v0、γ（由于制造误差产生的两个坐标轴偏斜参数，通常很小）表示5个相机内参，R,t表示相机外参。
求解内参和外参的求解思路是利用旋转向量的约束关系，求解过程略。
因为旋转向量在构造中是相互正交的，即r1和r2相互正交，由此我们就可以利用“正交”的两个含义，得出每个单应矩阵提供的两个约束条件：
- 约束条件1：旋转向量点积为0（两垂直平面上的旋转向量互相垂直）；
- 约束条件2：旋转向量长度相等（旋转不改变尺度）。
从本质上去看，牛顿法是二阶收敛，梯度下降是一阶收敛，所以牛顿法收敛更快。比如你想找一条最短的路径走到一个盆地的最底部，梯度下降法每次只从你当前所处位置选一个坡度最大的方向走一步，牛顿法在选择方向时，不仅会考虑坡度是否够大，还会考虑你走了一步之后，坡度是否会变得更大。所以，可以说牛顿法比梯度下降法看得更远一点，能更快地走到最底部。也就是说梯度下降法更贪心，只考虑了眼前，反而容易走出锯齿形状走了弯路；而牛顿法目光更加长远，所以少走弯路。
高斯牛顿（Gauss-Newton）法是对牛顿法的一种改进，它用雅克比矩阵的乘积近似代替牛顿法中的二阶 Hessian 矩阵，从而省略了求二阶 Hessian 矩阵的计算。
Levenberg-Marquardt（LM）法在一定程度上修正了高斯牛顿法的缺点，因此它比高斯牛顿法更加鲁棒，不过这是以牺牲一定的收敛速度为代价的--它的收敛速度比高斯牛顿法慢。LM 采用的搜索方法是信赖域（Trust Region）方法，高斯牛顿法中采用近似二阶泰勒函数只在展开点附近有较好的近似效果，如果步长太大近似就不准确，因此应该给步长加个信赖区域，在信赖区域里，我们认为近似是有效的，出了这个区域，近似会出问题。LM算法可以一定程度避免系数矩阵的非奇异和病态问题，可以提供更鲁棒、更准确的步长。因此LM算法在相机标定、视觉SLAM等领域中应用非常广泛。