一起学ORBSLAM2（9）ORBSLAM的PNP解决方案

转载请注明原创地址：https://blog.csdn.net/qq_30356613/article/category/6897125

PNP问题：已知空间中存在地图点世界坐标系下的坐标为Pw1，Pw2，Pw3...... Pwn 以及对应于在该帧中的匹配点像素坐标u1，u2，u3......un，求解此时相机的位姿Tcw。

ORBSLAM中采用EPNP的方案对解决PNP问题，EPNP将匹配点的当前相机坐标系下的坐标得到，然后根据ICP算法进一步求解得到相机的位姿Tcw。整体框架使用RANSAC的方式不断进行迭代求解因此ORBSLAM对PNP问题的求解主要运用了三个算法 EPNP ICP RANSAC 下面分别介绍：
EPNP：（参考论文EPnP: An Accurate O(n) Solution to the PnP Problem）
基本思想：首先选取4个非共面的虚拟点作为控制点，然后根据空间点和控制点的位置关系以及空间点图像，求解控制点在摄像机坐标系下的坐标，从而得到摄像机位姿。

1. 将已知的世界坐标系下的匹配点坐标（xw1,xw2,xw3,xw4）进行分解，用四个控制点的加权和表示

……

四个控制点的选择：用世界坐标系下特征点集中心点和主成分分析法得到的三个主方向，四个模拟点作为四个控制点

其中,反求权重系数：

此时因为不好求解，因此，我们将（1）式中的用代替，则（1）变为：

进一步整理得到：

从而得到

从而对于每一个三维点（世界坐标系下）都存在：

2.根据线性关系在欧式变换下的不变性得到在相机坐标系下特征点的坐标为：

例：

因此存在：

即:

同理可得到四个匹配点的像素坐标表达

尺度s的表达式：

从而得到：

进一步对于每一个匹配点都存在：，注意这里（）为归一化平面的坐标，为控制点在相机坐标系下的坐标。

展开后每一匹配点都有：

整理成矩阵形式：

Mx = 0 ，x由四个控制点组成，一共12维向量，其中

3.因此，x的解是矩阵M的核空间，对于透视模型矩阵M的零空间的维数为1，在图像数据有噪声时，x的最小二乘解xls是矩阵M的最小奇异值的右奇异向量。

因此x的解可以由公式给出：

其中是零特征值（奇异值）对应的特征向量（奇异向量），求奇异值奇异向量需要分解得到。这里需要确定N为多少，论文中提到：

意思是指在由于尺度不确定性，因此N的维度至少为1，而且如果相机是正投影而不是仿射变换，则N的维度变为4，因此如果焦距很小的话就只有1个0特征值，如果焦距长度越来越大，他就越趋向于正投影，N的维度就慢慢趋向于4，而由于有噪声的原因，特征值可能不是完全是0，而是一个足够小的数。在ORBSLAM中计算了N=1-4的所有可能，然后计算反投影误差取最小的反投影误差对应的N值作为正确的N。我们通常利用范数的理论来计算。即对于两边同时取范数，如下

当N=1 时

当N=2时

同理N=3，N=4

将等式左边展开得到：

以N=2为例：

由于

可以计算出N=4的式子，将其写成矩阵形式

，,

4.求解方程使用SVD的方式，求解，从而解出X，即得到相机坐标系下控制点坐标，再带入得到相机坐标系下匹配点的坐标。根据匹配点在相机坐标系下的坐标以及在世界坐标系下的坐标，然后根据ICP算法求得相机位姿。

RANSAC ：

在ORBSLAM中，为了得到更加准确的相机位姿，采用了RANSAC的方式，也是为了能利用更多的匹配点对。由于EPNP每次需要四个点对，因此，我们每次需要选择四个点对作为待处理点对，这四个点对是随机从所有匹配点中获得的，根据这是四个点对计算出相机位姿（RANSAC模型）之后，通过判断内点数量（通过计算反投影误差，大于阈值判定为内点，小于阈值判定为非内点）大于阈值证明此次计算的相机位姿是有效的，并且将最大内点量对应的位姿存储起来，证明此时计算的位姿应该是最准确的。经过一定迭代次数之后，将获得的最优相机位姿作为当前相机位姿交给系统。RANSAC实际上是一种思想，是一种充分利用所给信息通过迭代不断更新最优模型的方法。如果单独说RANSAC具体的算法细节，他贯穿于整个系统，作为一个框架应用于PNP问题的求解

ICP：

该方法是解决EPNP方法求解相机位姿遗留下来的问题，EPNP是得到相机坐标系下的3D坐标，而ICP问题描述的是怎么通过匹配点在世界坐标系下的坐标和匹配点在相机坐标系下的坐标来结算相机位姿。推倒如下：

首先我们定义第i对点的误差项：