SLAM ORB-SLAM2（29）PnP估计姿态

1. PnP问题

在 《SLAM ORB-SLAM2（26）重定位过程》 提到的重定位过程中
先计算当前帧特征点的词袋向量ComputeBoW
接着就是用词袋找到与当前帧相似的候选关键帧DetectRelocalizationCandidates
然后通过词袋模型进行初步匹配，紧接着查询较匹配的关键帧，此时先通过PnP投影估计姿态

根据匹配的 3D地图点 和当前帧特征点的 2D像素坐标 估计相机位姿
这种根据3D-2D映射关系求解相机位姿的问题，就是所谓的PnP问题(Perspective-n-Point Problem)

在机器视觉领域中，这是一个非常常见的问题
OpenCV 库还专门提供了函数 solvePnP进行求解

PnP 问题有很多种解法，ORB-SLAM2 采用了一种称为 EPnP 的算法，求解效率还不错

通常，EPnP的解都会拿来作为高斯牛顿之类迭代优化算法的初值， 经过迭代之后得到一个更优的解

从 OpenCV的文档 中找到上图所示
已知相机内参矩阵$K$ 和 $n$ 个 3D 空间点 { c 1 , c 2 , ⋯ , c n } \{ \boldsymbol{c_1, c_2, \cdots, c_n} \} {c1​,c2​,⋯,cn​}
及其到图像上 2D 的投影点 { μ 1 , μ 2 , ⋯ , μ n } \{ \boldsymbol{\mu_1, \mu_2, \cdots, \mu_n} \} {μ1​,μ2​,⋯,μn​}，求解相机的位置和姿态

记第 $i$ 个 3D 空间点的齐次坐标为 ${\mathbf{c}}_{\mathbf{i}}={\left[\begin{array}{cccc}{x}_i& y_i& z_i& 1\end{array}\right]}^T$
其在图像上投影的 2D 像素坐标为 ${\mathbf{\mu }}_{\mathbf{i}}={\left[\begin{array}{ccc}{u}_i& v_i& 1\end{array}\right]}^T$

其投影过程，可以分解为两步：

1. 根据相机的位姿，将空间点 ${\mathbf{c}}_{\mathbf{i}}$ 从世界坐标系下变换到相机坐标系下$\left[\mathbf{R}|\mathbf{t}\right]{\mathbf{c}}_{\mathbf{i}}$
2. 将相机坐标系下的点，根据相机内参矩阵$\mathbf{K}$，投影到图像 ${\mathbf{\mu }}_{\mathbf{i}}$ 上

其整个过程相当于连续乘了两个矩阵：
$$s\left[\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right]=\mathbf{K}\left[\mathbf{R}|\mathbf{t}\right]\left[\begin{array}{c}{x}_i\\ y_i\\ z_i\\ 1\end{array}\right]=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}{t}_1& t_2& t_3& t_4\\ t_5& t_6& t_7& t_8\\ t_9& t_{10}& t_{11}& t_{12}\end{array}\right]\left[\begin{array}{c}{x}_i\\ y_i\\ z_i\\ 1\end{array}\right]$$

其中，$s$ 是一个尺度系数，在计算时通常通过叉乘或者归一化将之消除掉
$K,R,t$ 分别是相机的内参矩阵、姿态矩阵和位置向量

参照 单应矩阵和基础矩阵的求解过程， 用矩阵 $A=K[R\mid t]$ 将上式改写为：
$$s\left[\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right]=\underset{\mathbf{A}}{\underbrace{\left[\begin{array}{cccc}{a}_1& a_2& a_3& a_4\\ a_5& a_6& a_7& a_8\\ a_9& a_{10}& a_{11}& a_{12}\end{array}\right]}}\left[\begin{array}{c}{x}_i\\ y_i\\ z_i\\ 1\end{array}\right]\Rightarrow \{\begin{array}{l}{u}_i=\frac{a_1{x}_i+a_2{y}_i+a_3{z}_i+a_4}{a_9{x}_i+a_{10}{y}_i+a_{11}{z}_i+a_{12}}\\ v_i=\frac{a_5{x}_i+a_6{y}_i+a_7{z}_i+a_8}{a_9{x}_i+a_{10}{y}_i+a_{11}{z}_i+a_{12}}\end{array}\Rightarrow \left[\begin{array}{cccccccccccc}{x}_i& y_i& z_i& 1& 0& 0& 0& 0& -x_i& -y_i& -z_i& -1\\ 0& 0& 0& 0& x_i& y_i& z_i& 1& -x_i& -y_i& -z_i& -1\end{array}\right]\left[\begin{array}{c}{a}_1\\ a_2\\ ⋮\\ a_{11}\\ a_{12}\end{array}\right]=0$$
如此，对于 $n$ 个匹配点对，就可以得到下面形式的线性方程组
SVD分解，解零空间，就可以解得矩阵A

最少6个匹配点对，就可以完成求解，这就是一个DLT（Direct Linear Transformation）的方法
$$\left[\begin{array}{cccccccccccc}{x}_0& y_0& z_0& 1& 0& 0& 0& 0& -x_0& -y_0& -z_0& -1\\ 0& 0& 0& 0& x_0& y_0& z_0& 1& -x_0& -y_0& -z_0& -1\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ x_{n-1}& y_{n-1}& z_{n-1}& 1& 0& 0& 0& 0& -x_{n-1}& -y_{n-1}& -z_{n-1}& -1\\ 0& 0& 0& 0& x_{n-1}& y_{n-1}& z_{n-1}& 1& -x_{n-1}& -y_{n-1}& -z_{n-1}& -1\end{array}\right]\left[\begin{array}{c}{a}_1\\ a_2\\ ⋮\\ a_{11}\\ a_{12}\end{array}\right]=0$$

当然上述DLT算法解得的是矩阵$A$，它包含了相机内参$K$、姿态矩阵$R$和平移向量$t$

进一步的，通过QR分解，可以从矩阵$A$中把这三个都给分解出来
看起来这一过程还附带算出了相机的内参，这也正是相机的内参标定的求解过程
DLT算法简单直接，但是它忽略了太多的约束，所以结果一般都不会很好

后来人们还研究出了很多求解 PnP 问题的算法，有只需要3个点就可以求解的P3P算法
ORB-SLAM2 用的就是EPnP算法，效率高而且稳定，据说其算法复杂度是 O(n) 的

2. EPnP算法

在 《SLAM ORB-SLAM2（28）PnP估计姿态过程》 中
提到的估计相机位姿函数 compute_pose 及其调用的子函数是 EPnP 算法的核心

下面结合论文中关于算法原理的介绍，分析函数 compute_pose 的实现过程

根据论文中的表述，EPnP 算法的时间复杂度是 $O(n)$ 的， 相比于其它$O(n^5)$,$O(n^8)$的算法而言，它要高效很多了
其核心思想体现在三个方面：

1. 将世界坐标系下的 $n$ 个 3D 点，用 4 个虚拟的控制点通过加权和的形式表达
2. 这种虚拟控制点的加权和关系，经过相机位姿 $[R\mid \mid t]$ 变换到相机坐标系下仍然成立，既对应点的加权系数不变
3. 根据2D像素坐标估计出 4 个虚拟的控制点在相机坐标系的坐标，可以将 3D-2D 的映射问题转换为 3D-3D 点映射问题

总体上，EPnP 算法可以分为四步：

1. 计算世界坐标系下的虚拟控制点
2. 计算表达各个3D点的控制点权重
3. 求解相机坐标系下的控制点坐标
4. ICP 求解相机位姿

2.1. 计算4对控制点的世界坐标

下面是其具体实现的函数 compute_pose 的代码片段
它有两个参数 R, t 分别输出相机的旋转矩阵和平移向量
用于求解相机位姿的匹配点信息已经保存在成员变量 pws 和 us 中

/*** @brief 估计相机位姿函数* @param R  相机的旋转矩阵* @param t  相机的平移向量* @return rep_errors 重投影误差*/
double PnPsolver::compute_pose(double R[3][3], double t[3])
{/* 计算世界坐标系下的四个虚拟控制点 */choose_control_points();

该函数一开始，先调用函数 choose_control_points 计算了世界坐标系下的四个虚拟控制点

2.2. 计算齐次质心坐标

理论上控制点的选取可以是任意的
但论文中说，以 3D点的质心以及PCA分解后的三个主轴上选取的三个点
所构成的控制点有助于提高算法的稳定性

  /* 计算齐次质心坐标 */compute_barycentric_coordinates();

把计算得到的 4 个控制点的世界坐标记为 ${\mathbf{c}}_{\mathbf{j}}^{\mathbf{w}},j=1,\cdots ,4$
那么世界坐标系下的第 $i$ 个 3D 点坐标都可以写成这四个控制点的加权和形式
$${\mathbf{p}}_{\mathbf{i}}^{\mathbf{w}}=\sum _{j=1}^4{\alpha }_{ij}{\mathbf{c}}_{\mathbf{j}}^{\mathbf{w}},\text{with}\sum _{j=1}^4{\alpha }_{ij}=1$$

${\alpha }_{ij}$ 齐次质心坐标，可以将上式写成齐次矩阵的形式，求解得到
$$\left[\begin{array}{c}{\mathbf{p}}_{\mathbf{i}}^{\mathbf{w}}\\ 1\end{array}\right]=\underset{C}{\underbrace{\left[\begin{array}{cccc}{\mathbf{c}}_1^{\mathbf{w}}& {\mathbf{c}}_2^{\mathbf{w}}& {\mathbf{c}}_3^{\mathbf{w}}& {\mathbf{c}}_4^{\mathbf{w}}\\ 1& 1& 1& 1\end{array}\right]}}\left[\begin{array}{c}{\alpha }_{i1}\\ {\alpha }_{i2}\\ {\alpha }_{i3}\\ {\alpha }_{i4}\end{array}\right]\Rightarrow \left[\begin{array}{c}{\alpha }_{i1}\\ {\alpha }_{i2}\\ {\alpha }_{i3}\\ {\alpha }_{i4}\end{array}\right]=C^{-1}\left[\begin{array}{c}{\mathbf{p}}_{\mathbf{i}}^{\mathbf{w}}\\ 1\end{array}\right]$$
通过函数 compute_barycentric_coordinates 计算得到
结果将被保存在成员变量 $a$ 中
该函数并没有直接构建 矩阵$C$ 然后对其求逆
而是将 3D 坐标和四个控制点都减去质心，再进行求解

2.3. 计算4对控制点的相机坐标

2.3.1. 构造M矩阵

世界坐标系下的点 ${\mathbf{p}}_{\mathbf{i}}^{\mathbf{w}}$ 经过变换$[R\mid t]$ 之后得到相机坐标系下的坐标 ${\mathbf{p}}_{\mathbf{i}}^{\mathbf{c}}$
对控制点 ${\mathbf{c}}_{\mathbf{j}}^{\mathbf{w}}$ 做相同的变换得到 ${\mathbf{c}}_{\mathbf{j}}^{\mathbf{j}}$，仍然满足相同的加权关系，即：
$${\mathbf{p}}_{\mathbf{i}}^{\mathbf{c}}=\sum _{j=1}^4{\alpha }_{ij}{\mathbf{c}}_{\mathbf{j}}^{\mathbf{c}},\text{with}\sum _{j=1}^4{\alpha }_{ij}=1$$
再考虑到针孔相机模型，对于每一个 3D-2D 匹配点关系，都有下式成立：
$$\forall i,w_i\left[\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right]=\left[\begin{array}{ccc}{f}_u& 0& u_c\\ 0& f_v& v_c\\ 0& 0& 1\end{array}\right]\left(\sum _{j=1}^4{\alpha }_{ij}\left[\begin{array}{c}{x}_j^c\\ y_j^c\\ z_j^c\end{array}\right]\right)$$
其中，${{\mathbf{c}}_{\mathbf{j}}^{\mathbf{c}}}^T=\left[\begin{array}{ccc}{x}_j^c& y_j^c& z_j^c\end{array}\right]$是第$j$个控制点在相机坐标系下的坐标
$u_i,v_i$是2D像素坐标， $f_u,f_v,u_c,v_c$分别是相机的焦距和光心
$w_i$是一个尺度因子，将在后续计算中约去

参照 DLT 算法的套路，每个点都可以写出两个约束，$n$个点可以构造如下的 $\mathbf{Mx}=0$ 形式的线性方程组:
$$\left[\begin{array}{ccccccc}{\alpha }_{11}{f}_x& 0& {\alpha }_{11}(c_x-u_1)& \cdots & {\alpha }_{14}{f}_x& 0& {\alpha }_{14}(c_x-u_1)\\ 0& {\alpha }_{11}{f}_y& {\alpha }_{11}(c_y-v_1)& \cdots & 0& {\alpha }_{14}{f}_y& {\alpha }_{14}(c_y-v_1)\\ ⋮& ⋮& ⋮& & ⋮& ⋮& ⋮\\ {\alpha }_{n1}{f}_x& 0& {\alpha }_{n1}(c_x-u_1)& \cdots & {\alpha }_{n4}{f}_x& 0& {\alpha }_{n4}(c_x-u_1)\\ 0& {\alpha }_{n1}{f}_y& {\alpha }_{n1}(c_y-v_1)& \cdots & 0& {\alpha }_{n4}{f}_y& {\alpha }_{n4}(c_y-v_1)\end{array}\right]\left[\begin{array}{c}{\mathbf{c}}_1^{\mathbf{c}}\\ ⋮\\ {\mathbf{c}}_4^{\mathbf{c}}\end{array}\right]=0$$

其中，$\mathbf{x}={\left[\begin{array}{cccc}{{\mathbf{c}}_1^{\mathbf{c}}}^T& {{\mathbf{c}}_2^{\mathbf{c}}}^T& {{\mathbf{c}}_3^{\mathbf{c}}}^T& {{\mathbf{c}}_4^{\mathbf{c}}}^T\end{array}\right]}^T$

求解出 $x$ 就得到了相机坐标系下的控制点坐标

在下面的代码片段中，先通过 OpenCV 的接口创建了一个 $2n\times 12$ 的矩阵
再通过函数 fill_M 完成上式中矩阵 $M$ 的构建工作

  /* 构造M矩阵 */CvMat *M = cvCreateMat(2 * number_of_correspondences, 12, CV_64F);for (int i = 0; i < number_of_correspondences; i++)fill_M(M, 2 * i, alphas + 4 * i, us[2 * i], us[2 * i + 1]);

2.3.2. 计算$M^{T}M$的0特征值对应的特征向量

根据线性代数的说法，这个方程组的解一定属于 $M$ 的零空间
既 $x$ 可以写成 $M$ 的零空间正交基的线性组合
$$\mathbf{x}=\sum _{i=1}^N{\beta }_i{\mathbf{v}}_i$$
其中，${\mathbf{v}}_i,i\in [1,N]$ 是 $M$ 的 零空间正交基
既该矩阵的 $N$ 个为 0 的奇异值所对应的列向量
${\beta }_i$ 是线性组合的系数

下面的代码中， 先构建了局部变量 MtM, D, Ut 分别用来记录矩阵 MTM，以及它分解的特征值和特征向量
OpenCV 的接口 cvMulTransposed 根据 M 生成 MTM，cvSVD 完成实际的SVD分解操作

  /* 定义矩阵MTM，分解的特征值和特征向量 */double mtm[12 * 12], d[12], ut[12 * 12];CvMat MtM = cvMat(12, 12, CV_64F, mtm);CvMat D = cvMat(12, 1, CV_64F, d);CvMat Ut = cvMat(12, 12, CV_64F, ut);/* 通过SVD分解，计算MTM的0特征值对应的特征向量 */cvMulTransposed(M, &MtM, 1);cvSVD(&MtM, &D, &Ut, 0, CV_SVD_MODIFY_A | CV_SVD_U_T);cvReleaseMat(&M);

通过对 12×12 的矩阵 ${\mathbf{M}}^T\mathbf{M}$ 进行奇异值分解可以得到 ${\mathbf{v}}_i,i\in [1,N]$

2.3.3. 计算零空间的秩

现在，还需要确定零空间的秩 $N$ 和线性组合系数 ${\beta }_i$
论文中通过一些实验发现， $N$ 跟相机的焦距之间呈现一种正相关的关系
下面是从原文中抠出来的一张图，图中横坐标是MTM的12个奇异值， 纵轴是奇异值的大小

不同的曲线对应着不同的焦距，随着焦距的增大，曲线不断下移
从右侧放大的图中可以看到，$f=10000$ 时也就只有4个奇异值接近为0
于是乎，作者就针对 $N\in [1,4]$ 的四种情况分别讨论了系数${\beta }_i$的计算方法

2.3.4. 计算线性组合的系数

控制点坐标是经过旋转和平移，从世界坐标系转换到相机坐标系的
这两种变换都不会改变控制点之间的距离，因此存在如下的关系式:

$$\begin{array}{rc}& \Vert {\mathbf{c}}_{\mathbf{i}}^{\mathbf{c}}-{\mathbf{c}}_{\mathbf{i}}^{\mathbf{c}}\Vert =\Vert {\mathbf{c}}_{\mathbf{i}}^{\mathbf{w}}-{\mathbf{c}}_{\mathbf{j}}^{\mathbf{w}}\Vert \\ \Rightarrow & {\sum }_{k=1}^N\Vert {\beta }_k{\mathbf{v}}_k^{[i]}-{\beta }_k{\mathbf{v}}_k^{[j]}\Vert =\Vert {\mathbf{c}}_{\mathbf{i}}^{\mathbf{w}}-{\mathbf{c}}_{\mathbf{j}}^{\mathbf{w}}\Vert \end{array}$$
其中，${\mathbf{v}}_k^{[i]}$ 是零空间中特征向量 ${\mathbf{v}}_k$ 与 ${\mathbf{c}}_{\mathbf{i}}^{\mathbf{c}}$ 对应的 3×1 的子向量
EPnP 的论文中说是针对 N=1,⋯4 四种情况分别计算了部分 β 的取值
然后通过高斯-牛顿的方法求解所有的 β值，构建相机坐标系下的控制点${\mathbf{c}}_{\mathbf{i}}^{\mathbf{c}}$

接着，求解 ICP 问题计算相机的姿态矩阵和平移向量，
实际看代码，好像只依次讨论了 $N=4,2,3$ 三种情况
再根据路标点在世界坐标系中的坐标和相机坐标系中的坐标，使用ICP估计相机位姿

  double l_6x10[6 * 10], rho[6];CvMat L_6x10 = cvMat(6, 10, CV_64F, l_6x10);CvMat Rho = cvMat(6, 1, CV_64F, rho);compute_L_6x10(ut, l_6x10); /* SVD分解出的Ｕ矩阵和Ｌ矩阵 */compute_rho(rho);           /* 计算世界坐标系下的四个控制点两两之间的距离 */double Betas[4][4], rep_errors[4];double Rs[4][3][3], ts[4][3];find_betas_approx_1(&L_6x10, &Rho, Betas[1]);                /* 线性组合的系数 N = 4所有的β值 */gauss_newton(&L_6x10, &Rho, Betas[1]);                       /* 通过高斯-牛顿的方法求解所有的β值 */rep_errors[1] = compute_R_and_t(ut, Betas[1], Rs[1], ts[1]); /* 计算相机位姿变换并返回重投影误差 *//* 线性组合的系数 N = 2 */find_betas_approx_2(&L_6x10, &Rho, Betas[2]);gauss_newton(&L_6x10, &Rho, Betas[2]);rep_errors[2] = compute_R_and_t(ut, Betas[2], Rs[2], ts[2]);/* 线性组合的系数 N = 3 */find_betas_approx_3(&L_6x10, &Rho, Betas[3]);gauss_newton(&L_6x10, &Rho, Betas[3]);rep_errors[3] = compute_R_and_t(ut, Betas[3], Rs[3], ts[3]);

2.4. 选择最小重投影误差

选取使得重投影误差最小的那组作为最后的解

  /* 选取最小重投影误差的那个作为最后的解 */int N = 1;if (rep_errors[2] < rep_errors[1])N = 2;if (rep_errors[3] < rep_errors[N])N = 3;copy_R_and_t(Rs[N], ts[N], R, t);return rep_errors[N];

3. 标题

谢谢