转载请注明原创地址：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）进行分解，用四个控制点的加权和表示
……
四个控制点的选择：用世界坐标系下特征点集中心点和主成分分析法得到的三个主方向，四个模拟点作为四个控制点
其中,反求权重系数：
此时因为
进一步整理得到：
从而得到
从而对于每一个三维点（世界坐标系下）都存在：
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.求解方程
RANSAC ：
在ORBSLAM中，为了得到更加准确的相机位姿，采用了RANSAC的方式，也是为了能利用更多的匹配点对。由于EPNP每次需要四个点对，因此，我们每次需要选择四个点对作为待处理点对，这四个点对是随机从所有匹配点中获得的，根据这是四个点对计算出相机位姿（RANSAC模型）之后，通过判断内点数量（通过计算反投影误差，大于阈值判定为内点，小于阈值判定为非内点）大于阈值证明此次计算的相机位姿是有效的，并且将最大内点量对应的位姿存储起来，证明此时计算的位姿应该是最准确的。经过一定迭代次数之后，将获得的最优相机位姿作为当前相机位姿交给系统。RANSAC实际上是一种思想，是一种充分利用所给信息通过迭代不断更新最优模型的方法。如果单独说RANSAC具体的算法细节，他贯穿于整个系统，作为一个框架应用于PNP问题的求解
ICP：
该方法是解决EPNP方法求解相机位姿遗留下来的问题，EPNP是得到相机坐标系下的3D坐标，而ICP问题描述的是怎么通过匹配点在世界坐标系下的坐标和匹配点在相机坐标系下的坐标来结算相机位姿。推倒如下：
首先我们定义第i对点的误差项：
然后，构建最小二乘问题，求使误差平方和达到极小的R，t：
下面我们推导它的求解方法。首先，定义两组点的质心：
请注意质心是没有下标的。随后，在误差函数中，我们作如下处理：
注意到交叉项部分中，
ICP算法流程：
（1）计算两组点的质心位置
（2）根据以下优化问题计算旋转矩阵：
（3）根据第二步的R，计算t：
其中，算法中最困难的应该是关于第二步的优化问题的求解，这里单独拿出来讲解一下： 
注意到式3.4第一项与R无关，第二项,亦与R无关。因此，实际上优化目标函数变为：
令
