(01)ORB-SLAM2源码无死角解析-(56) 闭环线程→计算Sim3:理论推导(1)求解s,t

本人讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的(01)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

上一篇博客，对 ComputeSim3() 进行了整体的讲解，大致明白了了 sim3 的作用。但是应该如何求解相似变换(Similarity Transformation)呢？在上一篇博客中提到:
$$\mathbf{T}=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ & \\ 0& 1\end{array}\right]Ts=\left[\begin{array}{cc}s\mathbf{R}& \mathbf{t}\\ & \\ 0& 1\end{array}\right]\text{\hspace{1em}(01)}$$左边为欧式变换矩阵，右边的是相似变换矩阵，可以很明显的知道仅仅相差一个尺度因子 $s$，当 $s=1$的时候，相似变换就成了欧式变换。总的来说计算Sim3 实际就是计算这三个参数：旋转 $\mathbf{R}$ 平移 $\mathbf{t}$ 尺度因子 $s$。理论来说计算Sim3需要3对不共线的点对即可求解。为什么三对不共线点就可以求解？那么下面来推导一下:

$(1)：$假设坐标系1下有三个不共线三维点 $P_1$，$P_2$，$P_3$，他们分别和坐标系2下的三个不共线三维点 $Q_1$，$Q_2$，$Q_3$ 一一匹配，如下图所示：

$(2)：$首先，我们根据坐标系1下的三个不共线三维点来构造一个新的坐标系。沿着 $x$ 上的单位向量 $\hat{x}$:$$x=P_2-P_1\hat{x}=\frac{x}{\Vert x\Vert }\text{\hspace{1em}(02)}$$沿着 $y$ 轴的单位向量 $\hat{y}$:$$y=\overrightarrow{A{P}_3}=\overrightarrow{P_1{P}_3}-\overrightarrow{P_{1}A}=\left(P_3-P_1\right)-\left[\left(P_3-P_1\right)\hat{x}\right]\hat{x}\hat{y}=\frac{y}{\Vert y\Vert }\text{\hspace{1em}(03)}$$沿着 $z$ 轴的单位向量$\hat{z}$:$$\hat{z}=\hat{x}\times \hat{y}\text{\hspace{1em}(04)}$$
 
$(3)：$同理，我们对于坐标系2下的 $Q_1$，$Q_2$，$Q_3$ 也可以得到沿着3个坐标轴的单位向量 $\widehat{x^{\prime }},\widehat{y^{\prime }},\widehat{z^{\prime }}$。
 
$(4)：$我们现在要计算 坐标系1 到 坐标系2 的旋转，记坐标系单位向量构成的基底矩阵为：$${\mathbf{M}}_1=[\hat{x},\hat{y},\hat{z}]{\mathbf{M}}_2=\left[\widehat{x^{\prime }},\widehat{y^{\prime }},\widehat{z^{\prime }}\right]\text{\hspace{1em}(05)}$$
$(5)：$ 假设坐标系1下有一个向量 $v_1$，他在坐标系2下记为 $v_2$，因为向量本身没没有变化，根据坐标系定义有：$${\mathbf{M}}_1{v}_1={\mathbf{M}}_2{v}_2\Rightarrow v_2={\mathbf{M}}_2^T{\mathbf{M}}_1{v}_1\text{\hspace{1em}(06)}$$那么从坐标系1到坐标系2的旋转就是
$$R={\mathbf{M}}_2^T{\mathbf{M}}_1\text{\hspace{1em}(07)}$$

$(6)：$看起来好像没什么问题，但是实际上我们不会这样使用，因为存在如下问题：
①这个旋转的结果和选择点的顺序关系密切，我们分别让不同的点做坐标系原点，得到的结果不同。
②这种情况不适用于匹配点大于3个的情况。
因此实际上我们不会使用以上方法。我们通常能够拿到远大于3个的三维匹配点对，我们会使用最小二乘法来得到更稳定、更精确的结果。

$提示：$如果熟悉EPnP，阅读过之前的 (01)ORB-SLAM2源码无死角解析-(37) EPnP 算法原理详解→理论基础一:控制点选取、透视投影约束，那么理解接下来的推导应该是比较简单的。
 

二、计算SIM3平移

$(1)：$假设得到了 $n>3$ 组匹配的三维点，分别记为 { P i } ， { Q i } \{P_i\}，\{Q_i\} { Pi​}，{ Qi​}，其中 $i=1,...,n$ 我们的目的是对于
每对匹配点，找到如下的变换关系：$$Q_i=s\mathbf{R}{P}_i+\mathbf{t}\text{\hspace{1em}(08)}$$其中 $s$ 是尺度因子，$\mathbf{R}$ 是旋转， $\mathbf{t}$ 是平移。
 
$(2)：$如果数据是没有任何噪音的理想数据，理论上我们可以找到满足上述关系的尺度因子、旋转和平移。但实际上数据是不可避免会有噪音和误差，所以我们转换思路，定义一个误差 $e_i$，我们的目的就是寻找合适的尺度因子、旋转和平移，使得它在所有数据上的误差最小。$$\begin{array}{rl}{e}_i& =Q_i-s\mathbf{R}{P}_i-\mathbf{t}\\ \underset{s,\mathbf{R},\mathbf{t}}{\min }\sum _{i=1}^n{\Vert e_i\Vert }^2& =\underset{s,\mathbf{R},\mathbf{t}}{\min }\sum _{i=1}^n{\Vert Q_i-s\mathbf{R}{P}_i-\mathbf{t}\Vert }^2\end{array}\text{\hspace{1em}(09)}$$$(3)：$在开始求解之前，我们先定义两个三维点集合中所有三维点的均值（或者称为质心、重心）$$\bar{P}=\frac{1}{n}\sum _{i=1}^n{P}_i\bar{Q}=\frac{1}{n}\sum _{i=1}^n{Q}_i\text{\hspace{1em}(10)}$$我们对每个三维点 $P_i,Q_i$ 分别减去均值，得到去中心化后的坐标 $P_i^{\prime },Q_i^{\prime }$ 则有$$P_i^{\prime }=P_i-\bar{P}{Q}_i^{\prime }=Q_i-\bar{Q}\text{\hspace{1em}(11)}$$$$\begin{gathered} \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\text{\hspace{1em}(12)} \end{gathered}$$上面的结论很重要，我们在后面推导的时候要使用。
 
$(4)：$下面开始推导我们的误差方程：
$$\begin{array}{rl}\sum _{i=1}^n{\Vert e_i\Vert }^2& =\sum _{i=1}^n{\Vert Q_i-s\mathbf{R}{P}_i-\mathbf{t}\Vert }^2\\ & =\sum _{i=1}^n{\Vert Q_i^{\prime }+\bar{Q}-s\mathbf{R}{P}_i^{\prime }-s\mathbf{R}\bar{P}-\mathbf{t}\Vert }^2\\ & =\sum _{i=1}^n\Vert \left(Q_i^{\prime }-s\mathbf{R}{P}_i^{\prime }\right)+\underset{{\mathbf{t}}_0}{\underbrace{(\bar{Q}-s\mathbf{R}\bar{P}-\mathbf{t})}}{\Vert }^2\\ & =\sum _{i=1}^n{\Vert \left(Q_i^{\prime }-s\mathbf{R}{P}_i^{\prime }\right)\Vert }^2+2{\mathbf{t}}_0\sum _{i=1}^n\left(Q_i^{\prime }-s\mathbf{R}{P}_i^{\prime }\right)+n{\Vert {\mathbf{t}}_0\Vert }^2\end{array}\text{\hspace{1em}(13)}$$为了推导不显得那样臃肿，其中我们简记 ${\mathbf{t}}_0=\bar{Q}-s\mathbf{R}\bar{P}-\mathbf{t}$
 
$(5)：$根据前面(12)式的推导可得等式右边中间项$$\sum _{i=1}^n\left(Q_i^{\prime }-s\mathbf{R}{P}_i^{\prime }\right)=\sum _{i=1}^n{Q}_i^{\prime }-s\mathbf{R}\sum _{i=1}^n{P}_i^{\prime }=0\text{\hspace{1em}(14)}$$这样前面误差方程(13)式可以化简为：$$\sum _{i=1}^n{\Vert e_i\Vert }^2=\sum _{i=1}^n{\Vert \left(Q_i^{\prime }-s\mathbf{R}{P}_i^{\prime }\right)\Vert }^2+n{\Vert {\mathbf{t}}_0\Vert }^2\text{\hspace{1em}(15)}$$
$(6)：$等式右边的两项都是大于等于0的平方项，并且只有第二项里的 ${\mathbf{t}}_0$ 和我们要求的平移 $\mathbf{t}$ 有关，所以当 ${\mathbf{t}}_0=0时$ ，我们可以得到平移的最优解 $t^{\ast }$:
$${\mathbf{t}}_0=\bar{Q}-s\mathbf{R}\bar{P}-\mathbf{t}=0\Rightarrow {\mathbf{t}}^{\ast }=\bar{Q}-s\mathbf{R}\bar{P}\text{\hspace{1em}(16)}$$也就是说我们知道了旋转 $\mathbf{R}$ 和尺度 $s$ 就能根据三维点均值做差得到平移 $\mathbf{t}$ 了，注意这里平移的方向是 { P i } → { Q i } \{P_i\}→\{Q_i\} { Pi​}→{ Qi​}。$\mathbf{R}$ 的求解在下一篇博客中讲解，接着来看看如何求解 $s$。
 

三、计算SIM3尺度

$(1)：$针对于误差函数(15)式，因为第二项与 $s$没有关系，所以可以进一步简化为：
$$\begin{array}{rl}\sum _{i=1}^n{\Vert e_i\Vert }^2& =\sum _{i=1}^n{\Vert Q_i^{\prime }-s\mathbf{R}{P}_i^{\prime }\Vert }^2\\ & =\sum _{i=1}^n{\Vert Q_i^{\prime }\Vert }^2-2s\sum _{i=1}^n{Q}_i^{\prime }\mathbf{R}{P}_i^{\prime }+s^2\sum _{i=1}^n{\Vert \mathbf{R}{P}_i^{\prime }\Vert }^2\end{array}\text{\hspace{1em}(17)}$$

$(2)：$由于向量的模长不受旋转的影响，所以 ${\Vert \mathbf{R}{P}_i^{\prime }\Vert }^2={\Vert P_i^{\prime }\Vert }^2$，为了后续更加清晰的表示，我们用简单的符号代替上述式子里的部分内容，所以有$$\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 }\mathbf{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}(18)}$$
$(3)：$由于$\mathbf{R}$ 是已知的(下一篇博客会讲解其具体来源)，所以很容易看出来上面是一个以 为自变量的一元二次方程，要使得该方程误差最小，我们可以得到此时尺度 $s$ 的取值：$$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}\text{\hspace{1em}(19)}$$ORB–SLAM2和3里都是使用上述公式求尺度。注意这里尺度的方向是 { P i } → { Q i } \{P_i\}→\{Q_i\} { Pi​}→{ Qi​}，但是，到这里还存在一个问题，我们对 $P,Q$ 做个调换后得到$$\frac{{\sum }_{i=1}^n{P}_i^{\prime }{\mathbf{R}}^T{Q}_i^{\prime }}{{\sum }_{i=1}^n{\Vert Q_i^{\prime }\Vert }^2}\ne \frac{1}{s}\text{\hspace{1em}(20)}$$我们看到尺度并不具备对称性，也就是从 { P i } → { Q i } \{P_i\}→\{Q_i\} { Pi​}→{ Qi​} 得到的尺度并不是从 { Q i } → { P i } \{Q_i\}→ \{P_i\} { Qi​}→{ Pi​} 得到尺度的倒数，这也说明我们前面方法得到的尺度并不稳定。
 
$(4)：$所以需要重新构造误差函数，使得我们得到 的尺度是对称的、稳定的。当然我们不用自己绞尽脑汁去构造，直接搬运论文里大佬的构造方法即可:
$$\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}(21)}$$
$(5)：$上面等式右边第一项只和尺度 $s$ 有关的平方项，第二项和 $s$ 无关，但是和旋转 $R$ 有关，因此令第一项为 0，就能够得到最佳得尺度 $s^{\ast }$ $$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}(22)}$$同时，第二项里的 $S_p$，$S_q$ 都是平方项，所以令第二项里的 $D={\sum }_{i=1}^n{Q}_i^{\prime }\mathbf{R}{P}_i^{\prime }$ 最大，可以使得剩下的误差函数最小。可以使得剩下的误差函数最小。
 

四、结语

从(22)式可以看出，其是一个对称结构，也就是说从 { P i } → { Q i } \{P_i\}→\{Q_i\} { Pi​}→{ Qi​} 得到的尺度并是从 { Q i } → { P i } \{Q_i\}→ \{P_i\} { Qi​}→{ Pi​} 得到尺度的倒数。这里我们总结下对称形式的优势：
①使得尺度的解和旋转、平移都无关
②反过来，旋转的确定不受数据选择不同的影响
③直观理解，尺度就是三维点到各自均值中心的距离之和。

但是这里还遗留了一个待解决的问题，那就是求解平移 $\mathbf{t}$ 的时候，需要用到旋转矩阵 $\mathbf{R}$， 那么下一篇博客，就来看看 $\mathbf{R}$ 是如何求解的吧！