再次总结以下，若已经有匹配好的点对，要根据点对估计相机的运动，可以分为以下三种情况：

1. 2D-2D：即点对都是2D点，比如单目相机匹配到的点对。我们可以用对极几何来估计相机的运动。在估计完相机运动之后，我们还可以用三角测量（Triangulation）来估计特征点的空间位置（包括深度）。注意，是估计相机之间的运动。
2. 3D-2D：点对一组为3D，一组为2D，可以通过PnP求解。注意，是估计相机的位姿。
3. 3D-3D：通过双目或RGB-D或者某种方式得到了空间点的深度，即得到两组3D点，常用ICP解决。注意，得到的是相机之间的运动。

1. 2D-2D：对极约束

对极约束求解的是两个相机坐标系之间的运动。

1. 设点在第一个相机坐标系下的坐标
2. 构建该坐标与像素1的关系
3. 构建该坐标经过相机运动之后即R,t与像素2的关系
4. 取两个像素对应的归一化平面坐标
5. 归一化平面坐标关系式代入上式
6. 关系式左乘，关系式一边为0
7. 此时尺度意义下相等的符号可以直接变为等号，对极约束成功推出

2. 三角测量

之前使用了对极几何约束估计了相机运动，现在可以通过三角测量估计地图点的深度。

三角测量是指，通过不同位置对同一路标点进行观察，从观察到的位置推断路标点的距离。三角测量最早由高斯提出并应用于测量学中。

我们现在已知R，t，即两帧之间的运动，则知道。如果我们相求s1s2，从几何上看，我们可以在射线O1P1或者O2P2上找，或者两条射线的中间找。如果我们要求s1，就左乘一个，得：

然后根据已知信息即可直接求得。但是由于Rt本身具有误差，所以更常见的做法是求最小二乘解而不是直接求解。

讨论：

• 三角测量是由平移得到的，有平移才会有对极几何中的三角形，才谈得上三角测量，因此，纯旋转是无法使用三角测量的，因为在平移为0时，对极约束一直为0
• 增大平移，可能会导致失效，而平移太小，则三角化精度不够，这就是三角化的矛盾。我们把这个问题称为“视差”（parallax）
• 在单目视觉中，由于单目图像没有深度信息，我们要等待特征点北追踪几帧之后，产生了足够的视角，再通过三角化来确定新特征点的深度值。这有时也被称为延迟三角化。
• 本节只介绍了三角化的深度估计，但只要我们愿意，也能够定量地计算每个特征点的位置以及不确定性。所以，如果假设特征点服从高斯分布，并且不断地对它进行观测，在信息正确的情况下，我们就能够期望它的方差不断减小乃至收敛。这就得到了一个滤波器，称为深度滤波器（Depth Filter）。

3. 3D-2D：PnP

PnP（Perspective-n-Point）是求解3D到2D点对运动的方法。它描述了当知道n个3D空间点及其投影位置时，如何估计相机的位姿。注意是相机的位姿，而不是相机两帧之间的运动。

如果两张图像中的一张的特征点的3D位置已知，则最少只需要3个点对（以及额外的一个用于验证结果）就可以估计相机运动（位姿）。在双目或RGBD的视觉里程计中，我们可以直接使用PnP估计相机运动。而在单目视觉里程计中，必须先进行初始化，才能使用PnP。3D-2D的PnP不需要对极约束，又可以在很少的匹配点中获得较好的运动估计，是一种最重要的姿态估计方法。

PnP求解方法：

1. 使用3对点估计位姿的P3P
2. 直接线性变换（DLT）
3. EPnP、UPnP
4. 非线性优化方式，即构建最小二乘问题迭代求解，也就是万金油式的光束法平差（Bundle Adjustment，BA）
   

   3.1 直接线性变换DLT

   DLT求解的是相机的位姿。直接看式子：
   ，然后用最后一行把s消掉，得到u1和v1的关系式。在使得关系式一侧为0，这样就构建类似于其次方程，然后再求解方程。这里的x是R|t展开后的12维向量，因此只需要6对匹配点便能求解。当匹配点大于6对时，也可以使用SVD方法对超定方程求最小二乘解。

在DLT求解中，我们直接将T矩阵看成了12个未知数，忽略了它们之间的联系。因为R属于特征正交群，用DLT求出的解不一定满足该约束。所以我们必须针对DLT估计的R的矩阵，寻找一个最好的旋转矩阵对它进行近似。这可以由QR分解完成。

3.2 P3P

P3P利用给定的3个点的几何关系，即利用了三角形相似性质，进行PNP的求解。此外，P3P还需要使用一对验证点，以从可能的解中选出正确的那一个（类似于对极几何情形）。图像中的2D点a,b,c和世界坐标系的3D点A,B,C。P3P是求解3D点在相机坐标系下的坐标，一旦3D点在相机坐标系下的坐标能够算出，我们就得到了3D-3D的对应点，把PnP转换为了ICP问题。
理解P3P的求解流程：

1. 利用三角形对应关系和余弦定理构造OAB和Oab的关系。余弦定理：。一共构造三条
   
2. 上面三条式子同时除以，记
3. 记
4. 可以把第一个式子的v放到等式一边，然后带入其余两个式子得两个新式子
5. 然后就得到了要求解的式子。注意x,y是未知量，随着相机的移动会发生改变。u,v可以通过ABC在世界坐标系下的坐标算出来，变换到相机坐标系下之后，这个比值并不改变。
6. 利用吴消元法，解出该方程，最多可能得到4个解，然后用验证点来计算最可能的解，最后得到了3个点的相机坐标系坐标，将P3P问题转化为ICP问题，然后再计算相机运动的R,t

带有匹配信息的3D-3D位姿求解非常容易，但是P3P存在一些问题：

1. P3P只利用了3个点的信息，当给定的点的配对点多于3组时，难以利用更多的信息。
2. 如果3D点或者2D点受噪声印象，或者存在误匹配，则算法失效。

   3.3 最小化投影误差求解PnP

   最小化投影误差直接求解相机位姿和空间点。前面说的线性方法，往往是先求相机位姿，再求空间点位置，而非线性优化则是把它们都看成优化变量，放在一起优化。这是一种非常通用的求解方式，我们可以用它对PnP或ICP给出的结果进行优化。这一类把相机和三维点放在一起进行最小化的问题，统称为Bundle Adjustment（光束法平差）。

最小化投影误差流程：

1. 世界坐标与像素坐标的关系式，这里的s就是深度
2. 构建误差项，用最小二乘法求解。也可使用李代数，构建无约束的优化问题，然后通过高斯牛顿法和列文伯格——马夸尔特方法求解。这时我们需要知道每个误差项关于优化变量的导数：
   
   类似于BCH，是一阶导数矩阵，是2*6的矩阵。
3. 知道空间点相机坐标系和世界坐标系的转换关系，变换矩阵T
4. 相机坐标系与uv的转换关系，K
5. 对T左乘扰动量，考虑e关于扰动量的导数，利用链式法则，求解e关于相机坐标系的导数（矩阵关于向量求导），再求解相机坐标系关于扰动的导数（其实就是SE(3)上的李代数求导）
6. 两项相乘，就得到了2*6的雅可比矩阵，即误差e关于扰动量的一阶导数。这个雅可比矩阵描述了重投影误差e关于相机位子T李代数的一阶变化关系

以上是优化位姿的。我们还希望优化特征点的空间位置：

1. 讨论e关于空间点P的导数
2. 链式法则:，第一个是矩阵关于向量求导，第二个求导完只剩旋转矩阵R
3. 得到关于特征点的导数矩阵

4. 3D-3D：ICP

迭代最近点（Iterative Closest Point，ICP），是根据一组配对好的3D点，比如两幅RGBD图像的匹配，求解两组点之间的变换关系，也就是相机的运动，跟相机模型没有关系K。和PNP类似，ICP的求解也分为两种方式：利用线性代数的求解（主要是SVD奇异值分界），以及利用非线性优化方式的求解（类似于BA）

4.1 SVD方法

1. 构建误差项
2. 构建最小二乘问题，求使得误差平方和达到最小R,t
3. 找到两组点的质心
4. 利用质心在误差函数中做处理，简化误差函数。原理是所有点的和减去质心等于0
5. 得到只有两项的误差函数，第一项只与R有关，第二项与Rt都有关，所以先求第一项，再令第二项为0求解t

求R的时候利用了SVD分解，其证明过程较为复杂，总之是定义了一个矩阵，对其进行奇异值分解，当W满秩时，

4.2 非线性优化方法

1. 对第二组点的相机坐标系进行左乘扰动
2. 做第一组点相机坐标系和第二组点扰动后的误差项
3. 再对误差项进行李代数扰动的求导

于是，在非线性优化中只需要不断迭代，就能找到极小值。而且可以证明，ICP问题存在唯一解或无穷多解。在唯一解的情况下，只要能找到极小值，这个极小值就是全局最优解，因此不会遇到局部极小值的情况。这也意味着ICP求解可以任意选定初始值（相机运动Rt），这是已匹配点时求解ICP的一大好处。

缺点：前面讲的是匹配好的点的情况，ICP的研究者们往往更关心匹配未知的情况。

5. 总结

在对极约束——PnP——ICP这个过程中，可以认为利用了越来越多的信息（没有深度——有一个图的深度——有两个图的深度），因此，在深度准确的情况下，得到的估计也将越来越准确。但是如果有噪声，深度图不准确，数据丢失，所以我们不得不丢弃一些没有深度数据的特征点。这可能导致ICP的估计不够准确，并且如果特征点丢弃的太多，可能由于特征点太少，无法进行运动估计的情况。

• 对极约束求的两个相机坐标系的运动Rt。
• 三角测量求得是3D点的深度。（在对极几何之后）
• PnP
  • DLT直接先行优化求解的是相机的位姿
  • P3P求解3D点在相机坐标系下的坐标
  • 最小化投影误差通过BA优化相机位姿和3D点的相机坐标系坐标
• ICP求解的是匹配好的两组点之间的运动，即相机运动