【学习笔记】3D-2D:PnP

主要解决什么问题？

主要解决的是已知空间中N个3D点及其图像中的2D点坐标，求相机在空间中的位置与姿态

求解PnP问题最少需要几个点？

最少只需要3个点对

求解PnP问题的常用方法

主要有用3对点估计位姿的P3P，另外还有DLT（直接线性变换）、EPnP（Efficient PnP）、UPnP、Bundle Ajustment

几种求解PnP方法的主要原理及其特点

DLT（直接线性变化）

原理说直白点就像名字一样，直接解，我们知道待估计的是旋转矩阵R以及平移矩阵t，其中R用矩阵的形式表示有9个未知数（由于旋转矩阵本身是带约束的，所以这9个数不是独立的），t可以用3个未知数表示，假设相机的内参是已知的，那么可以得到2D点与3D点的关系方程，懒得敲公式了，参考高博的《视觉SLAM十四讲》中部分
在这里插入图片描述

可以看到每对特征点可以提供两个方程约束，而我们待估计因此DLT方法求解PnP的话最少可以用6对特征点来估计

上面公式中s指深度，我们知道如果知道深度信息其实可以把一个图像中的点转换到相机坐标系下，而我们知道3D点的位置是世界坐标系下的，所以相机坐标系下的3D点与世界坐标系下的3D点其实就差相机在世界坐标系下的位姿R、t，这也是上面那个公式的本质。

DLT方法的特点是原理简单直接，但是因为R矩阵本身是带约束的（行列式为1），因此通过这种方式直接估计出来的R的9个参数可能不满足这个约束，需要再继续用类似QR分解的方式，求出一个满足约束的，近似的旋转矩阵，至于如何用QR分解来近似我们先留一个问题，有时间再继续探究一下

P3P

核心思想是利用相似三角形的原理求解出2D图像点在相机坐标系下的坐标，最后把问题转换成一个3D到3D问题求解的方式，具体表述太复杂了，直接参考高博的十四讲吧，这里就不赘述了

主要特点：
1、只用3对点即可以求解出R、t（但是求解出会有多个解，因此需要额外一个点来验证得到真正的解）；
2、但是最终求解的是非线性方程，相对于线性方程求解比较复杂；
3、由于只用了3对点的信息，容易受噪声或者误匹配的影响；SLAM中通常做法是先用类似的方法初步估计得到一个位姿，然后再构建最小二乘问题，使用BA的方法优化位姿

Bundle Ajustment

核心思想是构建一个最小二乘问题，将3D点重投影到2D图像平面，最小化重投影误差。
在这里插入图片描述

EPnP

核心思想是通过选取控制点，求解控制点分别在相机坐标系与世界坐标系下的坐标，转化成3D-3D的求解方式，再通过SVD分解求解得出R、t
具体步骤，知乎这篇文章写的很好
https://zhuanlan.zhihu.com/p/361791835
简要步骤如下：
1、在世界坐标系下选取4个控制点，那么世界坐标系下每个点都可以用这四个控制点及其不同的系数组成；通过方程可以求解出每个3D点对应的系数
2、由于欧式不变性，可以证明3D点转换到相机坐标系下时，对应的系数是不变的。
3、建立像点坐标与待求解的控制点在相机坐标系下坐标的方程，通过一系列复杂的求解可以求出控制点在相机坐标系下的坐标
4、已知控制点在相机坐标系下的坐标与世界坐标系下的坐标，问题就转换成为3D到3D的ICP求解问题，从而求出R、t;