【ORB-SLAM3】sim3求解-论文Closed-form solution of absolute orientation using unit quaternions 的阅读笔记

1.前言

聊sim3之前需要聊聊单目视觉slam的尺度漂移问题：

单目尺度漂移问题：
单目视觉里程计前端主要涉及以下内容：

1. 单目初始化：选择满足条件的前后帧图像，进行特征点提取与匹配，借助对极几何求取本质矩阵E,将本质矩阵分解得到前后帧位姿的R、t分量，随后，对图像匹配上的特征点进行三角化，以恢复空间点。（ps:分解得到的t仅是满足极线约束的特解，t乘以任意缩放系数所得到的新值仍满足约束条件。）
2. 初始化成功后，对新进来的每一帧图像仍进行特征点提取与匹配，其中，和前一帧图像匹配上的部分特征点已三角化成功，此时，可以利用运动模型+BA/IMU预积分+BA/PNP，解算新图像帧与上一阵的R,t关系，并利用R,t对剩余特征点进行三角化。

然而,在第二步解算的R,t始终存在误差，这造成利用三角化恢复的特征点空间位置不准确，对新来的每一帧图像均存在这一问题，误差通过图像传递并积累，最终造成一段时间后三角化出来的特征点和初始化时的尺度不一致，即造成尺度漂移问题。

2.相似变换 sim3

由于上述单目尺度漂移的问题，在单目视觉检测到闭环时，当前帧和闭环帧对应的空间点在各自相机坐标系下的坐标不仅存在旋转R、平移t关系，还存在缩放s的关系，这一对应关系称为相似变换（Similarity Transformation），简称sim3，sim3=$\left[\begin{array}{cc}sR& t\\ 0& 1\end{array}\right]$。

2.1 数学建模

给定匹配成功的两组三维点，分别记为{$P_i$}、{$Q_i$},需找到如下变换关系,以描述{$P_i$}到{$Q_i$}相似变换关系：
$$Q_i=sR{P}_i+t\text{\hspace{1em}(1)}$$
由于数据中不可避免存在噪声与误差，我们将上述任务转换为最小二乘求解问题，即寻找合适的尺度因子、旋转和平移，使得它在所有数据上的误差最小：
$$\begin{array}{rl}{e}_i& =Q_i-sR{P}_i-t\\ \underset{s,R,t}{\min }\sum _{i=1}^n{\Vert e_i\Vert }^2& =\underset{s,R,t}{\min }\sum _{i=1}^n{\Vert Q_i-sR{P}_i-t\Vert }^2\end{array}\text{\hspace{1em}(2)}$$

在正式开始求解时，需先定义一些相关变量：
两个三维点集合的质心定义如下：
$$\begin{array}{rl}& \bar{P}=\frac{1}{n}\sum _{i=1}^n{P}_i\\ & \bar{Q}=\frac{1}{n}\sum _{i=1}^n{Q}_i\end{array}\text{\hspace{1em}(3)}$$
每个{$P_i$}、{$Q_i$}的去中心化坐标$P_i^{\prime },Q_i^{\prime }$：
$$\begin{array}{rl}& P_i^{\prime }=P_i-\bar{P}\\ & Q_i^{\prime }=Q_i-\bar{Q}\end{array}\text{\hspace{1em}(4)}$$
其中：$P_i^{\prime },Q_i^{\prime }$满足：
$$\begin{array}{rl}& \sum _{i=1}^n{P}_i^{\prime }=\sum _{i=1}^n\left(P_i-\bar{P}\right)=\sum _{i=1}^n{P}_i-n\bar{P}=0\\ & \sum _{i=1}^n{Q}_i^{\prime }=\sum _{i=1}^n\left(Q_i-\bar{Q}\right)=\sum _{i=1}^n{Q}_i-n\bar{Q}=0\end{array}\text{\hspace{1em}(5)}$$

2.2 求解平移t

对（2）中的误差方程进行如下变换
$$\begin{array}{rl}\sum _{i=1}^n{\Vert e_i\Vert }^2& =\sum _{i=1}^n{\Vert Q_i-sR{P}_i-t\Vert }^2\\ & =\sum _{i=1}^n{\Vert Q_i^{\prime }+\bar{Q}-sR{P}_i^{\prime }-sR\bar{P}-t\Vert }^2\\ & =\sum _{i=1}^n\Vert \left(Q_i^{\prime }-sR{P}_i^{\prime }\right)+\underset{t_0}{\underbrace{(\bar{Q}-sR\bar{P}-t)}}{\Vert }^2\\ & =\sum _{i=1}^n{\Vert \left(Q_i^{\prime }-sR{P}_i^{\prime }\right)\Vert }^2+2t_0\sum _{i=1}^n\left(Q_i^{\prime }-sR{P}_i^{\prime }\right)+n{\Vert t_0\Vert }^2\end{array}\text{\hspace{1em}(6)}$$
简记：
$$t_0=\bar{Q}-sR\bar{P}-t\text{\hspace{1em}(7)}$$
由等式（5）可知，（6）式中间项：
$$\sum _{i=1}^n\left(Q_i^{\prime }-sR{P}_i^{\prime }\right)=\sum _{i=1}^n{Q}_i^{\prime }-sR\sum _{i=1}^n{P}_i^{\prime }=0$$
故误差方程（6）进一步化简为：
$$\sum _{i=1}^n{\Vert e_i\Vert }^2=\sum _{i=1}^n{\Vert \left(Q_i^{\prime }-sR{P}_i^{\prime }\right)\Vert }^2+n{\Vert t_0\Vert }^2\text{\hspace{1em}(8)}$$
（8）式仅有第二项与待求的平移t有关，易当平方项${\Vert t_0\Vert }^2$=0时，误差方程取得最小值，即平移最优解$t^{\ast }$为
$$\begin{array}{rl}& t_0=\bar{Q}-sR\bar{P}-t=0\\ & t^{\ast }=\bar{Q}-sR\bar{P}\end{array}\text{\hspace{1em}(9)}$$
即，若我们已知旋转R与缩放系数s,即可求取平移最优解$t^{\ast }$。

2.3求解缩放因子

误差方程（8）可进一步简化为：
$$\begin{array}{rl}\sum _{i=1}^n{\Vert e_i\Vert }^2& =\sum _{i=1}^n{\Vert Q_i^{\prime }-sR{P}_i^{\prime }\Vert }^2\\ & =\sum _{i=1}^n{\Vert Q_i^{\prime }\Vert }^2-2s\sum _{i=1}^n{Q}_i^{\prime }R{P}_i^{\prime }+s^2\sum _{i=1}^n{\Vert R{P}_i^{\prime }\Vert }^2\end{array}\text{\hspace{1em}(10)}$$
由于向量模长不受旋转的影响，故${\Vert R{P}_i^{\prime }\Vert }^2={\Vert P_i^{\prime }\Vert }^2$，（10）式可进一步表示为：
$$\begin{array}{rl}\sum _{i=1}^n{\Vert e_i\Vert }^2& =\underset{S_Q}{\underbrace{\sum _{i=1}^n{\Vert Q_i^{\prime }\Vert }^2}}-2s\underset{D}{\underbrace{\sum _{i=1}^n{Q}_i^{\prime }R{P}_i^{\prime }}}+s^2\underset{S_P}{\underbrace{\sum _{i=1}^n{\Vert P_i^{\prime }\Vert }^2}}\\ & =S_Q-2sD+s^2{S}_P\end{array}\text{\hspace{1em}(11)}$$
式（11）是关于s的一元二次方程,其极小值点为：
$$s=\frac{D}{S_P}=\frac{{\sum }_{i=1}^n{Q}_i^{\prime }R{P}_i^{\prime }}{{\sum }_{i=1}^n{\Vert P_i^{\prime }\Vert }^2}\text{\hspace{1em}(12)}$$
注意上述s为的方向为{$P_i$}→{$Q_i$}，然而按照此方法计算的
$$s_{Q_i\to P_i}=\frac{{\sum }_{i=1}^n{P}_i^{\prime }{R}^T{Q}_i^{\prime }}{{\sum }_{i=1}^n{\Vert Q_i^{\prime }\Vert }^2}\ne \frac{1}{s}$$
即按照上述求解的缩放因子不具备对称性，因此对式（10）重新简化为：
$$\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}R{P}_i^{\prime }\Vert }^2\\ & =\frac{1}{s}\underset{S_Q}{\underbrace{\sum _{i=1}^n{\Vert Q_i^{\prime }\Vert }^2}}-2\underset{D}{\underbrace{\sum _{i=1}^n{Q}_i^{\prime }R{P}_i^{\prime }}}+s\underset{S_P}{\underbrace{\sum _{i=1}^n{\Vert 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}(13)}$$
令第一项为0，可得满足对称性的缩放因子
$$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}}$$
此时，误差方程仅剩第二项，令$D={\sum }_{i=1}^n{Q}_i^{\prime }R{P}_i^{\prime }$最大，即可使误差方程最小。

2.4 求解旋转R

由于旋转矩阵受制于矩阵正交性的约束，而四元数代替旋转矩阵R仅需保证模长为1的约束，故使用四元数代替矩阵进行旋转的求解。

• 四元数的旋转计算：
  空间三维点 $P=[x,y,z]$对应的四元数表示为 $\dot{p}=[0,x,y,z{]}^T$ 。
  旋转用一个单位四元数 $\dot{q}$ 来表示，则 $\dot{p}$ 旋转后的三维点用四元数表示为:
  $${\dot{p}}^{\prime }=\dot{q}\dot{p}{\dot{q}}^{-1}=\dot{q}\dot{p}{\dot{q}}^{\ast }$$
  四元数 ${\dot{p}}^{\prime }$ 的虚部即为旋转后的坐标。其中 ${\dot{q}}^{\ast }$ 表示取 $\dot{q}$ 的共轭。
• 四元数的性质1
  三个四元数满足如下条件。直接相乘的形式，表示四元数乘法，中间的 $\cdot$ 表示向量点乘。
  $$\dot{p}\cdot \left(\dot{r}{\dot{q}}^{\ast }\right)=(\dot{p}\dot{q})\cdot \dot{r}$$
• 四元数的性质2
  假设四元数 $\dot{r}=\left[r_0,r_x,r_y,r_z\right]$ ，则有
  $$\begin{array}{rl}\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}\end{array}$$
  其中 $\mathbb{R},\overline{\mathbb{R}}$ 都是 $4\times 4$ 的对称矩阵。

下面进入旋转矩阵R的求解，求误差方程（13）的最小值等价于求下式的最大值：
$$\begin{array}{rl}D=\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 }\dot{q}\right)\cdot \left(\dot{q}{\dot{P}}_i^{\prime }\right)\\ & =\sum _{i=1}^n\left({\mathbb{R}}_{\mathbb{Q},\dot{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}\\ & ={\dot{q}}^{T}N\dot{q}\end{array}\text{\hspace{1em}(14)}$$
其中:
$$\begin{array}{rl}{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\end{array}$$
$$\begin{array}{r}{\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},i}\dot{q}\\ \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}}_{P,\mathrm{i}}}\dot{q}\end{array}$$

我们定义
$$\begin{array}{rl}M& =\sum _{i=1}^n{P}_i^{\prime }{Q}_i^{\prime 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}$$
其中
$$\begin{array}{rl}{S}_{xx}& =\sum _{i=1}^n{P}_{i,x}{Q}_{i,x}\\ S_{xy}& =\sum _{i=1}^n{P}_{i,x}{Q}_{i,y}\end{array}$$
则
$$\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}$$

式（14）等价于：
q ˙ = a r g m a x q ˙ { q ˙ T N q ˙ } , s u b j e c t   t o ∣ ∣ q ˙ ∣ ∣ 2 = 1 (15) \dot{q}= arg \underset{\dot q}{\mathrm{max}} \{\dot{q}^{T} N \dot{q}\},subject\ to ||\dot q||^2=1 \tag{15} q˙​=argq˙​max​{q˙​TNq˙​},subject to∣∣q˙​∣∣2=1(15)
依据拉格朗日法：
$$L(\dot{q},\lambda )={\dot{q}}^{T}N\dot{q}+\lambda (1-||\dot{q}|{|}^2)\text{\hspace{1em}(16)}$$

对$\dot{q}、\lambda$计算偏导得：
$$L^{\prime }(\dot{q})=2N\dot{q}-2\lambda \dot{q}=0\text{\hspace{1em}(17)}$$
$$L^{\prime }(\lambda )=1-||\dot{q}|{|}^2=0\text{\hspace{1em}(18)}$$

由（17）可知 $\dot{q}$是矩阵N特征值为$\lambda$的特征向量
将（17）代入（15）式可得：
$${\dot{q}}^{T}N\dot{q}={\dot{q}}^T\lambda \dot{q}=\lambda \text{\hspace{1em}(5)}$$
因此，当$\lambda$取最大值时，${\dot{q}}^{T}N\dot{q}$取得最大值，此时x对应为$N$最大特征值对应的特征向量；
对N进行特征值分解，求得最大特征值对应的特征向量就是待求的用四元数表示的{$P_i$}到{$Q_i$}的旋转 。

3.总结

sim3计算步骤：
1、先计算旋转 $R$ 。
具体来说，先构建 $\mathrm{M}$ 矩阵
$$\begin{array}{rl}M& =\sum _{i=1}^n{P}_i^{\prime }{Q}_i^{\prime 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}$$
然后得到矩阵 $N$ ，对 $N$ 进行特征值分解，求得最大特征值对应的特征向量就是待求的用四元数表示的旋 转 $R$ 。注意这里旋转的方向是 { P i } → { Q i } \left\{P_{i}\right\} \rightarrow\left\{Q_{i}\right\} {Pi​}→{Qi​} 。
2、根据上面计算的旋转 $R$ 来计算尺度 $s$ 。
具体来说，可以使用以下两种方法来计算，第一种是具有对称性的尺度（推荐）
$$s=\sqrt{\frac{S_Q}{S_P}}=\sqrt{\frac{{\sum }_{i=1}^n{\Vert Q_i^r\Vert }^2}{{\sum }_{i=1}^n{\Vert P_i^{\prime }\Vert }^2}}$$
第二种是不具有对称性的尺度 (ORBSLAM使用)
$$s=\frac{D}{S_p}=\frac{{\sum }_{i=1}^n{Q}_i^{\prime }R{P}_i^{\prime }}{{\sum }_{i=1}^n{\Vert P_i^{\prime }\Vert }^2}$$
3、根据旋转 $R$ 和尺度 $s$ 计算平移 $t_{\text{。 }}$
$$t=\bar{Q}-sR\bar{P}$$

Sim3的逆变换矩阵
假设Sim3的变换矩阵为:
$$\mathrm{Sim}3=\left[\begin{array}{cc}sR& t\\ 0& 1\end{array}\right]$$
那么，它的逆变换矩阵为:
$$(\mathrm{Sim}3{)}^{-1}={\left[\begin{array}{cc}sR& t\\ 0& 1\end{array}\right]}^{-1}=\left[\begin{array}{cc}\frac{1}{s}{R}^T& -\frac{1}{s}{R}^{T}t\\ 0& 1\end{array}\right]$$

ps:
本文部分内容参考《计算机视觉life》的ORB-SLAM2课程