ORBSLAM2系列-重定位中求解位姿R,t

重定位中对于位姿的求解使用了EPnP算法和ICP算法，其中EPnP算法用于将匹配点对的2D坐标转化为3D坐标（相机坐标系），然后ICP算法使用3D坐标（世界坐标系）和3D坐标（相机坐标系）求解位姿R,t。在求解的过程中，使用RANSAC策略，用于判断求解的该位姿是否合格

EPnP算法

EPnP算法流程

在视觉slam十四讲中描述过PnP算法是解决3D-2D的位姿运动问题，我在查找资料过程中发现计算机视觉life的资料很好，非常明了的讲解了EPnP算法，在这里将整个算法讲解copy到这里，方便查看：

如有涉及侵权，望告知，可删除

简单梳理流程

EPnP算法最终要得到什么：并不是直接得到了位姿，而是得到了地图点在当前相机坐标系下的坐标，然后根据世界坐标和相机坐标，通过ICP算法，就求解了位姿，因此在EPnP算法中最主要求得的就是 $p_i^c$ ，而 $p_i^c$ 是由4个控制点的相机坐标和对应的权重（不同坐标系下权重是相同的）合成的，那么算法的主要流程就是在求这两个东西：

• 1.根据数据点对，求得4个控制点的世界坐标 $c_1^w,c_2^w,c_3^w,c_4^w$
• 2.根据4个控制点的世界坐标和地图点的世界坐标，求得对应的权重 ${\alpha }_{i1},{\alpha }_{i2},{\alpha }_{i3},{\alpha }_{i4}$
• 3.权重已经求得，就差控制点的相机坐标。根据相机成像原理，列出了矩阵 $Mx=0$ 而其中 $x$ 中的量就是4个控制点的相机坐标
• 4.求解矩阵，就是 $x={\sum }_{i=1}^n{\beta }_i{v}_i$ ，就引出了 $\beta ,v$ 分别代表这 $M^{T}M$ 的特征值和特征向量（特征值为0时）
• 5.由此引出了 $N=1,2,3,4$ 的情况，对不同情况分别使用刚体不变形（空间中任意两点距离不受坐标系的变化而变化），求解了不同情况下的 $\beta ,v$
• 6.得到了 $\beta ,v$ 就能得到 $x$ ，也就是4个控制点的相机坐标，加上权重系数，就得到了地图点的相机坐标 $p_i^c$

EPnP算法的流程就是要得到了地图点的相机坐标，再加上地图点的世界坐标，就可以使用下面的ICP算法求解位姿

ICP算法

在视觉slam十四讲中描述过ICP是解决3D-3D的位姿估计问题，那么这个3D分别指的就是一个空间地图点的世界坐标系中坐标和相机坐标系中坐标，由此，也表明了ICP算法的输入和输出：

• 已知：某空间点的3D世界坐标 $p_i^{\prime }$（可以上一帧求解）和其对应的相机坐标 $p_i$（匹配后的当前帧相机坐标，一般都是经过EPnP才有的相机坐标）
• 已知：相机内参矩阵 $K$
• 未知：位姿 $R,t$

ICP算法流程

根据上面的已知和未知条件，这就是一个欧式变换问题：$p_i=R{p}_i^{\prime }+t$ ，由此可以构建一个最小二乘问题：

$$\underset{R,t}{\min }\frac{1}{2}\sum _{i=1}^n||(p_i-(R{p}_i^{\prime }+t))|{|}_2^2$$

下面就是求解这个最小二乘问题的流程：

• 1.分别求世界坐标和相机坐标的质心（质心没有下标）

$$p=\frac{1}{n}\sum _{i=1}^n(p_i),p^{\prime }=\frac{1}{n}\sum _{i=1}^n(p_i^{\prime })$$

• 2.对最小二乘问题进行适当处理：

$$\begin{array}{rl}\frac{1}{2}\sum _{i=1}^n||(p_i-(R{p}_i^{\prime }+t))|{|}_2^2& =\frac{1}{2}\sum _{i=1}^n||p_i-R{p}_i^{\prime }-t-p+R{p}^{\prime }+p-R{p}^{\prime }|{|}^2\\ & =\frac{1}{2}\sum _{i=1}^n||(p_i-p-R(p_i^{\prime }-p^{\prime }))+(p-R{p}^{\prime }-t)|{|}^2\\ & =\frac{1}{2}\sum _{i=1}^n(||p_i-p-R(p_i^{\prime }-p^{\prime })|{|}^2+||p-R{p}^{\prime }-t|{|}^2+2(p_i-p-R(p_i^{\prime }-p^{\prime }){)}^T(p-R{p}^{\prime }-t))\end{array}$$

• 3.上面式子中最后一项在求和之后为0，因此只剩下前两项，而这两项都与质心有关：设 $q_i=p_i-p,q_i^{\prime }=p_i^{\prime }-p^{\prime }$ 上面式子就转化如下并展开范数计算：

$$arg\underset{R}{\min }\frac{1}{2}\sum _{i=1}^n||q_i-R{q}_i^{\prime }|{|}^2=\frac{1}{2}\sum _{i=1}^n(q_i^T{q}_i+q_i^{\prime T}{R}^{T}T{q}_i^{\prime }-2q_i^{T}R{q}_i^{\prime })$$

• 4.上面式子中，第一项与 $R$ 无关，第二项由于$R^{t}R=I$ 也与 $R$ 无关，因此这个优化函数变为：
• • 迹的相关性质（迹是对角线之和）：
  • 二次型的迹：$x^{T}Ax=tr(x^{T}Ax)=tr(Ax{x}^T)=tr(x{x}^{T}A)$
  • 矩阵相乘的迹：$tr(AB)=tr(BA)$
  • 常数的迹依然是常数

$$\begin{gathered} \underset{R}{\min }\sum _{i=1}^n-q_i^{T}R{q}_i^{\prime }=\underset{R}{\min }\sum _{i=1}^n-tr(R{q}_i^{\prime }{q}_i^T)=\underset{R}{\min }-tr(R\sum _{i=1}^n{q}_i^{\prime }{q}_i^T) \\ 即：\underset{R}{\max }tr(RW)设W=\sum _{i=1}^n{q}_i^{\prime }{q}_i^T \end{gathered}$$

• 5.经过迹的相关性质，就变为求解 $tr(RW)$ 最大时的 $R$ 。对 $W$ 进行SVD分解：$W=U\Sigma V^T$

$$tr(RW)=tr(RU\Sigma V^T)=tr(\Sigma V^{T}RU)$$

• 6.设其中 $M=V^{T}RU$ ，因为等式右边全是正交矩阵，因此 $M$ 也是正交矩阵，因此满足 $|m_{ij}|\le 1$
• • 正交矩阵性质：正交矩阵每个元素绝对值都小于等于1，如果有一个元素为1，那么这个元素所在的行列的其余元素一定都为零。

$$设：tr(\Sigma M)=tr(\left(\begin{array}{cccc}{\sigma }_1& & & \\ & {\sigma }_2& & \\ & & ...& \\ & & & {\sigma }_i\end{array}\right)\left(\begin{array}{cccc}{m}_{11}& m_{12}& ...& m_{1i}\\ m_{21}& m_{22}& ...& m_{2i}\\ ...& ...& ...& ...\\ m_{i1}& m_{i2}& ...& mii\end{array}\right))=\sum _{i-1}^n{\sigma }_i{m}_{ii}\le \sum _{i=1}^n{\sigma }_i$$

• 7.需注意的是上面要求的是 $tr(\Sigma M)$ 的最大值，也就意味着只有当 $m_{ii}$ 最大时成立，即 $m_{ii}=1$ 且 $M$ 正交，那么 $M=E$ 得到了 $R$ ，对于 $t$ ，只需要让之前的一个式子为0可求得：

$$\begin{gathered} 根据：E=M=V^{T}RU\Rightarrow R=U{V}^T \\ 根据：p-R{p}^{\prime }-t=0\Rightarrow t=p-R{p}^{\prime } \end{gathered}$$

ORBSLAM2中使用

ORBSLAM2中的使用其实就是上面的算法流程，下面是参考计算机视觉life的内容，比较精简：

RANSAC策略

在初始化求解基础矩阵F或者单应矩阵H时使用过RANSAC策略，那时候表明过：RANSAC(Random Sample Consensus)随机采样一致性是从一组含有外点的数据集中，通过迭代的方式估计出符合该数据集的数学模型的参数。因此，它也可以用来检测出数据集中的外点。总体思想就是通过数据集中的一部分点建立符合内点的数学模型，然后计算其他点与该模型之间的距离，超过阈值就认为是外点，重复迭代k次取内点多的那一次结果。

在初始化时使用了误差分数对结果进行筛选，而在重定位中，是迭代多次选取内点数量多的那一次结果（就如上面说的），而这个判断该点是否为外点，就是使用的重投影误差，将3D点投影到当前帧图像上，计算其与匹配的2D点坐标的最小二乘误差，具体流程如下：

• 1.从匹配的3D-2D点对中随机找4个对，作为计算位姿使用的输入
• 2.使用随机的4个点对经过EPnP算法和ICP算法，计算位姿R,t
• 3.使用计算得到的位姿，对每个点对（整个匹配点对）计算其是否为外点：根据重投影误差是否超过阈值判断
• 4.循环多次上面的过程（迭代多次），当计算的当前位姿的内点数量大于阈值，就进行精确求解：
• • 4-1.使用内点经过EPnP算法和ICP算法，计算位姿R,t（不是4个点了）
  • 4-2.使用计算得到的位姿，对每个点对计算其是否为外点（看看精确位姿是否有外点变为内点）：根据重投影误差是否超过阈值判断
  • 4-3.如果精确位姿的内点数量足够，就返回这个位姿，否则退出来这个精确求解，继续进行粗糙求解
• 5.如果有精确位姿并且内点数量足够，就直接返回位姿，就结束了重定位的位姿求解过程；如果精确求解后没有达到要求的内点数量，就选择一个差不多的位姿返回（但是也要大于最小的内点数量阈值）

总结

在重定位中求解位姿的具体函数在代码中详细的注释，就不再一一列举，因为整个过程就如上面所介绍的算法流程一样，一一对比就能看明白。