ORBSLAM3中的MLPnP在重定位时计算当前帧和候选帧的位姿变换

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

PnP的任务是找到一个旋转R∈SO(3)R \in SO(3)R∈SO(3)和平移t∈R3t \in \mathbb R^3t∈R3，将世界坐标系下的点pip_ipi映射到相机坐标系下的观测点viv_ivi，即：
λivi=Rpi+t(1)\lambda_iv_i=Rp_i+t \tag{1}λivi=Rpi+t(1)
这里的λi\lambda_iλi是每个点的深度，观测值viv_ivi是测量得到的结果，因此该不确定量具有单位长度∥vi∥=1\lVert v_i \rVert=1∥vi∥=1。下图中描述了参数和观测值：

每个方位向量viv_ivi上的平面是由各自的零空间向量rir_iri和sis_isi张成的。

观测与不确定性传播
设标定后的相机对世界点pi∈R3p_i \in \mathbb R^3pi∈R3进行观测，在像平面的不确定观测为：
x′=[x′y′]，Σx′x′=[σx′2σx′y′σy′x′σy′2]\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}x′=[x′y′]，Σx′x′=[σx′2σy′x′σx′y′σy′2]
关于观测点的不确定性用二维协方差矩阵Σx′x′Σ_{\bf x'\bf x'}Σx′x′来描述。
通过正投影π\piπ(K−1K^{-1}K−1)将图像点x′\bf x'x′变换到相机坐标系下的三维方向上（不确定深度信息）。
x=πx′==[xy1]，Jπ=[∂πx′∂x′∂πx′∂y′∂πy′∂x′∂πy′∂y′00]\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}x=πx′==⎣⎡xy1⎦⎤，Jπ=⎣⎢⎡∂x′∂πx′∂x′∂πy′0∂y′∂πx′∂y′∂πy′0⎦⎥⎤
JpiJ_{pi}Jpi为正投影矩阵的雅可比，因此利用图像点x′\bf x'x′的不确定性传播，可到的相机点x\bf xx的协方差矩阵：
Σxx=JπΣx′x′JπT=[σx2σxy0σyxσy20000]Σ_{\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}Σxx=JπΣx′x′JπT=⎣⎡σx2σyx0σxyσy20000⎦⎤
其中协方差矩阵的秩为2，因此它是奇异的，不可逆的。接下来球面归一化产生了最终的观察表达式，称之为方位向量：
v=[vxvyvz]=x∥x∥，Σvv=[σvx2σvxvyσvxvzσvyvxσvy2σvyvzσvzvxσvzvyσvz2]\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}v=⎣⎡vxvyvz⎦⎤=∥x∥x，Σvv=⎣⎡σvx2σvyvxσvzvxσvxvyσvy2σvzvyσvxvzσvyvzσvz2⎦⎤
根据协方差的传播：
Σvv=JΣxxJT，J=1∥x∥(I3−vvT)Σ_{\bf {vv}}=\bf JΣ_{\bf xx}\bf J^T，\bf J={1\over \lVert \bf x \rVert}(\bf I_3 -vv^T)Σvv=JΣxxJT，J=∥x∥1(I3−vvT)
此时的协方差矩阵ΣxxΣ_{\bf xx}Σxx依然是奇异的。

向量的零空间

零空间定义：已知AAA为一个m∗nm*nm∗n矩阵。A的零空间（nullspace），又称核（kernel），是一组由下列公式定义的nnn维向量：ker(A)={x∈Cn:Ax=0}ker(A)=\{x \in \mathbb C^n:Ax=0\}ker(A)={x∈Cn:Ax=0}即线性方程组Ax=0Ax=0Ax=0 的所有解xxx的集合。
在数学中，一个算子AAA的零空间是方程Av=0Av=0Av=0的所有解vvv的集合。它也叫做A的核，核空间。用集合建造符号表示为Null(A)={v∈V:Av=0}Null(A)=\{v \in V:Av=0\}Null(A)={v∈V:Av=0}
vvv的零空间张成一个二维坐标系，其轴rrr和sss垂直于vvv，并位于vvv的切线空间：
Jvr(v)=null(vT)=[rs]=[r1s1r2s2r3s3](7){\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}Jvr(v)=null(vT)=[rs]=⎣⎡r1r2r3s1s2s3⎦⎤(7)
函数null(⋅)null(·)null(⋅)计算vvv的奇异值分解(SVD)，取两个零特征值对应的两个特征向量。即：
vTr=0\bf v^T r=0 vTr=0vTs=0\bf v^T s=0vTs=0
进一步假设Jvr{\bf J}_{{\bf v}_r}Jvr是一个标准正交矩阵，JvrT(v)Jvr(v)=I2{\bf J}^T_{{\bf v}_r}({\bf v}){\bf J}_{{\bf v}_r}({\bf v})={\bf I}_2JvrT(v)Jvr(v)=I2。这个Jvr{\bf J}_{{\bf v}_r}Jvr还表示了从正切空间到原始向量的变换的雅可比矩阵?\color{red}{?}?，因此，JvrT{\bf J}^T_{{\bf v}_r}JvrT可以得到从原始齐次向量vvv到其简化后的等价向量vrv_rvr的变换。
vr=[drds]=JvrT(v)v=0(8){\bf v}_r=\begin{bmatrix} dr \\ ds \\ \end{bmatrix}={\bf J}^T_{{\bf v}_r}({\bf v})\bf v=\bf 0 \tag{8}vr=[drds]=JvrT(v)v=0(8)
与非奇异的协方差：
Σvrvr=JvrT(v)ΣvvJvr(v)=[σvrx2σvrxy′σvrxyσvry2](9)Σ_{{\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}Σvrvr=JvrT(v)ΣvvJvr(v)=[σvrx2σvrxyσvrxy′σvry2](9)
另一种看待vrv_rvr的方式是把它看作是切线空间中的残差。在下面，我们将通过最小化正切空间的残差，利用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
[drds]=[rTsT]λi−1(Rpi+t)(10)\begin{bmatrix} dr \\ ds \\ \end{bmatrix}=\begin{bmatrix} {\bf r}^T \\ {\bf s}^T \\ \end{bmatrix}\lambda^{-1}_i (Rp_i+t) \tag{10}[drds]=[rTsT]λi−1(Rpi+t)(10)
其中λ≠0\lambda \neq 0λ=0。因此，如果我们知道相机的绝对方位，世界点pip_ipi在vvv的正切空间中的投影，应该会得到相同的简化后的坐标。由公式10可得到：
0=r1(r^11px+r^12py+r^13pz+t^1)+r2(r^21px+r^22py+r^23pz+t^2)+r3(r^31px+r^32py+r^33pz+t^3)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=r1(r^11px+r^12py+r^13pz+t^1)+r2(r^21px+r^22py+r^23pz+t^2)+r3(r^31px+r^32py+r^33pz+t^3)0=s1(r^11px+r^12py+r^13pz+t^1)+s2(r^21px+r^22py+r^23pz+t^2)+s3(r^31px+r^32py+r^33pz+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)0=s1(r^11px+r^12py+r^13pz+t^1)+s2(r^21px+r^22py+r^23pz+t^2)+s3(r^31px+r^32py+r^33pz+t^3)
现在，两个等式的未知数都是线性的，可以将他们叠加到一个矩阵A中，得到一个线性方程组的齐次方程组：
Au=0(12)\bf Au=0 \tag{12}Au=0(12)
u=[r^11,r^12,r^13,r^21,r^22,r^23,r^31,r^32,r^33,t^1,t^2,t^3]Tu = [\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]^Tu=[r^11,r^12,r^13,r^21,r^22,r^23,r^31,r^32,r^33,t^1,t^2,t^3]T。由于每个观测产生两个残差，求解公式12至少需要5个点。利用不相关的观测结果，给出了随机模型：
P=[Σvr1vr1−1⋯0⋮⋱⋮0⋯Σvrivri−1](13)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}P=⎣⎢⎢⎡Σvr1vr1−1⋮0⋯⋱⋯0⋮Σvrivri−1⎦⎥⎥⎤(13)
最终的正规方程为：
ATPAu=Nu=0(14)A^TPAu=Nu=0 \tag{14}ATPAu=Nu=0(14)
在∥u∥=1\lVert u\rVert=1∥u∥=1的条件下，找到uuu使得等式14最小化，使用SVD分解：
N=UDVT(15)N=UDV^T\tag{15}N=UDVT(15)
解为V中对应于D中最小奇异值的特定列：
R^=[r^11r^12r^13r^21r^22r^23r^31r^32r^33]，t^=[t^1t^2t^3](16)\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}R^=⎣⎡r^11r^21r^31r^12r^22r^32r^13r^23r^33⎦⎤，t^=[t^1t^2t^3](16)
它由一个尺度因子决定，因为平移部分t^\hat{t}t^仅指向正确的方向。这个尺度可以从现实中恢复，旋转矩阵R^\hat{R}R^每一列的模必须等于1，因此最终的平移为：
t=t^∥r^1∥∥r^2∥∥r^3∥3(17)t={\hat{t}\over \sqrt[3]{\lVert \hat{r}_1 \rVert \lVert \hat{r}_2 \rVert \lVert \hat{r}_3 \rVert}}\tag{17} t=3∥r^1∥∥r^2∥∥r^3∥t^(17)
约束表明，这9个旋转参数并没有定义一个正确的旋转矩阵。可以通过SVD分解计算R^\hat{R}R^：
R^=URDRVRT(18)\hat{R}=U_RD_RV_R^T\tag{18}R^=URDRVRT(18)
最小化Frobenius范式的最佳矩阵为：
R=URVRT(19)R=U_RV_R^T\tag{19}R=URVRT(19)
到此为止，获得了相机绝对姿态的线性ML估计。为了提高精度，采用了非线性的优化方法。对初始估计进行后续的优化是一个常见的过程。

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

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

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

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

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

一般来说，这些点可以位于世界坐标系中的任意平面上。因此，我们使用S的特征向量作为旋转矩阵RSR_SRS，将世界点旋转到一个新的坐标系下：
p^i=RSTpi(20)\hat{p}_i=R_S^Tp_i \tag{20}p^i=RSTpi(20)

在这里，我们可以确定坐标的常量元素，忽略矩阵A中相应的列。注意，这种转换并不改变问题的结构。SVD后得到的旋转矩阵(式19)只需旋转回原始坐标系即可：
R=RSR(21)R=R_SR\tag{21}R=RSR(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=[x1x2⋯xny1y2⋯ynz1z2⋯zn]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}points3=⎣⎡x1y1z1x2y2z2⋯⋯⋯xnynzn⎦⎤

Eigen::MatrixXd points3(3, numberCorrespondences);

空间点向量，单个空间点的齐次坐标矩阵
points3v=[xiyizi]，points4v=[xiyizi1]points3v = \begin{bmatrix} x_i \\y_i\\z_i \end{bmatrix}，points4v = \begin{bmatrix} x_i \\y_i\\z_i\\1 \end{bmatrix}points3v=⎣⎡xiyizi⎦⎤，points4v=⎣⎢⎢⎡xiyizi1⎦⎥⎥⎤

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

由于indices里保存的是提出的内点的索引值，即indices[i]是当前点坐标和向量的索引值，因此其索引并不是连续的。fff为单位向量，ppp为点的3D坐标。
取出当前点的单位向量（方向向量）f_current ，以及点的3D坐标组合在points3中，用于接下来求解矩阵S。
利用公式7.8，SVD分解计算零空间向量：Jvr(v)=null(vT)=[rs]，N=UDVT{\bf J_{v_r}(v)}=null(\bf v^T)=\begin{bmatrix} \bf r & \bf s \\ \end{bmatrix}，N=UDV^TJvr(v)=null(vT)=[rs]，N=UDVT
取特征值最小的那两个对应的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=[p1,p2,...,pi]points3=[p_1,p_2,...,p_i]points3=[p1,p2,...,pi]是包括所有世界点3∗I3*I3∗I的矩阵。S=MMTS = MM^TS=MMT，如果矩阵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：p^i=RSTpi\hat{p}_i=R_S^Tp_ip^i=RSTpi，得到在新的坐标系下的世界点pi′p'_ipi′。

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=0Au=0Au=0，
[drds]=[rTsT]λi−1(Rpi+t)(10)\begin{bmatrix} dr \\ ds \\ \end{bmatrix}=\begin{bmatrix} {\bf r}^T \\ {\bf s}^T \\ \end{bmatrix}\lambda^{-1}_i (Rp_i+t) \tag{10}[drds]=[rTsT]λi−1(Rpi+t)(10)u=[r^11,r^12,r^13,r^21,r^22,r^23,r^31,r^32,r^33,t^1,t^2,t^3]Tu = [\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]^Tu=[r^11,r^12,r^13,r^21,r^22,r^23,r^31,r^32,r^33,t^1,t^2,t^3]T
对单位向量vvv的2个零空间向量做微分，所以有[dr,ds]T[dr, ds]^T[dr,ds]T，一个点有2行，N个点就有2∗N2*N2∗N行。

const int rowsA = 2 * numberCorrespondences;

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

int colsA = 12;

如果世界点位于分别跨2个坐标轴的平面上，即所有世界点的一个元素是常数的时候，可简单地忽略矩阵A中对应的列，而且这不影响问题的结构本身，所以在计算公式20：pi′=RST∗pip_i' = R_S^T * p_ipi′=RST∗pi的时候，忽略了r11,r21,r31r_{11},r_{21},r_{31}r11,r21,r31，即第一列，对应的uuu只有9个元素u=[r^12,r^13,r^22,r^23,r^32,r^33,t^1,t^2,t^3]Tu = [\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]^Tu=[r^12,r^13,r^22,r^23,r^32,r^33,t^1,t^2,t^3]T所以A的列个数是9个。
因此在构造矩阵A的时候要分平面和非平面两种情况，平面情况忽略了第一列，而非平面则完全保留所有列。

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

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

 Eigen::MatrixXd AtPA;if (use_cov)// 有协方差信息的情况下，一般方程是 A^T*P*A*u = N*u = 0AtPA = A.transpose() * P * A; // setting up the full normal equations seems to be unstableelse// 无协方差信息的情况下，一般方程是 A^T*A*u = N*u = 0AtPA = A.transpose() * A;// SVD分解，满秩求解VEigen::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=URVRTR=U_RV_R^TR=URVRT
本质上，采用矩阵，将其元素平方，将它们加在一起并对结果平方根。计算得出的数字是矩阵的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=RSRR=R_SRR=RSR其中，RSR_SRS表示特征向量的旋转矩阵
注意eigenRot之前已经转置过了transposeInPlace()，所以这里Rout1在之前也转置了，即tmp.transposeInPlace()。

Rout1 = eigenRot.transpose() * Rout1;

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

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种结果，即：
Ts1=[R,t]，Ts2=[R,−t]，Ts3=[−R,t]，Ts4=[−R,−t]T_s1=[R,t]，T_s2=[R,-t]，T_s3=[-R,t]，T_s4=[-R,-t]Ts1=[R,t]，Ts2=[R,−t]，Ts3=[−R,t]，Ts4=[−R,−t]
接下来便是遍历这4种解，计算世界点ppp到切线空间vvv的投影的残差，对应最小的就是最好的解。

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=t^∥r^1∥∥r^2∥∥r^3∥3t={\hat{t}\over \sqrt[3]{\lVert \hat{r}_1 \rVert \lVert \hat{r}_2 \rVert \lVert \hat{r}_3 \rVert}}t=3∥r^1∥∥r^2∥∥r^3∥t^

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），从相机坐标系到世界坐标系的转换关系是：
λivi=Rpi+t\lambda_iv_i=Rp_i+t λivi=Rpi+t那么从世界坐标系到相机坐标系的转换关系为：
RT(λivi−t)=piR^T(\lambda_iv_i -t)=p_i RT(λivi−t)=pi因此先恢复平移部分的尺度再计算，tout=RT∗ttout = R^T*ttout=RT∗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中定义的正切空间残差。

ORBSLAM3中的MLPnP在重定位时计算当前帧和候选帧的位姿变换相关推荐

linux内核全局变量重定位,关于可重定位文件中全局变量的一个重定位疑惑，借各位牛刀一用^...
/// 不需要牛刀,不需要阅读源码,如果只是为解决109的含义.楼主执行的查询命令readelf -S test2.o [ 8] .symtab SYMTAB ...
Ubuntu16.04中的可重定位目标文件
最近在看<CSAPP>这本神书,其中看到了第七章链接中的可重定位目标文件,自己动手在ubuntu16.04上试了一试,发现有很多都做了一些改动,在此记录 1.源程序 main.c stat ...
了解动态链接（六）—— 重定位表
柳条青青,南风熏熏,幻化奇峰瑶岛,一天的黄云白云,那边麦浪中间,有农妇笑语殷殷.问后园豌豆肥否,问杨梅可有鸟来偷:好几天不下雨了,玫瑰花还未曾红透:梅夫人今天进城去,且看她有新闻无有.-- 徐志摩·夏 ...
Tiny6410之重定位代码到SDRAM
在上一章中,将代码重定位到了SRAM中,但是这样的做法作用不大.正确的做法的是将代码重定位到更大的主存中,即DRAM.Tiny6410的DRAM控制寄存器最多只能支持两个同一类型的芯片.每个芯片最多可 ...
【论文笔记】2019 基于激光点云pole检测的重定位方法 Long-Term Urban Vehicle Localization Using Pole Landmarks
https://github.com/acschaefer/polex 本文提出了一个基于激光三维点云的二维车辆定位系统,其根据道路场景中的 "pole landmarks" (极 ...
一文详解回环检测与重定位
标题:VINS-Mono代码解读-回环检测与重定位 pose graph loop closing 作者:Manii 来源:https://blog.csdn.net/qq_41839222/cate ...
重定位相关知识，为什么要重定位
在NT环境下隐藏进程,也就是说在用户不知情的条件下,执行自己的代码的方法有很多种,比如说使用注册表插入DLL,使用Windows挂钩等等.其中比较有代表性的是Jeffrey Richer在<Wi ...
动画重定位（相同骨架）
https://docs.unrealengine.com/latest/CHN/Engine/Animation/AnimationRetargeting/index.html 本页面的内容: 为何 ...
U-Boot的重定位实现机制
获取当前芯片平台的相关信息为了深入了解ARM 64位芯片架构,笔者为u-boot添加了archinfo命令,以获取CPU当前的工作状态等信息.不过在增加完整的64位ARM架构信息查看功能之前,笔者首 ...

ORBSLAM3中的MLPnP在重定位时计算当前帧和候选帧的位姿变换

ORBSLAM3中的MLPnP在重定位时计算当前帧和候选帧的位姿变换相关推荐

最新文章

热门文章