相机外参及利用PnP求解外参数矩阵

相机外参数矩阵

外参数矩阵：描述世界坐标系下的点，经过平移和旋转变换，转化为相机坐标系下的点。

（1）式中，下标c：表示相机坐标系下的坐标；
下标w表示世界坐标系下的坐标；
R表示旋转矩阵；
t表示平移矩阵。

外参数矩阵实际上是某点P的世界坐标系和相机坐标系的对齐，即经过平移和旋转运动，两坐标系能完全重合。
就我个人经历而言，外参数矩阵很容易被误解，容易被误解为只是相机的运动和平移矩阵。之所以是误解，因为当且仅当某点P的相机坐标就是它的世界坐标时，相机的旋转和平移才就是他的外参数矩阵，当相机坐标不是它的世界坐标时，则外参数还应该补充世界坐标系到相机坐标系的对齐矩阵。

公式表示相机坐标与世界坐标对齐的过程

和（1）式的区别是，此刻的R、t就分别表示着相机坐标系和世界坐标系的旋转与平移的关系（当然，为了矩阵维数的统一，将矩阵利用0、1增广为4x4的矩阵）。当相机之后发生R1、t1的运动呢？外参数矩阵将如何变化呢？将变化为（3）式。

就是在原有矩阵的基础上左乘该次运动的矩阵，同样的继续安装后，外参还会变化，外参矩阵的具体表达式为（4）式：

式中：n表示第n次运动，外参数矩阵的关键就是要确定初始的相机坐标系与世界坐标系是否统一，这对相机外参数的确性很重要。

初始相机坐标系与世界坐标系间的参数关系

以下图二维空间中的旋转变换为例为例，分析相机坐标系与世界坐标系间的变换矩阵的参数的实际意义。

某点P在不同坐标系下，都可以用不同坐标系下的基向量进行表示：
（其中，xc、yc、xw、yw都是坐标系下的单位正交基向量）

将上（5）式用矩阵进行表示，可简化为（6）式和（7）式：


（7）式明确了P点在两个坐标系间变换时的参数关系，即操作旋转变换的矩阵可以通过旋转角度具体表示出。
对矩阵参数更进一步的理解是：
旋转矩阵第一列表示世界坐标系下的Xw轴在相机坐标系下的方向向量；
旋转矩阵第二列表示世界坐标系下的Yw轴在相机坐标系下的方向向量；

将此类推道三维空间中的变换矩阵，变换矩阵可以表示为下式：

此时，使两坐标系对齐的矩阵M的具体参数就有了现实的意义和具体的值。tx、ty、tz表式两坐标系间的平移关系，结果也很容易推出来，就不赘述了。

PnP求解相机外参数矩阵

相机运动时，如果相机的运动状况时时已知，则可以直接将运动参数代入（4）式，即可求得外参数矩阵。如果相机的运动情况未知，已知空间中n个点的世界坐标系的坐标和相机图像中的像素坐标，利用点与点之间的对应关系，来求解相机的外参数矩阵。

线性求解共面P4P问题

假设四个点的空间坐标已知，四点在同一平面上，我们在四点所在平面上建立一个参考坐标系，使得点Pi在参考坐标系下的坐标为Pri=（Xri，Yri，Zri=0）。将点坐标带入外参数公式（8）得：

在归一化平面中，有内参数方程（10），式中（x1ci，y1ci）为归一化平面下的坐标，且归一化平面坐标值可以由内参数及空间中点在像平面坐标（ui，vi）求出：

整理(9）式带入（10）式，得（11）式

为方便计算，将（11）式以矩阵的形式重新表示为(12)式：

由H1、H2得知，上式中共有9个未知数，每一个空间中的已知点可以依据（11）式，列出两个方程式，四个点即可列出八个方程，而H1为参考坐标中单位的X轴在相机坐标系中的投影，所以，H1的二范数为1，由此可以列出方程||H1||=1。
从而利用上述九个方程式，即可求解出外参数中的九个未知数。

p4p参数求解的优化

当已知的坐标点的数目为n，n大于4时，则可以利用构建有约束的最小二乘的误差模型的方式，来求H1与H2。
首先构建误差函数 δ，并将n点的位姿信息都带入到矩阵中：
利用 δ函数将求解问题转化为了带约束的优化问题，利用拉格朗日因子λ处理约束条件，可以得到如下（14）式方程：

注：拉格朗日条件是必要而非充分条件，即满足上述方程的点不一定是极值点。
优化方程F利用偏导数进行求解，即（15）式：

求导这些矩阵时，要遵循矩阵求导的运算规则，我这里将用到的矩阵求导运算的公式列出来，仅供大家再自行推导使用。

利用上述（15）式求解（14）式的F函数，求解得到结果（17）式：

通过（17）式求解得出相机外参数矩阵的第一列x和第二列y轴在相机坐标系的投影的值，第三列的值由第一列和第二列叉乘得到。因此，摄像机相对于建立的参考坐标系的外参即可全部求出。

线性共面P4P求解的一些问题

1. 无法保证外参数中旋转的矩阵为单位正交矩阵；
2. 解的精度较低，可以利用带有遗忘因子的最小二乘法进一步提高精度。

带有遗忘因子的递推最小二乘法优化外参数

首先，具体的遗忘因子的递推最小二乘法公式如（18）式所述：

（18）式的方程形式上， 类似于卡尔曼滤波方程，从原理上也是，都是利用增益、预测并修正下一时刻的状态。不同的是，操作上具有了优化，引入了遗忘因子，即人为增大修正的增益和方差，防止其陷入局部极值，从而对数据的修正失去敏感性（对误差大的数据，修正效果减弱）。程序编程也可以参考卡尔曼滤波。

openCV中pnp的求解函数

openCV中继承了求解PnP的常用函数，只需要按照格式要求输入相应的参数，即可自动求得外参数的值。

void solvePnP(objectPoints,imagePoints,cameraMatrix,distCoeffs,rvec,tvec,useExtrinsicGuess,flags);
objectPoints:世界坐标系中的3D点坐标，单位mm；
imagePoints: 图像坐标系中的坐标，单位像素；
cameraMatrix：相机的内参数矩阵；
distCoeffs:畸变系数；
rvec：旋转矩阵；
tvec:平移矩阵；
useExtrinsicGuess:迭代初始值是否提供初始的rvec与tvec，没有提供则为false；
flags:SOLVEPNP_ITERATIVE(迭代法、点在同一平面上)、SOLVEPNP_P3P（3点法）、
SOLVEPNP_EPNP（引入齐次重心坐标，点可以不在同一平面上）、
SOLVEPNP_DLS（直接最小二乘，四点不在同一平面）、
SOLVEPNP_UPNP(尝试估计相机的内在参数)

EpPnP在ORB_SLAM中主要用于Tracking线程中的重定位Relocalization模块，需要通过当前关键帧与候选匹配上的3D地图点，迅速建立当前相机的初始姿态。

外参数小结

本文首先明晰了外参数矩阵的由来，外参数矩阵是用来对齐世界坐标系和相机图像坐标系的；之后讨论了外参数矩阵中各参数的实际意义，表示着世界坐标系的坐标轴在相机坐标系的投影；再之后共同学习了，在已知世界坐标系和相机图像系的具体坐标值时，PNP中的P4P法求解外参数的具体计算过程；之后一同学习了P4P中的外参数值利用带有遗忘因子的递推最小二乘法进一步优化取值，可以借鉴卡尔曼滤波进行编程实现；之后，介绍了实际编程中，更普遍的是直接调用openCV函数库中的函数，进行求解。

参考文献

1. 《机器人视觉测量与控制》国防工业出版社 徐德等编著；
2. 《视觉SLAM十四讲》
3. 基于OpenCV和Dlib的头部姿态估计