SLAM前端 - 相机运动估计 & 特征点位置估计

参考：
高翔——《视觉SLAM十四讲》第7讲 视觉里程计1
高翔——《视觉SLAM十四讲》系列讲解视频及PPT ch7_1 & ch7_2

---

• 目录：
  1 相机运动估计之 2D-2D: 对极几何
  2 相机运动估计之 3D-2D: PnP
  3 相机运动估计之 3D-3D: ICP
  4 特征点位置估计之 三角测量

• 综述（可能有错，需检查）
  （1）（仅单目相机初始化时服用）2D-2D，已知匹配好的若干特征点在两帧图像上的位置，得到估计的初始R、t；
  （2）（单目初始化时2D-2D之后立即服用）三角测量，已知匹配好的若干特征点在两帧图像上的位置，以及估计的R、t，得到估计的特征点空间位置P；
  （3）（单目三角测量后服用；以后每来一帧图像都服用）3D-2D，已知前一帧的特征点的空间位置P（之前的三角测量实际上给了单目相机深度信息，变成Depth不准确的RGBD），以及后一帧对应到前一帧特征点的特征点在该帧图像的位置，得到估计的R、t；
  （4）（双目或RGBD相机服用）3D-3D，已知匹配好的若干特征点在两帧图像对应的空间中的位置，得到估计的R、t。

---

1 相机运动估计之 2D-2D: 对极几何

1. 对极约束

• 对极几何中的几何关系认知：
  

  （1）O1、O2是光心，p1、p2是投影，e1、e2是极点，l1、l2是极线，PO1O2是极平面
  （2）约束关系：若p1已知、p2未知，可判断p2在极线l2上；反之亦然。

• 实践中对问题的分析：
  （1）条件：p1、p2已知，P未知
  （2）待求：T12

• 数学描述：
  

  基于“图1的p1经R、t到达图2的p2”的想法，经过一系列转换：
  
  得到简单的对极约束形式：
  
  定义E和F并进一步化简对极约束的表示：
  
  其中E为Essential本质矩阵，F为Fundamental基础矩阵。
  E和F差的是内参的关系。p和x差的也是内参的关系。
  对极约束本质上刻画的是O1O2P共面的关系。
• 进一步分析：
  E共有5个自由度（R有3个，t有3个；但是由于对极约束中，等式两遍同乘以一个非零常数后，照样成立，即E乘以一个非零常数后等式依然成立，故自由度减1），这就意味着至少可由5对点求得E，但是这样的五点法很麻烦（因为要用到什么非线性什么鬼的，反正就是麻烦）；而若当作普通矩阵，则有8个自由度（3*3矩阵9个自由度，原因同上再减1个）。
• 解决问题流程：
  （1）由匹配点计算E矩阵；
  （2）由E恢复R、t。

2. 求解E本质矩阵（然后使用E或F恢复R、t）：八点法+SVD

• 八点法的整体意思：
  

  E是(3 * 3)的，对极约束就是(1 * 3) * (3 * 3) * (3 * 1) = (1 * 1)的，可以把E转成向量形式的e进行计算，那么左边是1对点的形式，右边是8对点的形式，相当于是解Ax=0的矩阵，这样解得E。
  

• SVD解R、t：
  

  反正解出来之后，可能性有4种，但只有一个是合理的，这样就解完了。
  
• 八点法的讨论和分析：
  （1）只用于单目的初始化，后面就用PnP和ICP了。
  （2）尺度不确定性：E=t^ R，E乘以任意倍数仍可行，也就是t^ R乘以任意倍数仍可行，R是正交矩阵，本身有约束（？），而t这个平移向量乘以任意倍数，解仍可行。所以直观上，PO1O2这个三角（轨迹和地图），缩放任意倍，得到的观测值都是一样的，这就是单目的尺度不确定性。所以实践上，一般将t的模或特征点的平均深度设为1，不过求得的t跟真实的t差的倍数还是无从得知的。
  （3）对于纯旋转问题（t=0），由于E定义为t^ R，故此时E为0矩阵，那就没法分解E求得R、t了，所以实际上没有平移是没法单目初始化的，单目初始化时一定要带有平移。
  （4）多于8对点时，就成了算最小二乘解，或者是用RANSAC算法（好像上课学过，记得思路很神奇）。

3. 求解H单应矩阵（然后使用H恢复R、t）

• 什么时候用到单应矩阵：
  若场景中的特征点都落在同一平面上（比如墙、地面等），则可以通过单应矩阵来进行运动估计。这种情况在无人机携带的俯视相机、扫地机携带的顶式相机中比较常见。
• 好像跟用E时是差不多的，先不看了。

---

2 相机运动估计之 3D-2D: PnP

• 综述：
  （1）3D-2D的情况是已知n个3D空间点以及其投影位置的情况；
  （2）PnP，Perspective-n-Point；
  （3）具体方法分两类：代数方法（直接线性变换 DLT、用三对点估计位姿的P3P）；非线性优化方法（光束法平差 Bundle Adjustment），一般用优化的方法，因为代数方法对噪声不太鲁棒

2.1 直接线性变换 DLT

• 分析：

  
  
  
  展开后最后一行左边只有s，那就可以用这一行消去前两行中的s。则一个特征点提供两个方程：
  
  为了求解12个未知数，需要6对点。
• 讨论：
  （1）由于求解时将12个元素看成是独立的，而R要有正交矩阵的约束，所以就要用QR分解什么鬼的；
  （2）其他的先空着

2.2 P3P

* 本质上就是利用3对三角形的相似关系：

而根据余弦定理：

然后各种代入，得到一个关于x和y的二元二次方程组：

其中：

而解这个二元二次方程组需要用到吴消元法，求解析解。但解出的是4个解，再用一对验证点D、d计算最可能解。最终得到ABC三点在相机坐标系下的3D坐标。

2.3 Bundle Adjustment

参考：Bundle Adjustment---即最小化重投影误差（高翔slam---第七讲）

• 这个词很抽象的样子，先整体感受一下：
  （1）一般翻译成“光束法平差”，很久没有读通，现在觉得“平”是个动词，“平差”指摆平误差。依据什么去“平差”？是用“光束”的方法，而“光束法”本身并不是理解成一个现成的方法，“光束”只是指相机拍照时三维空间点投影到二维图像的连线集合。合起来，就是“利用三维到二维投影的光束，消除误差”。
  （2）光束法平差的本质是优化，目标函数是（min重投影误差）。
  （3）“第一次投影”指相机拍照时三维空间点投影到图像；“第二次投影 / 重投影”指计算得到的三维坐标点（非真实）投影到计算得到的相机位姿（非真实）。

---

3 相机运动估计之 3D-3D: ICP

• 综述：
  

  （1）首先，3D-3D的情况是双目相机或RGBD相机的情况；
  （2）要搞清楚，所谓“ICP（Iterative Closest Point 迭代最近点）”并不是指解决这类问题的方法。对于激光SLAM来说，由于激光中数据特征不够丰富，我们无从得知两个点集之间的匹配关系，只能认为距离最近的两个点为同一个，这是“迭代最近点”；而在视觉SLAM中，特征点提供了较好的匹配关系（其实就省了“ICP”），而ICP在这里指代匹配好的两组点间的运动估计问题（其实还是感觉很奇怪，ICP就理解成“两组点的特征匹配已经做好的这种状态下，去做运动估计”吧）；
  （3）具体方法分两种：线性代数求解（SVD）；非线性优化求解（类似Bundle Adjustment）。
  （4）注意，在3D-3D问题中，并没有出现相机模型，只与匹配好的两组点有关。

3.1 SVD

• 分析：
  

  （1）构建最小二乘问题，找使其最小的R、t：
  
  变换后，目标函数变成：
  

  左边只与R有关，而且与每个点的去质心坐标有关；右边与R、t有关，而且只与两个质心有关。
  

  （2）而左边部分变成了优化这个：
  
  在这个东西里用SVD求解R。
  （3）用右边部分就能直接求出t了：
  
• 真正的SVD部分：
  就不想看了。

---

4 特征点位置估计之 三角测量

• 综述：
  有了相机的运动估计，下一步需要通过三角测量来估计特征点的空间位置。

• 分析1：
  
  理论上O1p1与O2p2会交于一点P，然而由于噪声影响，往往无法相交，因此可以通过最小二乘法求解。按照对极几何：
  
  其中x1、x2为特征点在两个像平面成像的归一化坐标，s1、s2为特征点的深度。
  这两个深度是可以分开求的，例如算s2，则对上式两侧左乘x1^：
  

  其中左侧为0，右侧可以看做关于s2的方程。如此可分别求得s1和s2，即可求得P的空间坐标。

• 分析2：
  

  但是上面单独求解s1、s2的过程中，由于噪声的存在，导致估得的R、t不能使得s1x1=s2Rx2+t这个式子精确成立。所以更常见的方法是求最小二乘解而不是零解：
  
• 讨论：
  先空着。