ORB-SLAM闭环端相似变换算法

ORB-SLAM闭环端相似变换

论文：Closed-form solution of absolute orientation using unit quaternions

参考资料

[1]Horn B K P . Closed-form solution of absolute orientation using unit quaternions[J]. Journal of the Optical Society of America A, 1987, 4(4):629-642.

[2] ORB-SLAM2代码阅读笔记（十）：sim3求解

[3] 一起学ORBSLAM2（10）ORBSLAM中的相似变换矩阵的计算

问题模型

如上图，已知空间中有$n$个三维点，它们能够被两个不同的笛卡尔直角坐标系测量到，在$\mathbf{L}$坐标系下三维点坐标为$r_i^l,i=1,...,n$，在$\mathbf{R}$坐标系下三维点坐标为$r_i^r,i=1,...,n$，需要求解两坐标系间的相似变换$Sim3$：
$$\left[\begin{array}{cc}sR& t\\ 0& 1\end{array}\right]$$
​ 这个问题实际上是3D点对运动估计的问题（3D-3D），3D-3D问题一般使用ICP方法求6自由度的解，但是该问题是求相似变换，多了尺度因子$s$，因此是求7自由度的解。

​ 相似变换矩阵各参数意义可参考高翔博士《视觉SLAM十四讲》。

求解方法

1. 计算旋转$R$

​ 选取3对匹配点，在$\mathbf{L}$坐标系中为$[r_{l,1},r_{l,2},r_{l,3}]$和在$\mathbf{R}$坐标系中为$[r_{r,1},r_{r,2},r_{r,3}]$，重建左右两边的坐标系，如下图：

$$\begin{gathered} \widehat{x_l}=\frac{x_l}{||x_l||} \\ \widehat{y_l}=\frac{y_l}{||y_l||} \\ \widehat{z_l}=\widehat{x_l}\times \widehat{y_l}\text{\hspace{1em}(1)} \end{gathered}$$
其中
$$\begin{gathered} x_l=r_{l,2}-r_{l,1} \\ {y}_l=(r_{l,3}-r_{l,1})-[(r_{l,3}-r_{l,1})\cdot \widehat{x_l}]\widehat{x_l}\text{\hspace{1em}(1)} \end{gathered}$$
​ 右边坐标同理建立新的坐标系，且令：
$$\begin{gathered} M_l=|\widehat{x_l},\widehat{y_l},\widehat{z_l}| \\ {M}_r=|\widehat{x_r},\widehat{y_r},\widehat{z_r}|\text{\hspace{1em}(2)} \end{gathered}$$
​ 如果左边坐标系有一个向量$r_l$，那么计算$M_l^T{r}_l$（注：属于绝对变换，$M^T=M^{-1}$，因为$M$是正交矩阵）可以将其转换到世界坐标系下，再将其转到右边坐标系下：
$$r_r=M_r{M}_l^T{r}_l\text{\hspace{1em}(3)}$$
​ 显然可得到两坐标系间（从左侧坐标系旋转到右侧坐标系）的旋转矩阵：
$$R=M_r{M}_l^T\text{\hspace{1em}(4)}$$

2. 计算平移量$t$

​ 找$n$对匹配点对，在左右坐标系中分别为 { r l , i } \{r_{l,i}\} {rl,i​}和 { r r , i } \{r_{r,i}\} {rr,i​}，$i=1,...,n$。

​ 匹配点之间经相似变换$Sim3$：
$$r_r=sR(r_l)+r_0\text{\hspace{1em}(5)}$$
​ 其中$s$是尺度因子，$R$是旋转矩阵，$r_0$是坐标系间的平移量。

​ 但是事实上，除非匹配点对数据非常完美，否则式(5)难以精确地求出平移向量，因此我们可以使用最小二乘法的思路去求解，设置平移量的残差为：
$$e_i=r_{r,i}-sR(r_{l,i})-r_0\text{\hspace{1em}(6)}$$
​ 构建最小二乘问题：
$$min\sum _{i=1}^n||e_i|{|}^2\text{\hspace{1em}(7)}$$
​ 就匹配点对在各坐标系下的质心：
$$\overline{r_l}=\frac{1}{n}\sum _{i=1}^n{r}_{l,i},\overline{r_r}=\frac{1}{n}\sum _{i=1}^n{r}_{r,i}\text{\hspace{1em}(8)}$$
​ 定义新的坐标系：
$$r_{l,i}^{\prime }=r_{l,i}-\overline{r_l},r_{r,i}^{\prime }=r_{r,i}-\overline{r_r}\text{\hspace{1em}(9)}$$
​ 显然：
$$\sum _{i=1}^n{r}_{l,i}^{\prime }=0,\sum _{i=1}^n{r}_{r,i}^{\prime }=0\text{\hspace{1em}(10)}$$
​ 式(6)可重新表达成：
$$\begin{gathered} e_i=r_{r,i}^{\prime }-sR(r_{l,i}^{\prime })-r_0^{\prime } \\ {r}_0^{\prime }=r_0-\overline{r_r}+sR(\overline{r_l})\text{\hspace{1em}(11)} \end{gathered}$$
​ 最小二乘问题：
$$\begin{gathered} min\sum _{i=1}^n||e_i|{|}^2=min\sum _{i=1}^n||r_{r,i}^{\prime }-sR(r_{l,i}^{\prime })-r_0^{\prime }|{|}^2 \\ \text{\hspace{1em}(12)} \end{gathered}$$
​ 其中：
$$\begin{gathered} \sum _{i=1}^n||r_{r,i}^{\prime }-sR(r_{l,i}^{\prime })-r_0^{\prime }|{|}^2 \\ =\sum _{i=1}^n||r_{r,i}^{\prime }-sR(r_{l,i}^{\prime })|{|}^2-2r_0^{\prime }\cdot \sum _{i=1}^n[r_{r,i}^{\prime }-sR(r_{l,i}^{\prime })]+n||r_0^{\prime }|{|}^2\text{\hspace{1em}(13)} \end{gathered}$$
​ 根据(10)，可知式(13)的第二项为0，第一项与$r_0^{\prime }$无关，第三项不可能为负，容易知当且仅当$r_0^{\prime }=0$时，残差项之和最小，根据(11)，得到：
$$r_0=\overline{r_r}-sR(\overline{r_l})\text{\hspace{1em}(14)}$$
​ 当$r_0^{\prime }=0$时，残差项可以写成：
$$e_i=r_{r,i}^{\prime }-sR(r_{l,i}^{\prime })\text{\hspace{1em}(15)}$$
​ 最小二乘问题(12)改写成：
$$min\sum _{i=1}^n||e_i|{|}^2=min\sum _{i=1}^n||r_{r,i}^{\prime }-sR(r_{l,i}^{\prime })||\text{\hspace{1em}(16)}$$

3. 计算尺度因子$s$

现在的问题转化为寻找$s$是的式子(16)的值最小，首先存在一条约束式：
$$||R(r_{l,i}^{\prime })|{|}^2=||r_{l,i}^{\prime }|{|}^2\text{\hspace{1em}(17)}$$
利用(17)，将(16)展开得到：
$$\sum _{i=1}^n||r_{r,i}^{\prime }|{|}^2-2s\sum _{i=1}^n{r}_{r,i}^{\prime }\cdot R(r_{l,i}^{\prime })+s^2\sum _{i=1}^n||r_{l,i}^{\prime }|{|}^2\text{\hspace{1em}(18)}$$
可以将上式各项缩写，表达成如下：
$$S_r-2sD+s^2{S}_l\text{\hspace{1em}(19)}$$
对(19)进行配方，得到：
$$\sum _{i=1}^n||e_i|{|}^2=(s\sqrt{S_l}-D/\sqrt{S_l}{)}^2+(S_r{S}_l-D^2)/S_l\text{\hspace{1em}(20)}$$
(20)中只有第一项与$s$有关，因此为了使第一项的值最小，可以得到$s$：
$$s=(\sum _{i=1}^n{r}_{r,i}^{\prime }\cdot R(r_{l,i}^{\prime }))/\sum _{i=1}^n||r_{l,i}^{\prime }|{|}^2\text{\hspace{1em}(21)}$$
注：当已知两个系统中的一个系统的坐标比另一个系统的坐标精度高得多时，上述两个不对称结果中的一个可能是合适的。

---

​ 如果两组测量中的误差相似，则对误差项使用对称表达式更为合理：

​ 在最小二乘问题里，残差项(15)与下式等价：
$$e_i=\frac{1}{\sqrt{s}}{r}_{r,i}^{\prime }-\sqrt{s}R(r_{l,i}^{\prime })\text{\hspace{1em}(22)}$$
​ 然后残差项之和为：
$$\sum _{i=1}^n||e_i|{|}^2=\frac{1}{s}\sum _{i=1}^n||r_{r,i}^{\prime }|{|}^2-2\sum _{i=1}^n{r}_{r,i}^{\prime }\cdot R(r_{l,i}^{\prime })+s\sum _{i=1}^n||r_{l,i}^{\prime }|{|}^2\text{\hspace{1em}(23)}$$
​ 同样可以缩写成：
$$\frac{1}{s}{S}_r-2D+s{S}_l\text{\hspace{1em}(24)}$$
​ 对(24)配方，得到：
$$(\sqrt{s}{S}_l-\frac{1}{\sqrt{s}}{S}_r{)}^2+2(S_l{S}_r-D)\text{\hspace{1em}(25)}$$
​ 显然当$s=S_r/S_l$时使得第一项的值最小：
$$s=(\sum _{i=1}^n||r_{r,i}^{\prime }|{|}^2/\sum _{i=1}^n||r_{l,i}^{\prime }|{|}^2{)}^{\frac{1}{2}}\text{\hspace{1em}(26)}$$
对称表达式求解结果的好处是：允许我们在不知道选择的情况下确定尺度因子$s$。重要的是，旋转的确定不受我们选择比例因子的三个值之一的影响。

不过我们需要清楚的是，在ORB-SLAM2中，求尺度因子时采用了第一种方法，没有使用对称表达式。

---

​ 在每种情况下，当D尽可能大时，误差能取得最小值。也就是说，我们必须选择使${\sum }_{i=1}^n{r}_{r,i}^{\prime }\cdot R(r_{l,i}^{\prime })$尽可能大的旋转值$R$.
$$D=\sum _{i=1}^n{r}_{r,i}^{\prime }\cdot R(r_{l,i}^{\prime })\text{\hspace{1em}(26-1)}$$

---

​ 为了接下来的求解，将引入四元数表达，首先需要清楚一些定理。

​ 定义两个四元数：
$$\begin{gathered} \dot{q}=q_0+i{q}_x+j{q}_y+k{q}_z \\ \dot{r}=r_0+i{r}_x+j{r}_y+k{r}_z\text{\hspace{1em}(27)} \end{gathered}$$
​ 两四元数乘积（需要注意不满足交换律）：

$$\begin{array}{rl}\dot{r}q=& \left(r_0{q}_0-r_x{q}_x-r_y{q}_y-r_z{q}_z\right)\\ & +i\left(r_0{q}_x+r_x{q}_0+r_y{q}_z-r_z{q}_y\right)\\ & +j\left(r_0{q}_y-r_x{q}_z+r_y{q}_0+r_z{q}_x\right)\\ & +k\left(r_0{q}_z+r_x{q}_y-r_y{q}_x+r_z{q}_0\right)\end{array}$$

​ 也可以表示成矩阵形式：
$$\dot{r}\dot{q}=\left[\begin{array}{cccc}{r}_0& -r_x& -r_y& -r_z\\ r_x& r_0& -r_z& r_y\\ r_y& r_z& r_0& -r_x\\ r_z& -r_y& r_x& r_0\end{array}\right]\dot{q}=\mathbb{I}\mathbb{R}\dot{q}\text{\hspace{1em}(28)}$$
​ 或者：
$$\dot{q}\dot{r}=\left[\begin{array}{cccc}{r}_0& -r_x& -r_y& -r_z\\ r_x& r_0& r_z& -r_y\\ r_y& -r_z& r_0& r_x\\ r_z& r_y& -r_x& r_0\end{array}\right]\dot{q}=\overline{\mathbb{I}\mathbb{R}}\dot{q}\text{\hspace{1em}(29)}$$
​ 请注意，$\mathbb{I}\mathbb{R}$与$\overline{\mathbb{I}\mathbb{R}}$不同的是，右下角的3×3子矩阵是移位的。这再次说明了四元组乘法的非互换性。注意，$\mathbb{I}\mathbb{R}$和$\overline{\mathbb{I}\mathbb{R}}$的每一列（或每一行）的元素的平方之和等于：
$$r_0^2+r_x^2+r_y^2+r_z^2\text{\hspace{1em}(30)}$$
​ 实际上就是$\dot{r}\cdot \dot{r}$。

​ 因此四元数的点乘实际就是向量点乘：
$$\dot{p}\cdot \dot{q}=p_0{q}_0+p_x{q}_x+p_y{q}_y+p_z{q}_z\text{\hspace{1em}(31)}$$
​ 常用公式：

$$\begin{array}{r}(\dot{qp})\cdot (\dot{q}\dot{r})=(\dot{q}\cdot \dot{q})(\dot{p}\cdot \dot{r}),\\ (\dot{pq})\cdot \dot{r}=\dot{p}\cdot \left(\dot{r}{\dot{q}}^{\ast }\right),\end{array}$$

​ 向量可以用纯虚四元数来表示，例如有向量$\vec{r}=(x,y,z{)}^T$，可以将其表示为：
$$\dot{r}=0+ix+jy+kz\text{\hspace{1em}(32)}$$
​ 对于纯虚四元数，有如下特性：
$$\begin{gathered} \mathbb{I}{\mathbb{R}}^T=-\mathbb{I}\mathbb{R} \\ {\overline{\mathbb{I}\mathbb{R}}}^T=-\overline{\mathbb{I}\mathbb{R}}\text{\hspace{1em}(33)} \end{gathered}$$
​ 使用四元数表示旋转：
$$\dot{r}=\dot{q}\dot{r}{\dot{q}}^{\ast }\text{\hspace{1em}(34)}$$
​ 根据(28)和(29)，可以得到：
$$\dot{q}\dot{r}{\dot{q}}^{\ast }=(Q\dot{r}){\dot{q}}^{\ast }={\bar{Q}}^T(Q\dot{r})=({\bar{Q}}^{T}Q)\dot{r}\text{\hspace{1em}(35)}$$
$${\bar{Q}}^{T}Q=\left[\begin{array}{cccc}\dot{q}\cdot \dot{q}& 0& 0& 0\\ 0& \left(q_0^2+q_x^2-q_y^2-q_z^2\right)& 2\left(q_x{q}_y-q_0{q}_z\right)& 2\left(q_x{q}_z+q_0{q}_y\right)\\ 0& 2\left(q_y{q}_x+q_0{q}_z\right)& \left(q_0^2-q_x^2+q_y^2-q_z^2\right)& 2\left(q_y{q}_z-q_0{q}_x\right)\\ 0& 2\left(q_z{q}_x-q_0{q}_y\right)& 2\left(q_z{q}_y+q_0{q}_x\right)& \left(q_0^2-q_x^2-q_y^2+q_z^2\right)\end{array}\right]$$

​ 因此，若$\dot{r}$为纯虚四元数的话，${\dot{r}}^{\prime }$也是。如果$\dot{q}\cdot \dot{q}=1$，${\bar{Q}}^{T}Q$的右下角3X3子矩阵必然是正交矩阵。实际上，可以看出${\bar{Q}}^{T}Q$的右下角3X3子矩阵就是我们需要寻找的旋转矩阵$R$。并且有：
$$r^{\prime }=Rr\text{\hspace{1em}(36)}$$

4. 寻找最优旋转

​ 根据前面的四元数知识，式(26-1)可以改写成：
$$\begin{gathered} D=\sum _{i=1}^n(\dot{q}{\dot{r}}_{l,i}^{\prime }{\dot{q}}^{\ast })\cdot {\dot{r}}_{l,i}^{\prime } \\ =\sum _{i=1}^n(\dot{q}{\dot{r}}_{l,i}^{\prime })\cdot ({\dot{r}}_{l,i}^{\prime }\dot{q})\text{\hspace{1em}(37)} \end{gathered}$$

$$\dot{q}{r}_{l,i}^{\prime }=\left[\begin{array}{cccc}0& -x_{l,i}^{\prime }& -y_{l,i}^{\prime }& -z_{l,i}^{\prime }\\ x^{\prime }{}_{l,i}& 0& z_{l,i}^{\prime }& -y^{\prime }{}_{l,i}\\ y^{\prime }{}_{l,i}& -z_{l,i}^{\prime }& 0& x_{l,i}^{\prime }\\ z_{l,i}^{\prime }& y_{l,i}^{\prime }& -x_{l,i}^{\prime }& 0\end{array}\right]\dot{q}={\overline{\mathbb{R}}}_{l,i}\dot{q}$$

$${\dot{r}}_{r,i}^{\prime }\dot{q}=\left[\begin{array}{cccc}0& -x_{r,i}^{\prime }& -y_{r,i}^{\prime }& -z_{r,i}^{\prime }\\ x_{r,i}^{\prime }& 0& -z_{r,i}^{\prime }& y^{\prime }{}^{\prime }\\ y_{r,i}^{\prime }& z_{r,i}^{\prime }& 0& -x^{\prime }{}_{r,i}\\ z_{r,i}^{\prime }& -y_{r,i}^{\prime }& x_{r,i}^{\prime }& 0\end{array}\right]\dot{q}={\mathbb{R}}_{r,i}\dot{q}$$

因此(37)可以改写成：

$$\sum _{i=1}^n\left({\overline{\mathbb{R}}}_{l,i}\dot{q}\right)\cdot \left({\mathbb{R}}_{r,i}\dot{q}\right)$$
或者：
$$\sum _{i=1}^n{\dot{q}}^T{\overline{\mathbb{R}}}_{l,i}^T{\mathbb{R}}_{r,i}\dot{q}$$

按照分配率，得到：

$${\dot{q}}^T\left(\sum _{i=1}^n{\overline{\mathbb{R}}}_{l,i}^T{\mathbb{R}}_{r,i}\right)\dot{q}$$
或者:
$${\dot{q}}^{\circ }\left(\sum _{i=1}^n{N}_i\right)\cdot \dot{q}={\dot{q}}^{T}N\dot{q}$$

很容易可以证明当$N_i$矩阵是对称时，$N$也必然是对称矩阵。

存在一个矩阵$M$：
$$M=\sum _{i=1}^n{r}_{l,i}^{\prime }{r}_{r,i}^{\prime T}$$
其元素是左坐标系中匹配点的坐标与右坐标系中匹配点的坐标之和。事实证明，这个矩阵包含了解决旋转最小二乘问题所需的所有信息。我们可以通过写出M的形式来区别各个元素：
$$M=\left[\begin{array}{ccc}{S}_{xx}& S_{xy}& S_{xz}\\ S_{yx}& S_{yy}& S_{yz}\\ S_{zx}& S_{zy}& S_{zz}\end{array}\right]\text{\hspace{1em}(38)}$$
其中
$$\begin{gathered} S_{xx}=\sum _{i=1}^n{x}_{l,i}^{\prime }{x}_{r,i}^{\prime } \\ {S}_{xy}=\sum _{i=1}^n{x}_{l,i}^{\prime }{y}_{r,i}^{\prime } \\ ......andsoon\text{\hspace{1em}(39)} \end{gathered}$$
接着可以得到：

$$N=\left[\begin{array}{cccc}\left(S_{xx}+S_{yy}+S_{zz}\right)& S_{yz}-S_{zy}& S_{zx}-S_{xz}& S_{xy}-S_{yx}\\ S_{yz}-S_{zy}& \left(S_{xx}-S_{yy}-S_{zz}\right)& S_{xy}+S_{yx}& S_{zx}+S_{xz}\\ S_{zx}-S_{xz}& S_{xy}+S_{yx}& \left(-S_{xx}+S_{yy}-S_{zz}\right)& S_{yz}+S_{zy}\\ S_{xy}-S_{yx}& S_{zx}+S_{xz}& S_{yz}+S_{zy}& \left(-S_{xx}-S_{yy}+S_{zz}\right)\end{array}\right].$$

使用SVD对$N$进行分解，获取最小的特征值，它对应的特征向量就是待求四元数$q$，将其转为旋转矩阵$R$，即为使得前面的$D$最大化的旋转矩阵。

至此，相似变换的$s,R,t$都求出来了，在ORB-SLAM2中还需要通过RANSAC算法对其优化，并剔除外点。