淦ORB-SLAM2源码 08--PNP（DLT，P3P，EPNP）和ICP（SVD）

2D-3D的位姿求解和3D-3D的位姿求解也是SLAM中非常重要的一环，其中2D-3D的方法主要是PNP，3D-3D的方法主要是ICP，本文主要介绍几种PNP和ICP求解的方法。

PNP

PNP 相机位姿估计pose estimation就是通过几个已知坐标(世界坐标)的特征点,结合他们在相机照片中的成像(像素坐标),求解出相机所在的世界坐标以及旋转角度(这两个货叫相机的外参), 用旋转矩阵®和平移矩阵(t)表示. PnP, which is short for perspective n point就是其中的一种2D-3D解决上述问题的算法, 也就是说, 这个算法的输入是一组点的三维世界坐标和二维像素坐标,输出是相机的旋转矩阵®和平移矩阵(t). OpenCV有相应的库函数solvePnP()和solvePnPRansac(),后者利用了ransac的思想计算更加精确的结果,不过据说速度很慢,完全没法做到实时.

PNP具体细分为几种, 直接线性变换(DLT) P3P EPnP 等等, OpenCV目前提供了3种方法进行PNP计算, CV_P3P, CV_ITERATIVE, CV_EPNP.

DLT（直接线性变换）

最少6对匹配点。

P3P

我们首先需要知道的是P3P并不是直接根据3D-2D点求出相机位姿矩阵，而是先求出对应的2D点在当前相机坐标系下的3D坐标，然后根据世界坐标系下的3D坐标和当前相机坐标系下的3D坐标求解相机位姿的。P3P的求解是从余弦定理开始的，设相机坐标中心为点P，A、B、C为不共线的三个3D点，D为验证3D点，根据余弦定理有如下公式：

接下来其实是对上述3个式子消元化简的过程，同时除以

并且使得

则可得：

然后再次进行替换，令

可得：

将第一个式子代入第2，3式，可以化简得到：

接下来的过程就是如何通过上述两个式子求解A，B，C在当前相机坐标系下的坐标。首先需要明确的是哪些量是已知量，输入的是3D-2D的坐标，也即

都是已知的。因为首先AB，BC，AC的距离都是可以根据输入的3D点求得，而输入的2D点可以求解三个余弦值（如何求解，像素坐标根据相机内参矩阵和畸变参数可以求得在归一化图像平面上的3D坐标，此时 z=1，故余弦值可求）。此时未知数仅x，y两个，所以理论上两个未知数两个方程，是可求的。（从x，y求PA，PB，PC也可求）

具体的求解过程

1、首先是根据2D坐标求解余弦值得过程，首先是由像素坐标到归一化图像坐标的转变，根据就是相机模型

然后是L2归一化的过程，我们知道求解角度的时候用的是归一化坐标（此归一化非彼归一化，上面是归一化到z值等于1的平面上，这里讲的是数学上的归一化）

有了上述值就可以求解余弦值了

同理可求。

2、根据3D坐标求解AB，AC，BC的值，以AB为例

AC，BC同理可求，所以v，w也可以求解。

3、接下来就是一个二元二次方程的求解，比较难求，但是这在数学上是可以求解的，需要用到Wu Ritt的零点分解方法，它可以将原方程等效成一组特征列（Characteristic Serial, CS），凡是原方程组的解都会是CS的解，但是CS的解不一定是原方程的解，所以需要验证，这里的等效方程为：

其中的未知数a1~a4都是已知的，因为原方程的系数是已知的，后文有系数附录，因此我们可以求得x，y的值，4次方程组理论上有4组解，但其实只有一组是合适的。

4、求得了x，y的值，就可以求取PA，PB，PC的值，根据下面的公式，AB已知，可以先求PC，然后分别求解PB，PA：

但是我们需要的是A，B，C在相机坐标系下的坐标，而不是PA，PB，PC的长度，所以还需根据长度求取点的坐标，求解方法是用向量公式：

其中a是单位向量，||PA||是模值，所得即A在相机坐标系下的坐标。

最后求得了A，B，C的坐标就可以通过世界坐标系到当前相机坐标的变换求解相机位姿，注意上面求得了4组解，这里需要使用D点确认哪组解是最合适的。

需要用到3组点+1组验证点。

EPNP

我们目前知道 个世界坐标系下的3D点及其在图像上的2D投影点，还有相机内参，目的是为了求世界坐标系到相机坐标系下的位姿变换 。
EPnP的思路就是先把2D图像点通过内参变换到相机坐标系下的3D点，然后用ICP来求解3D-3D的变换就得到了位姿。那么问题的核心就转化为如何通过2D信息，加上一些约束，来得到相机坐标系下的3D点。
因为我们这里的位姿变换是欧式空间下的刚体变换，所以点之间的相对距离信息在不同坐标系下是不变的。我们称之为刚体结构不变性。后面就是紧紧围绕这个特性来求解的。

下面资料来源与公众号：计算机视觉life

统一变量格式

控制点如何选取

计算控制点系数，用控制点重新表达数据

透视投影关系构建约束

求解

近似求 β初始解

确定了初值就可以用高斯牛顿优化。

高斯牛顿优化

ICP

ICP算法是点云配准算法，给出两组点云，可以计算出两组点云的位姿关系R、t。问题数学表达为：

已知两组点云：

求R、t使下式最小：

1 如果两组点云中的点一一对应，则求解方法很简单。

（1）求解两组点云的质心：

（2）求两组点云去质心坐标：

（3）每点组成矩阵求均值、SVD求解：

（4）R、t结果如下：

2 一般的情况是点云不相匹配，如何求解R、t。

（1）对X中的每个店，在Y中找到距离X最近的点，一一对应

（2）筛选距离最近的若干点

（3）代入公式（0-1）最优化求解得到R、t

（4）利用求解得到的R、t，重新在Y中找距离X最近的点，重复（1）-（4）直到

小于一定阈值

公式证明