(01)ORB-SLAM2源码无死角解析-(57) 闭环线程→计算Sim3:理论推导(1)求解R,使用四元数

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析链接如下(本文内容来自计算机视觉life ORB-SLAM2 课程课件):
(01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/123092196
 
$文末正下方中心提供了本人联系方式，点击本人照片即可显示WX\to 官方认证$
 

一、前言

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

在上一篇博客中，推导出了最佳平移 ${\mathbf{t}}^{\ast }$ 尺度尺度 $s^{\ast }$

$${\mathbf{t}}^{\ast }=\bar{Q}-s\mathbf{R}\bar{P}{s}^{\ast }=\sqrt{\frac{S_Q}{S_P}}=\sqrt{\frac{{\sum }_{i=1}^n{\Vert Q_i^{\prime }\Vert }^2}{{\sum }_{i=1}^n{\Vert P_i^{\prime }\Vert }^2}}\text{\hspace{1em}(01)}$$另外，其中的(21)式这里还会用到，所以复写如下：$$\begin{array}{rl}\sum _{i=1}^n{\Vert e_i\Vert }^2& =\sum _{i=1}^n{\Vert \frac{1}{\sqrt{s}}{Q}_i^{\prime }-\sqrt{s}\mathbf{R}{P}_i^{\prime }\Vert }^2\\ & =\frac{1}{s}\underset{S_Q}{\underbrace{\sum _{i=1}^n{\Vert Q_i^{\prime }\Vert }^2}}-\underset{D}{\underbrace{2\sum _{i=1}^n{Q}_i^{\prime }\mathbf{R}{P}_i^{\prime }}}+s\underset{S_P}{\underbrace{\sum _{i=1}^n{\Vert \mathbf{R}{P}_i^{\prime }\Vert }^2}}\\ & =\frac{1}{s}{S}_Q-2D+s{S}_P\\ & ={\left(\sqrt{s{S}_P}-\sqrt{\frac{S_Q}{s}}\right)}^2+2\left(S_P{S}_Q-D\right)\end{array}\text{\hspace{1em}(02)}$$其中第二项只与$\mathbf{R}$相关，里的 $S_p$，$S_q$ 都是平方项，所以令第二项里的 $D={\sum }_{i=1}^n{Q}_i^{\prime }\mathbf{R}{P}_i^{\prime }$ 最大时，$\mathbf{R}$ 取得最优解。下面我们考虑用四元数来代替矩阵来表达旋转，那么为什么用四元数而不是矩阵表达旋转？

        ①因为直接使用矩阵必须要保证矩阵的正交性等约束，这个约束太强了，会带来很多困难。
        ②四元数只需要保证模值为1的约束，简单很多，方便推导。

针对于四元数的讲解，这里就不进行详细的推导与理解了，目前本人还没有编写四元数的相关文章，确实其也是一个比较复杂的东西，不过已经有规划了。后续有专门的文章对四元数进行讲解。这里展示暂时只列举一些四元数的性质。
 

二、四元数

假设空间三维点 $P=[x,y,z]$，用一个虚四元数来表示为 $\dot{p}=[0,x,y,z{]}^T$

$性质1：$用四元数来对三维点进行旋转，旋转用一个单位四元数 $\dot{p}$ 来表示，则 $\dot{p}$ 旋转后的三维点用四元数表示为：$${\dot{p}}^{\prime }=\dot{q}\dot{p}{\dot{q}}^{-1}=\dot{q}\dot{p}{\dot{q}}^{\ast }\text{\hspace{1em}(03)}$$四元数 $\dot{p^{\prime }}$ 的虚部取出即为旋转后的坐标。其中 ${\dot{q}}^{\ast }$ 表示取 $\dot{q}$ 的共轭。
 
$性质2：$三个四元数满足如下条件。直接相乘的形式，表示四元数乘法，中间的 $\cdot$ 表示向量点乘。$$\dot{p}\cdot \left(\dot{r}{\dot{q}}^{\ast }\right)=(\dot{p}\dot{q})\cdot \dot{r}\text{\hspace{1em}(04)}$$

$性质3：$假设四元数 $\dot{r}=\left[r_0,r_x,r_y,r_z\right]$，则有：$$\dot{r}\dot{q}=\left[\begin{array}{rrrr}{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{R}\dot{q}\dot{q}\dot{r}=\left[\begin{array}{rrrr}{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{R}}\dot{q}\text{\hspace{1em}(05)}$$ 其中 $\mathbb{R}$, $\overline{\mathbb{R}}$ 都是 4x4 的对称矩阵。
 
$提示：$ 其实呢，大家暂时可以不去关心什么四元数，只要知道可以用两个四元数相乘来表示旋转就可以了。
 

三、理论推导

根据上一篇博客的推导，式(02)的 $D={\sum }_{i=1}^n{Q}_i^{\prime }\mathbf{R}{P}_i^{\prime }$ 最大时取得 $\mathbf{R}$ 的最优解。
 
$(1)：$利用前面的性质，我们现在的的代价函数可以做如下变换(先根据性质1，再根据性质2)：$$\begin{array}{rl}\sum _{i=1}^n{Q}_i^{\prime }R{P}_i^{\prime }& =\sum _{i=1}^n\left({\dot{Q}}_i^{\prime }\right)\cdot \left(\dot{q}{\dot{P}}_i^{\prime }{\dot{q}}^{\ast }\right)=\sum _{i=1}^n\left({\dot{Q}}_i^{\prime }\right)\cdot \left((\dot{q}{\dot{P}}_i{)}^{\prime }{\dot{q}}^{\ast }\right)=\sum _{i=1}^n\left({\dot{Q}}_i^{\prime }\dot{q}\right)\cdot \left(\dot{q}{\dot{P}}_i^{\prime }\right)\end{array}\text{\hspace{1em}(06)}$$

$(2)：$根据性质3，现在再对上面的符号做一些具体化：$$Q_i^{\prime }={\left[Q_{i,x}^{\prime },Q_{i,y}^{\prime },Q_{i,z}^{\prime }\right]}^T{P}_i^{\prime }={\left[P_{i,x}^{\prime },P_{i,y}^{\prime },P_{i,z}^{\prime }\right]}^T\text{\hspace{1em}(07)}$$$${\dot{Q}}_i^{\prime }\dot{q}=\left[\begin{array}{cccc}0& -Q_{i,x}^{\prime }& -Q_{i,y}^{\prime }& -Q_{i,z}^{\prime }\\ Q_{i,x}^{\prime }& 0& -Q_{i,z}^{\prime }& Q_{i,y}^{\prime }\\ Q_{i,y}^{\prime }& Q_{i,z}^{\prime }& 0& -Q_{i,x}^{\prime }\\ Q_{i,z}^{\prime }& -Q_{i,y}^{\prime }& Q_{i,x}^{\prime }& 0\end{array}\right]\dot{q}={\mathbb{R}}_{\mathbb{Q},\dot{i}}\dot{q}\text{\hspace{1em}(08)}$$$$\dot{q}{\dot{P}}_i^{\prime }=\left[\begin{array}{cccc}0& -P_{i,x}^{\prime }& -P_{i,y}^{\prime }& -P_{i,z}^{\prime }\\ P_{i,x}^{\prime }& 0& P_{i,z}^{\prime }& -P_{i,y}^{\prime }\\ P_{i,y}^{\prime }& -P_{i,z}^{\prime }& 0& P_{i,x}^{\prime }\\ P_{i,z}^{\prime }& P_{i,y}^{\prime }& -P_{i,x}^{\prime }& 0\end{array}\right]\dot{q}=\overline{{\mathbb{R}}_{\mathbb{P},i}}\dot{q}\text{\hspace{1em}(09)}$$

$(3)：$根据性质3带入上式如下：$$\sum _{i=1}^n\left({\dot{Q}}_i^{\prime }\dot{q}\right)\cdot \left(\dot{q}{\dot{P}}_i^{\prime }\right)==\sum _{i=1}^n\left({\mathbb{R}}_{\mathbb{Q},\mathrm{i}}\dot{q}\right)\cdot \left(\overline{{\mathbb{R}}_{\mathbb{P},\mathrm{i}}}\dot{q}\right)==\sum _{i=1}^n{\dot{q}}^T{\mathbb{R}}_{\mathbb{Q},\mathrm{i}}^{\mathbb{T}}\overline{{\mathbb{R}}_{\mathbb{P},\mathrm{i}}}\dot{q}={\dot{q}}^T\left(\sum _{i=1}^n{\mathbb{R}}_{\mathbb{Q},\mathrm{i}}^{\mathbb{T}}\overline{{\mathbb{R}}_{\mathbb{P},\mathrm{i}}}\right)\dot{q}\text{\hspace{1em}(10)}$$

$(4)：$为了方便后续的讨论，令：$$N=\sum _{i=1}^n{\mathbb{R}}_{\mathbb{Q},\mathrm{i}}^{\mathbb{T}}\overline{{\mathbb{R}}_{\mathbb{P},\mathrm{i}}}\Rightarrow {\dot{q}}^T\left(\sum _{i=1}^n{\mathbb{R}}_{\mathbb{Q},\mathrm{i}}^{\mathbb{T}}\overline{{\mathbb{R}}_{\mathbb{P},\mathrm{i}}}\right)\dot{q}={\dot{q}}^{T}N\dot{q}\text{\hspace{1em}(11)}$$
$(5)：$这里定义：$$\begin{array}{rl}M& =\sum _{i=1}^n{P}_i^{\prime }{Q}_i^T=\left[\begin{array}{lll}{S}_{xx}& S_{xy}& S_{xz}\\ S_{yx}& S_{yy}& S_{yz}\\ S_{zx}& S_{zy}& S_{zz}\end{array}\right]\end{array}\text{\hspace{1em}(12)}$$也就是说$S_{xx}={\sum }_{i=1}^n{P}_{i,x}{Q}_{i,x}$，$S_{xy}={\sum }_{i=1}^n{P}_{i,x}{Q}_{i,y}$$\dots$，元素依此类推。 引入 $M$ 是为了方便用其元素来表示 $N$.
 
$(6)：$将 $M$ 的元素带入到 $N$ 中结果如下：$$\begin{array}{rl}N& =\sum _{i=1}^n{\mathbb{R}}_{\mathbb{Q},\mathrm{i}}^{\mathbb{T}}\overline{{\mathbb{R}}_{\mathbb{P},\mathrm{i}}}\\ & =\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]\end{array}\text{\hspace{1em}(13)}$$然后我们对 $N$ 进行特征值分解，求得最大特征值对应的特征向量就是待求的用四元数表示的旋转，
($这部分内容在原$$论文中有讲清楚原由$)，注意这里旋转的方向是 { P i } → { Q i } \left\{P_{i}\right\} \rightarrow\left\{Q_{i}\right\} {Pi​}→{Qi​}。
 
$(7)：$假设单位四元数为 $\dot{q}=w+x\mathbf{i}+y\mathbf{j}+z\mathbf{k}$，其中 $\Vert \dot{q}\Vert =x^2+y^2+z^2+w^2=1$。那么给定一个单位四元数，可以构造旋转矩阵：$$\mathbf{R}(\dot{q})=\left[\begin{array}{ccc}1-2y^2-2z^2& 2xy-2zw& 2xz+2yw\\ 2xy+2zw& 1-2x^2-2z^2& 2yz-2xw\\ 2xz-2yw& 2yz+2xw& 1-2x^2-2y^2\end{array}\right]\text{\hspace{1em}(14)}$$另外，也可以先把四元数转换成旋转向量，然后再转换成旋转矩阵(源码中使用的就是这种方式)，推导过程就不在这里进行讲解了，因为涉及到的东西较多：
$$\begin{array}{l}\theta =2\arccos w\text{vec}={\left[n_x,n_y,n_z\right]}^T={\left[x,y,z\right]}^T/\sin \frac{\theta }{2}\end{array}\text{\hspace{1em}(15)}$$然后再调用cv::Rodrigues(vec,mR12i)函数，即可以把旋转向量转换成旋转矩阵。
 

四、总结

在进行推导讲解的时候，是先求出平移 $\mathbf{t}$、尺度 $s$。但是：
$${\mathbf{t}}^{\ast }=\bar{Q}-s\mathbf{R}\bar{P}{s}^{\ast }=\sqrt{\frac{S_Q}{S_P}}=\sqrt{\frac{{\sum }_{i=1}^n{\Vert Q_i^{\prime }\Vert }^2}{{\sum }_{i=1}^n{\Vert P_i^{\prime }\Vert }^2}}\text{\hspace{1em}(16)}$$很明显 平移 $\mathbf{t}$ 依赖尺度 $s$ 与 旋转 $\mathbf{R}$，所以在实际的过程中，应该先求解 $\mathbf{t}$ 依赖尺度 $s$ 与 旋转 $\mathbf{R}$。所以在实际应用的时候流程一般如下。

$(1)：$先计算旋转矩阵 $\mathbf{R}$，首先构建：$$\begin{array}{rl}M& =\sum _{i=1}^n{P}_i^{\prime }{Q}_i^T=\left[\begin{array}{lll}{S}_{xx}& S_{xy}& S_{xz}\\ S_{yx}& S_{yy}& S_{yz}\\ S_{zx}& S_{zy}& S_{zz}\end{array}\right]\end{array}\text{\hspace{1em}(17)}$$然后得到矩阵 $N$，对 $N$ 进行特征值分解，求得最大特征值对应的特征向量就是待求的用四元数表示的旋转，再把该四元数转换成旋转矩阵 $\mathbf{R}$。
 
$(2)：$有两方式来计算尺度 $s$，第一种是非对称性尺度，依赖转矩阵 $\mathbf{R}$(ORBSLAM使用)；第二种是对称性尺度，不依赖旋转矩阵(推荐)；其计算方式分别如下：
$$s=\frac{D}{S_P}=\frac{{\sum }_{i=1}^n{Q}_i^{\prime }\mathbf{R}{P}_i^{\prime }}{{\sum }_{i=1}^n{\Vert P_i^{\prime }\Vert }^2}s=\sqrt{\frac{S_Q}{S_P}}=\sqrt{\frac{{\sum }_{i=1}^n{\Vert Q_i^{\prime }\Vert }^2}{{\sum }_{i=1}^n{\Vert P_i^{\prime }\Vert }^2}}\text{\hspace{1em}(18)}$$

$(3)：$有了旋转矩阵 $\mathbf{R}$ 以及 尺度 $s$之后，则进一步可以求出平移 $\mathbf{t}$

$$\mathbf{t}=\bar{Q}-s\mathbf{R}\bar{P}\text{\hspace{1em}(19)}$$

 
$结语：$到此为止，关于 Sim3 的理论推导，可以说是讲解完成了，就是对源代码进行分析了。
 
 
 
本文内容来自计算机视觉life ORB-SLAM2 课程课件