ORBSLAM3中的MLPnP在重定位时计算当前帧和候选帧的位姿变换本文已参与「新人创作礼」活动，一起开启掘金创作之路。

本文已参与「新人创作礼」活动，一起开启掘金创作之路。

参考论文：MLPNP - A REAL-TIME MAXIMUM LIKELIHOOD SOLUTION TO THE PERSPECTIVE-N-POINT PROBLEM

PnP的任务是找到一个旋转 $R \in SO(3)$ 和平移 $t \in \mathbb R^3$ ，将世界坐标系下的点 $p_i$ 映射到相机坐标系下的观测点 $v_i$ ，即：

\lambda_iv_i=Rp_i+t \tag{1}

这里的 $\lambda_i$ 是每个点的深度，观测值 $v_i$ 是测量得到的结果，因此该不确定量具有单位长度 $\lVert v_i \rVert=1$ 。下图中描述了参数和观测值：在这里插入图片描述每个方位向量 $v_i$ 上的平面是由各自的零空间向量 $r_i$ 和 $s_i$ 张成的。

观测与不确定性传播 设标定后的相机对世界点 $p_i \in \mathbb R^3$ 进行观测，在像平面的不确定观测为： $\bf x'=\begin{bmatrix} x' \\y' \\ \end{bmatrix}，Σ_{\bf x' \bf x'}=\begin{bmatrix} \sigma^2_{x'} & \sigma_{x'y'} \\ \sigma_{y'x'} & \sigma^2_{y'} \\ \end{bmatrix}$ 关于观测点的不确定性用二维协方差矩阵 $Σ_{\bf x'\bf x'}$ 来描述。通过正投影 $\pi$ ( $K^{-1}$ )将图像点 $\bf x'$ 变换到相机坐标系下的三维方向上（不确定深度信息）。 $\bf x=\pi \bf x'==\begin{bmatrix} x\\y\\1 \\ \end{bmatrix}，J_{\pi}=\begin{bmatrix} {∂\pi_{x'} \over ∂x'} & {∂\pi_{x'} \over ∂y'} \\{∂\pi_{y'} \over ∂x'} &{∂\pi_{y'} \over ∂y'} \\0&0 \\ \end{bmatrix}$ $J_{pi}$ 为正投影矩阵的雅可比，因此利用图像点 $\bf x'$ 的不确定性传播，可到的相机点 $\bf x$ 的协方差矩阵： $Σ_{\bf{xx}}=\bf J_{\pi}Σ_{\bf{\bf x'x'}}\bf J_{\pi}^T=\begin{bmatrix} \sigma^2_{x} & \sigma_{xy} & 0 \\ \sigma_{yx} & \sigma^2_{y} & 0 \\0&0&0\\ \end{bmatrix}$ 其中协方差矩阵的秩为2，因此它是奇异的，不可逆的。接下来球面归一化产生了最终的观察表达式，称之为方位向量： $\bf v=\begin{bmatrix} v_x \\v_y \\v_z \\ \end{bmatrix}={\bf x \over \lVert \bf x \rVert}，Σ_{\bf vv}=\begin{bmatrix} \sigma^2_{v_x} & \sigma_{v_xv_y} & \sigma_{v_xv_z} \\ \sigma_{v_yv_x} &\sigma^2_{v_y} & \sigma_{v_yv_z} \\\sigma_{v_zv_x} &\sigma_{v_zv_y} &\sigma^2_{v_z}\\ \end{bmatrix}$ 根据协方差的传播： $Σ_{\bf {vv}}=\bf JΣ_{\bf xx}\bf J^T，\bf J={1\over \lVert \bf x \rVert}(\bf I_3 -vv^T)$ 此时的协方差矩阵 $Σ_{\bf xx}$ 依然是奇异的。

向量的零空间

零空间定义：已知 $A$ 为一个 $m*n$ 矩阵。A的零空间（nullspace），又称核（kernel），是一组由下列公式定义的 $n$ 维向量： $ker(A)=\{x \in \mathbb C^n:Ax=0\}$ 即线性方程组 $Ax=0$ 的所有解 $x$ 的集合。在数学中，一个算子 $A$ 的零空间是方程 $Av=0$ 的所有解 $v$ 的集合。它也叫做A的核，核空间。用集合建造符号表示为

Null(A)=\{v \in V:Av=0\}

$v$ 的零空间张成一个二维坐标系，其轴 $r$ 和 $s$ 垂直于 $v$ ，并位于 $v$ 的切线空间：

{\bf J_{v_r}(v)}=null(\bf v^T)=\begin{bmatrix} \bf r & \bf s \\ \end{bmatrix}=\begin{bmatrix} r_1 & s_1 \\r_2 &s_2\\r_3&s_3 \\ \end{bmatrix} \tag{7}

函数 $null(·)$ 计算 $v$ 的奇异值分解(SVD)，取两个零特征值对应的两个特征向量。即：

\bf v^T r=0

\bf v^T s=0

进一步假设 ${\bf J}_{{\bf v}_r}$ 是一个标准正交矩阵， ${\bf J}^T_{{\bf v}_r}({\bf v}){\bf J}_{{\bf v}_r}({\bf v})={\bf I}_2$ 。这个 ${\bf J}_{{\bf v}_r}$ 还表示了从正切空间到原始向量的变换的雅可比矩阵 $\color{red}{?}$ ，因此， ${\bf J}^T_{{\bf v}_r}$ 可以得到从原始齐次向量 $v$ 到其简化后的等价向量 $v_r$ 的变换。

{\bf v}_r=\begin{bmatrix} dr \\ ds \\ \end{bmatrix}={\bf J}^T_{{\bf v}_r}({\bf v})\bf v=\bf 0 \tag{8}

与非奇异的协方差：

Σ_{{\bf v}_r{\bf v}_r}={\bf J}^T_{{\bf v}_r}({\bf v})Σ_{{\bf vv}}{\bf J}_{{\bf v}_r}({\bf v})=\begin{bmatrix} \sigma^2_{v_{r_x}} & \sigma_{v_{r_{xy}}'} \\ \sigma_{v_{r_{xy}}} & \sigma^2_{v_{r_y}} \\ \end{bmatrix} \tag{9}

另一种看待 $v_r$ 的方式是把它看作是切线空间中的残差。在下面，我们将通过最小化正切空间的残差，利用Eq. 8来得到相机在世界坐标系下的旋转和平移的线性估计。

补充"slam"东对于该部分的理解：对于向量v，是经过相机观测图像的像素点，经过内参的反投影得到的。所谓的正切平面，就是通过计算向量v转置的零空间，得到与v垂直的r和s坐标轴。因此定义J为从正切空间到原始向量v的雅可比变换矩阵，对于雅可比的理解可参考知乎如何理解雅可比矩阵，雅可比就是切空间。因为r和s本身与v垂直，所以v在该正切空间上的投影为0。但是对于空间点p，是经过旋转R与平移t，归一化，然后再乘上变换雅可比，便可以落在对应向量v的正切平面上。但是该p'点虽然理论上也是在原点，但是实际上因为Rt的估计存在误差，所以投影并不为0，所以这里把它看作切线空间中的残差。

相机位姿的线性估计

利用式1和7，我们可以改写式8

\begin{bmatrix} dr \\ ds \\ \end{bmatrix}=\begin{bmatrix} {\bf r}^T \\ {\bf s}^T \\ \end{bmatrix}\lambda^{-1}_i (Rp_i+t) \tag{10}

其中 $\lambda \neq 0$ 。因此，如果我们知道相机的绝对方位，世界点 $p_i$ 在 $v$ 的正切空间中的投影，应该会得到相同的简化后的坐标。由公式10可得到：

0 = r_1(\hat{r}_{11}p_x + \hat{r}_{12}p_y + \hat{r}_{13}p_z + \hat{t}_1)\\ + r_2(\hat{r}_{21}p_x + \hat{r}_{22}p_y + \hat{r}_{23}p_z + \hat{t}_2) \\+ r_3(\hat{r}_{31}p_x + \hat{r}_{32}p_y + \hat{r}_{33}p_z + \hat{t}_3)

0 = s_1(\hat{r}_{11}p_x + \hat{r}_{12}p_y + \hat{r}_{13}p_z + \hat{t}_1)\\ + s_2(\hat{r}_{21}p_x + \hat{r}_{22}p_y + \hat{r}_{23}p_z + \hat{t}_2) \\+ s_3(\hat{r}_{31}p_x + \hat{r}_{32}p_y + \hat{r}_{33}p_z + \hat{t}_3)

现在，两个等式的未知数都是线性的，可以将他们叠加到一个矩阵A中，得到一个线性方程组的齐次方程组：

\bf Au=0 \tag{12}

$u = [\hat{r}_{11}, \hat r_{12}, \hat{r}_{13}, \hat{r}_{21}, \hat{r}_{22}, \hat{r}_{23}, \hat{r}_{31}, \hat{r}_{32}, \hat{r}_{33},\hat{t}_1,\hat{t}_2,\hat{t}_3]^T$ 。由于每个观测产生两个残差，求解公式12至少需要5个点。利用不相关的观测结果，给出了随机模型：

P=\begin{bmatrix} Σ^{-1}_{\bf {v}^{1}_r \bf{v}^{1}_r} & \cdots &0 \\ \vdots & \ddots &\vdots \\ 0& \cdots & Σ^{-1}_{\bf {v}^{i}_r \bf{v}^{i}_r} \\ \end{bmatrix} \tag{13}

最终的正规方程为：

A^TPAu=Nu=0 \tag{14}

在 $\lVert u\rVert=1$ 的条件下，找到 $u$ 使得等式14最小化，使用SVD分解：

N=UDV^T\tag{15}

解为V中对应于D中最小奇异值的特定列：

\hat R=\begin{bmatrix} \hat{r}_{11} & \hat r_{12} & \hat{r}_{13} \\ \hat{r}_{21} & \hat{r}_{22}& \hat{r}_{23}\\ \hat{r}_{31}&\hat{r}_{32} & \hat{r}_{33} \\ \end{bmatrix}，\hat t=\begin{bmatrix} \hat{t}_1&\hat{t}_2&\hat{t}_3 \\ \end{bmatrix} \tag{16}

它由一个尺度因子决定，因为平移部分 $\hat{t}$ 仅指向正确的方向。这个尺度可以从现实中恢复，旋转矩阵 $\hat{R}$ 每一列的模必须等于1，因此最终的平移为：

t={\hat{t}\over \sqrt[3]{\lVert \hat{r}_1 \rVert \lVert \hat{r}_2 \rVert \lVert \hat{r}_3 \rVert}}\tag{17}

约束表明，这9个旋转参数并没有定义一个正确的旋转矩阵。可以通过SVD分解计算 $\hat{R}$ ：

\hat{R}=U_RD_RV_R^T\tag{18}

最小化Frobenius范式的最佳矩阵为：

R=U_RV_R^T\tag{19}

到此为止，获得了相机绝对姿态的线性ML估计。为了提高精度，采用了非线性的优化方法。对初始估计进行后续的优化是一个常见的过程。

非线性优化 我们应用高斯-牛顿优化迭代改进相机姿态。具体地说，我们最小化Eq. 10中定义的正切空间残差。

这个速度相当快，有两个原因：一方面，零空间向量已经计算过了我们只需要计算切空间向量和每个变换后的世界点之间的点积。另一方面，线性估计的结果已经接近局部极小值，即高斯-牛顿优化算法收敛速度快。

在实践中我们发现，最大的5次迭代是完全足够的。

平面的情况下 在平面情况下，SVD(等式15)产生最多四个解向量，因为相应的奇异值变小(接近于零)。在这种情况下，解是这些向量的线性组合。为了求解系数，必须求解一个带有非线性约束的方程组。我们使用下面的技巧来避免这种情况。

设 $M=[p_1,p_2,...,p_i]$ 是包括所有世界点 $3*I$ 的矩阵。 $S = MM^T$ 的特征值给了我们关于世界点分布的信息，在普通的三维情况下，矩阵S的秩为3，最小特征值不接近于零。在平面情况下，最小特征值变小，矩阵S的秩为2。如果世界点位于由两个坐标轴张成的平面上，所有世界点中的一个元素是一个常数，我们可以简单地忽略矩阵A的相应列而得到一个不同的解。

一般来说，这些点可以位于世界坐标系中的任意平面上。因此，我们使用S的特征向量作为旋转矩阵 $R_S$ ，将世界点旋转到一个新的坐标系下：

\hat{p}_i=R_S^Tp_i \tag{20}

在这里，我们可以确定坐标的常量元素，忽略矩阵A中相应的列。注意，这种转换并不改变问题的结构。SVD后得到的旋转矩阵(式19)只需旋转回原始坐标系即可：

R=R_SR\tag{21}

为了使我们的方法尽可能一般化，我们总是计算矩阵S。然后我们简单地在平面和普通情况之间切换，取决于矩阵S的秩。为了确定秩，我们使用展示秩的QR分解，并设置一个最小特征值的阈值(1e-10)。

以上是论文理论部分，接下来结合ORBSLAM3的代码，完整的ORBSLAM代码注释参考 ORB-SLAM3详细注释的代码持续更新，主要实现的函数在computePose中。

step1：要判断点的数量是否满足计算条件 因为在论文中提到，因为每个观测值会产生2个残差，所以至少需要6个点来计算公式12，所以要检验当前的点个数是否满足大于5的条件。

size_t numberCorrespondences = indices.size();
assert(numberCorrespondences > 5);

当numberCorrespondences不满足>5的条件时会发生错误(如果小于6根本进不来)。接下来定义一个标记，表明是否在平面上。

bool planar = false;

step2：计算点的方向向量的零空间 给每个向量都开辟一个零空间：

std::vector<Eigen::MatrixXd> nullspaces(numberCorrespondences);

存储世界坐标系下空间点的矩阵，3行N列，N是numberCorrespondences，即点的总个数 $points3 = \begin{bmatrix} x_1& x_2 & \cdots & x_n \\y_1&y_2 & \cdots &y_n\\z_1& z_2 & \cdots & z_n \\ \end{bmatrix}$

Eigen::MatrixXd points3(3, numberCorrespondences);

空间点向量，单个空间点的齐次坐标矩阵 $points3v = \begin{bmatrix} x_i \\y_i\\z_i \end{bmatrix}，points4v = \begin{bmatrix} x_i \\y_i\\z_i\\1 \end{bmatrix}$

points_t points3v(numberCorrespondences);
points4_t points4v(numberCorrespondences);

由于indices里保存的是提出的内点的索引值，即indices[i]是当前点坐标和向量的索引值，因此其索引并不是连续的。 $f$ 为单位向量， $p$ 为点的3D坐标。取出当前点的单位向量（方向向量）f_current ，以及点的3D坐标组合在points3中，用于接下来求解矩阵S。利用公式7.8，SVD分解计算零空间向量： ${\bf J_{v_r}(v)}=null(\bf v^T)=\begin{bmatrix} \bf r & \bf s \\ \end{bmatrix}，N=UDV^T$ 取特征值最小的那两个对应的2个特征向量，组成零空间向量nullspaces

for (size_t i = 0; i < numberCorrespondences; i++) {
          bearingVector_t f_current = f[indices[i]];
          points3.col(i) = p[indices[i]];
          Eigen::JacobiSVD<Eigen::MatrixXd, Eigen::HouseholderQRPreconditioner>
                  svd_f(f_current.transpose(), Eigen::ComputeFullV);
          nullspaces[i] = svd_f.matrixV().block(0, 1, 3, 2);
          points3v[i] = p[indices[i]];
      }

Step 3: 通过计算S的秩来判断是在平面情况还是在正常情况 设 $points3=[p_1,p_2,...,p_i]$ 是包括所有世界点 $3*I$ 的矩阵。 $S = MM^T$ ，如果矩阵S的秩为3且最小特征值不接近于0，则不属于平面条件，否则需要解决一下。

Eigen::Matrix3d planarTest = points3 * points3.transpose();
Eigen::FullPivHouseholderQR<Eigen::Matrix3d> rankTest(planarTest);

特征旋转矩阵，用在平面条件下的计算

Eigen::Matrix3d eigenRot;
eigenRot.setIdentity();

使用FullPivHouseholderQR分解可得到矩阵S的秩，秩为2时，则属于平面的情况。通过计算矩阵S的特征值和特征向量，得到特征旋转矩阵，利用公式20： $\hat{p}_i=R_S^Tp_i$ ，得到在新的坐标系下的世界点 $p'_i$ 。

if (rankTest.rank() == 2) {
	planar = true;
	 Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver(planarTest);
	 eigenRot = eigen_solver.eigenvectors().real();
	 eigenRot.transposeInPlace();
	 for (size_t i = 0; i < numberCorrespondences; i++)
	     points3.col(i) = eigenRot * points3.col(i);
	}

Step 4: 计算随机模型中的协方差矩阵 （没用到，跳过） Step 5: 构造矩阵A来完成线性方程组的构建 在公式10中，得到一个线性方程组的齐次方程组， $Au=0$ ，

\begin{bmatrix} dr \\ ds \\ \end{bmatrix}=\begin{bmatrix} {\bf r}^T \\ {\bf s}^T \\ \end{bmatrix}\lambda^{-1}_i (Rp_i+t) \tag{10}

u = [\hat{r}_{11}, \hat r_{12}, \hat{r}_{13}, \hat{r}_{21}, \hat{r}_{22}, \hat{r}_{23}, \hat{r}_{31}, \hat{r}_{32}, \hat{r}_{33},\hat{t}_1,\hat{t}_2,\hat{t}_3]^T

对单位向量 $v$ 的2个零空间向量做微分，所以有 $[dr, ds]^T$ ，一个点有2行，N个点就有 $2*N$ 行。

const int rowsA = 2 * numberCorrespondences;

对应上面 $u$ 的列数，因为旋转矩阵有9个元素，加上平移矩阵3个元素，总共12个元素

int colsA = 12;

如果世界点位于分别跨2个坐标轴的平面上，即所有世界点的一个元素是常数的时候，可简单地忽略矩阵A中对应的列，而且这不影响问题的结构本身，所以在计算公式20： $p_i' = R_S^T * p_i$ 的时候，忽略了 $r_{11},r_{21},r_{31}$ ，即第一列，对应的 $u$ 只有9个元素 $u = [\hat r_{12}, \hat{r}_{13}, \hat{r}_{22}, \hat{r}_{23}, \hat{r}_{32},\hat{r}_{33},\hat{t}_1,\hat{t}_2,\hat{t}_3]^T$ 所以A的列个数是9个。因此在构造矩阵A的时候要分平面和非平面两种情况，平面情况忽略了第一列，而非平面则完全保留所有列。

if (planar) {
     colsA = 9;
     A = Eigen::MatrixXd(rowsA, 9);
 } else
     A = Eigen::MatrixXd(rowsA, 12);
 A.setZero();

Step 6: 计算线性方程组的最小二乘解

 Eigen::MatrixXd AtPA;
 if (use_cov)
     // 有协方差信息的情况下，一般方程是 A^T*P*A*u = N*u = 0
     AtPA = A.transpose() * P * A; // setting up the full normal equations seems to be unstable
 else
     // 无协方差信息的情况下，一般方程是 A^T*A*u = N*u = 0
     AtPA = A.transpose() * A;
 // SVD分解，满秩求解V
 Eigen::JacobiSVD<Eigen::MatrixXd> svd_A(AtPA, Eigen::ComputeFullV);
 // 解就是对应奇异值最小的列向量，即最后一列
 Eigen::MatrixXd result1 = svd_A.matrixV().col(colsA - 1);

Step 7: 根据平面和非平面情况下选择最终位姿解 1.平面的情况下：暂时只估计了旋转矩阵的第1行和第2行，第3行等于第1行和第2行的叉积（这里应该是列，后面转置后成了行）。

tmp << 0.0, result1(0, 0), result1(1, 0),
         0.0, result1(2, 0), result1(3, 0),
         0.0, result1(4, 0), result1(5, 0);
tmp.col(0) = tmp.col(1).cross(tmp.col(2));
tmp.transposeInPlace();

利用Frobenious范数计算最佳的旋转矩阵，利用公式(19)： $R=U_RV_R^T$ 本质上，采用矩阵，将其元素平方，将它们加在一起并对结果平方根。计算得出的数字是矩阵的Frobenious范数，由于列向量是单列矩阵，行向量是单行矩阵，所以这些矩阵的Frobenius范数等于向量的长度（L）。

Eigen::JacobiSVD<Eigen::MatrixXd> svd_R_frob(tmp, Eigen::ComputeFullU | Eigen::ComputeFullV);
rotation_t Rout1 = svd_R_frob.matrixU() * svd_R_frob.matrixV().transpose();

如果估计出来的旋转矩阵的行列式小于0，则乘以-1（EPnP也是同样的操作）

if (Rout1.determinant() < 0)
      Rout1 *= -1.0;

因为是在平面情况下计算的，估计出来的旋转矩阵是要做一个转换的，根据公式(21)， $R=R_SR$ 其中， $R_S$ 表示特征向量的旋转矩阵注意eigenRot之前已经转置过了transposeInPlace()，所以这里Rout1在之前也转置了，即tmp.transposeInPlace()。

Rout1 = eigenRot.transpose() * Rout1;

估计最终的平移矩阵，带尺度信息的，根据公式(17)： $t={\hat{t}\over \sqrt[2]{\lVert \hat{r}_1 \rVert \lVert \hat{r}_2 \rVert}}$

double scale = 1.0 / std::sqrt(std::abs(tmp.col(1).norm() * tmp.col(2).norm()));
translation_t t = scale * translation_t(result1(6, 0), result1(7, 0), result1(8, 0));

由于在刚开始对temp做了转置，这里需要还原回去。

Rout1.transposeInPlace();

这样便可以得到4种结果，即： $T_s1=[R,t]，T_s2=[R,-t]，T_s3=[-R,t]，T_s4=[-R,-t]$ 接下来便是遍历这4种解，计算世界点 $p$ 到切线空间 $v$ 的投影的残差，对应最小的就是最好的解。

2.非平面情况下：在非平面情况下，并没有忽略第一行：

tmp << result1(0, 0), result1(3, 0), result1(6, 0),
       result1(1, 0), result1(4, 0), result1(7, 0),
       result1(2, 0), result1(5, 0), result1(8, 0);

因此尺度的计算为： $t={\hat{t}\over \sqrt[3]{\lVert \hat{r}_1 \rVert \lVert \hat{r}_2 \rVert \lVert \hat{r}_3 \rVert}}$

double scale = 1.0 /std::pow(std::abs(tmp.col(0).norm() * tmp.col(1).norm() * tmp.col(2).norm()), 1.0 / 3.0);

接下来同理，使用Frobenious计算最佳的旋转矩阵：

Eigen::JacobiSVD<Eigen::MatrixXd> svd_R_frob(tmp, Eigen::ComputeFullU | Eigen::ComputeFullV);
Rout = svd_R_frob.matrixU() * svd_R_frob.matrixV().transpose();
if (Rout.determinant() < 0)
    Rout *= -1.0;

已知公式（1），从相机坐标系到世界坐标系的转换关系是： $\lambda_iv_i=Rp_i+t$ 那么从世界坐标系到相机坐标系的转换关系为： $R^T(\lambda_iv_i -t)=p_i$ 因此先恢复平移部分的尺度再计算， $tout = R^T*t$ ：

tout = Rout * (scale * translation_t(result1(9, 0), result1(10, 0), result1(11, 0)));

在非平面情况下，一共有2种解，R,t和R,-t，利用前6个点计算重投影误差，选择残差最小的一个解。

Step 8: 利用高斯牛顿法对位姿进行精确求解，提高位姿解的精度 以上就选出了残差最小的旋转和平移解。下面用高斯-牛顿优化迭代改进相机姿态。具体地说，我们最小化Eq. 10中定义的正切空间残差。