相似变换Sim3详述

本文根据论文”Closed-form solution of absolute orientation using unit quaternions“对相似变换矩阵$\left(\begin{array}{cc}sR& t\\ 0& 1\end{array}\right)$的求解过程中涉及到的原理和公式进行详细阐述。

概念

图形的相似变换是指由一个图形到另一个图形，在改变的过程中保持形状不变（大小方向和位置可变）的图形，类似于相似三角形的变换形式。相比于单纯的欧式变换相似变换多一个变换尺度信息$s$，表示图像信息的缩放成度。在论文中提出了通过已知的图像间成功匹配的3对特征点求解图像之间存在的相似变换矩阵。在ORB_SLAM回环检测过程中需要对当前帧和回环备选帧之间的相似变换矩阵进行求解。

功能

用于表示俩个图像信息之间的相似变换，包括旋转矩阵$R$、平移向量$t$和图像之间的缩尺度信息$s$。

步骤

• 已知参与运算的俩个图象上有$n$对成功匹配的特征点坐标信息 { r l , i } a n d { r r , i } i = 1 ⋯ n . \{r_{l,i}\}\quad and \quad\{r_{r,i}\}\qquad i=1\cdots n. {rl,i​}and{rr,i​}i=1⋯n.这里的坐标信息表示归一化平面的齐次坐标信息$\left(\begin{array}{c}\frac{x}{z},\frac{y}{z},1\end{array}\right)$。
  左右俩个图像之间的相似变换有$$r_r=sR(r_l)+r_0$$构建重投影误差信息$$e_i=r_{r,i}-sR(r_{l,i})-r_0$$我们的目标就是最小化该误差$$\sum _{i=1}^n||e_i|{|}^2$$其中因为旋转矩阵$R$是单位正交阵，因此$$||R(r_l)|{|}^2=||r_l|{|}^2$$接下来进行去质心处理，是已知的俩帧图像中的点坐标信息零均值$$\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}$$ $$r_{l,i}^{\prime }=r_{l,i}-\overline{r_l}{r}_{r,i}^{\prime }=r_{r,i}-\overline{r_r}$$值得注意的是$$\sum _{i=1}^n{r}_{l,i}^{\prime }=0\sum _{i=1}^n{r}_{r,i}^{\prime }=0$$于是重投影误差可以写做如下形式$$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})$$因此误差的平方和形式可写成以下形式$$\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$$我们的目的是求解上述误差平方和公式的最小值，由于左侧第一项中和$r_0^{\prime }$没有关系，为了使得上述公式值最小我们应该有$$r_0^{\prime }=0or{r}_0=\overline{r_r}-sR(\overline{r_l})$$至此我们已知需要求解的平移向量$r_0$可以通过尺度信息和旋转矩阵获取。

• 接下来我们分情况求解尺度信息$s$，我们接下来的目的是在$r_0^{\prime }=0$的情况下求解尺度信息和旋转矩阵是的下式值最小$$\sum _{i=1}^n||r_{r,i}^{\prime }-sR(r_{l,i}^{\prime })|{|}^2$$由于$$||R(r_{l,i}^{\prime })|{|}^2=||r_{l,i}^{\prime }|{|}^2$$因此$$\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$$上述公式可以写成$$S_r-2sD+s^2{S}_l$$其中$$\{\begin{array}{l}{S}_r={\sum }_{i=1}^n||r_{r,i}^{\prime }|{|}^2\\ D={\sum }_{i=1}^n{r}_{r,i}^{\prime }\cdot R(r_{l,i}^{\prime })\\ S_l={\sum }_{i=1}^n||r_{l,i}^{\prime }|{|}^2\end{array}$$上述公式可以写成$$(s\sqrt{S_l}-D/\sqrt{S_l}{)}^2+(S_r{S}_l-D^2)/S_l$$为了让上述公式尽可能小，我们可以另其中第一项等于0，即另$s=D/S_l$
  即$$s=\sum _{i=1}^n{r}_{r,i}^{\prime }\cdot R(r_{l,i}^{\prime })/\sum _{i=1}^n||r_{l,i}^{\prime }|{|}^2$$至此我们已经获取了求解从左侧坐标系转换到右侧坐标系下是对应的尺度信息$s$的求解公式。
  同理我们获取从右侧坐标系转换到左侧坐标系下时对应的尺度信息$s$的求解公式。已知$$r_r=sR(r_l)+r_0$$进行逆向转换有$$r_l=\bar{s}\bar{R}(r_l)+\overline{r_0}$$其中
  $$\{\begin{array}{l}\bar{s}=1/s\\ \bar{R}=R^{-1}\\ \overline{r_0}=-\frac{1}{s}{R}^{-1}{r}_0\end{array}$$
  我们通过相同的求解方式获取了求解从右侧坐标系转换到左侧坐标系下是对应的尺度信息$\bar{s}$的求解公式$\bar{s}=\bar{D}/S_r$。
  即$$\bar{s}=\sum _{i=1}^n{r}_{l,i}^{\prime }\cdot \bar{R}(r_{r,i}^{\prime })/\sum _{i=1}^n||r_{r,i}^{\prime }|{|}^2$$我们发现获取的俩个尺度信息求解公式互为倒数，即结果不对称。如果已知左右俩帧图像中的某一侧精度更高我们可以选择使用其中的一个公式解算尺度信息。
  如果左俩侧的图像帧精度相差无几，我们可以直接默认俩侧求解的尺度信息对称，于是可以通过$$e_i=\frac{1}{\sqrt{s}}{r}_{r,i}^{\prime }-\sqrt{s}R(r_{l,i}^{\prime })$$误差平方和为$$\begin{gathered} \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 \\ \frac{1}{s}{S}_r-2D+s{S}_l \end{gathered}$$调整$s$是的上式最小化，$s=S_r/S_l$
  即$$\bar{s}=(\sum _{i=1}^n||r_{r,i}^{\prime }|{|}^2/\sum _{i=1}^n||r_{l,i}^{\prime }|{|}^2{)}^{\frac{1}{2}}$$值得注意的是，如果我们假设尺度因子的对称时可以直接通过俩帧图像中所给的坐标信息获取$s$，而无需求解旋转矩阵信息$R$。
  至此，我们获取了求解尺度因子的方法公式。

• 接下来就是求解旋转矩阵信息，我们的目标是使得$S_r-2sD+s^2{S}_l$的值尽可能的小，我们已知 $S_r,D,S_l$的具体表示形式，由于$$||r_{r,i}^{\prime }\cdot R(r_{l,i}^{\prime })|{|}^2=||r_{r,i}\cdot r_{l,i}|{|}^2$$所以有$$D^2<=S_r\cdot S_l$$为了使得$S_r-2sD+s^2{S}_l$尽可能小，我们应该是的 $D$ 尽可能大，即使得 ${\sum }_{i=1}^n{r}_{r,i}^{\prime }\cdot R(r_{l,i}^{\prime })$尽可能大，通过这一条件来求解旋转矩阵$R$。
  这里开始引入四元素的知识，用四元数求解线性方程来获得最优的旋转信息，有关四元数的知识很常见，且论文中已经给出本文所用到的所有四元数知识，这里不再赘述。根据四元数的知识我们有$$\sum _{i=1}^n{r}_{r,i}^{\prime }\cdot R(r_{l,i}^{\prime })=\sum _{i=1}^n(\dot{q}{r}_{l,i}^{\prime }{\dot{q}}^{\ast })\cdot {\dot{r}}_{r,i}^{\prime }=\sum _{i=1}^n(\dot{q}{r}_{l,i}^{\prime })\cdot (r_{r,i}^{\prime }\dot{q})$$已知 $r_{l,i}^{\prime }=(x_{l,i}^{\prime },y_{l,i}^{\prime },z_{l,i}^{\prime })$ ,$r_{r,i}^{\prime }=(x_{r,i}^{\prime },y_{r,i}^{\prime },z_{r,i}^{\prime })$,于是有
  $$\dot{q}{\dot{r}}_{l,i}^{\prime }=\left[\begin{array}{ccccc}0& -x_{l,i}^{\prime }& -y_{l,i}^{\prime }& -z_{l,i}^{\prime }& \\ x_{l,i}^{\prime }& 0& z_{l,i}^{\prime }& -y_{l,i}^{\prime }& \\ y_{l,i}^{\prime }& -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}={\bar{\mathbb{R}}}_{l,i}\dot{q}$$
  $$\dot{q}{\dot{r}}_{r,i}^{\prime }=\left[\begin{array}{ccccc}0& -x_{r,i}^{\prime }& -y_{r,i}^{\prime }& -z_{r,i}^{\prime }& \\ x_{r,i}^{\prime }& 0& z_{r,i}^{\prime }& -y_{r,i}^{\prime }& \\ y_{r,i}^{\prime }& -z_{r,i}^{\prime }& 0& x_{r,i}^{\prime }& \\ z_{r,i}^{\prime }& y_{r,i}^{\prime }& -x_{r,i}^{\prime }& 0& \end{array}\right]\dot{q}={\bar{\mathbb{R}}}_{r,i}\dot{q}$$于是我们可以将上述最大化的目标函数写做$$\begin{gathered} \sum _{i=1}^n({\bar{\mathbb{R}}}_{l,i}\dot{q})\cdot ({\bar{\mathbb{R}}}_{r,i}\dot{q}) \\ =\sum _{i=1}^n\dot{}{q}^T{\bar{\mathbb{R}}}_{l,i}{\bar{\mathbb{R}}}_{r,i}\dot{q} \\ =\dot{}{q}^T(\sum _{i=1}^n{\bar{\mathbb{R}}}_{l,i}^T{\bar{\mathbb{R}}}_{r,i})\dot{q} \\ ={\dot{q}}^T(\sum _{i=1}^n{N}_i)\dot{q} \\ ={\dot{q}}^{T}N\dot{q} \end{gathered}$$矩阵$N$是对称矩阵。
  接下来我们引出矩阵$M$，之后在此基础获得矩阵$N$的具体表示形式$$\begin{gathered} M=\sum _{i=1}^n{r}_{l,i}^{\prime }{r}_{r,i}^{\prime T} \\ 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] \end{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 }$$引出矩阵$N$
  $$N=\left[\begin{array}{cccc}(S_{xx}+S_{yy}+S_{zz})& S_{yz}-S_{zy}& S_{zx}-S_{xz}& S_{xy}-S_{yx}\\ S_{yz}-S_{zy}& (S_{xx}-S_{yy}-S_{zz})& S_{xy}+S_{yx}& S_{zx}+S_{xz}\\ S_{zx}-S_{xz}& S_{xy}+S_{yx}& (-S_{xx}+S_{yy}-S_{zz})& S_{yz}+S_{zy}\\ S_{xy}-S_{yx}& S_{zx}+S_{xz}& S_{yz}+S_{zy}& (-S_{xx}-S_{yy}+S_{zz})\end{array}\right]$$
  值得注意的是矩阵$N$主对角线上的元素和为0。
  为了使 ${\dot{q}}^{T}N\dot{q}$ 的值最大，我们需要获得矩阵$N$的最大特征值和对应的特征向量解，对应最大的特征值为${\lambda }_{max}$对应的特征向量为$e_{max}$，其中最大特征值对应的特征向量就是我们要求解的旋转矩阵对应的四元数表示形式，我们将问题转换为求解四个线性方程组的问题$$[N-{\lambda }_{max}I]{\dot{e}}_{max}=0$$论文中介绍的是$Ferrari$的求解方法，将问题转换为俩个二次方程进行求解。
  至此，我们已经将论文中求解相似变换$Sim3$的主要部分讲解完毕，再次总结过程如下：

• 对已知的左右俩侧图像点信息做去质心等处理；

• 构建重投影误差的平方和，将和平移向量有关的项归0时获得平移向量的求解公式；

• 将剩余的误差平方和项展开，将涉及到尺度因子$s$的项归0可以获得左右俩种非对称的求解公式，如果我们假设左右尺度因子对称可以直通过已知的匹配点坐标进行求解；

• 最后将剩余的误差平方和部分最小化问题转换为将其中的部分项最大化，引出四元数，并对$N$矩阵球最大特征值和对应的特征向量，该特征向量就是我们要求解的旋转矩阵的四元数形式；

• 将四元数转换为旋转矩阵，再求解尺度因子，在获得平移向量，于是图像之间的相似变换矩阵$\left(\begin{array}{cc}sR& t\\ 0& 1\end{array}\right)$求解成功。

链接

Sim3相似变换详解
相似变换简介版

补充