讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析链接如下(本文内容来自计算机视觉life ORB-SLAM2 课程课件): (01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/123092196 文末正下方中心提供了本人 联系方式， 点击本人照片即可显示 W X → 官方认证 {\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证} 文末正下方中心提供了本人联系方式，点击本人照片即可显示WX→官方认证
一、前言原论文 \color{red} 原论文 原论文 Closed-form solution of absolute orientation using unit quaternions
在上一篇博客中，推导出了最佳平移 t ∗ \mathbf t^* t∗ 尺度尺度 s ∗ s^* s∗
t ∗ = Q ˉ − s R P ˉ s ∗ = S Q S P = ∑ i = 1 n ∥ Q i ′ ∥ 2 ∑ i = 1 n ∥ P i ′ ∥ 2 (01) \color{Green} \tag{01} \mathbf t^{*}=\bar{Q}-s \mathbf R \bar{P} ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~s^{*}=\sqrt{\frac{S_{Q}}{S_{P}}}=\sqrt{\frac{\sum_{i=1}^{n}\left\|Q_{i}^{\prime}\right\|^{2}}{\sum_{i=1}^{n}\left\|P_{i}^{\prime}\right\|^{2}}} t∗=Qˉ−sRPˉ s∗=SPSQ =∑i=1n∥Pi′∥2∑i=1n∥Qi′∥2 (01)另外，其中的(21)式这里还会用到，所以复写如下： ∑ i = 1 n ∥ e i ∥ 2 = ∑ i = 1 n ∥ 1 s Q i ′ − s R P i ′ ∥ 2 = 1 s ∑ i = 1 n ∥ Q i ′ ∥ 2 ⏟ S Q − 2 ∑ i = 1 n Q i ′ R P i ′ ⏟ D + s ∑ i = 1 n ∥ R P i ′ ∥ 2 ⏟ S P = 1 s S Q − 2 D + s S P = ( s S P − S Q s ) 2 + 2 ( S P S Q − D ) (02) \color{Green} \tag{02} \begin{aligned} \sum_{i=1}^{n}\left\|e_{i}\right\|^{2} &=\sum_{i=1}^{n}\left\|\frac{1}{\sqrt{s}} Q_{i}^{\prime}-\sqrt{s} \mathbf R P_{i}^{\prime}\right\|^{2} \\ &=\frac{1}{s} \underbrace{\sum_{i=1}^{n}\left\|Q_{i}^{\prime}\right\|^{2}}_{S_{Q}}- \underbrace{2 \sum_{i=1}^{n} Q_{i}^{\prime} \mathbf R P_{i}^{\prime}}_{D}+s \underbrace{\sum_{i=1}^{n}\left\|\mathbf R P_{i}^{\prime}\right\|^{2}}_{S_{P}} \\ &=\frac{1}{s} S_{Q}-2 D+s S_{P} \\ &=\left(\sqrt{s S_{P}}-\sqrt{\frac{S_{Q}}{s}}\right)^{2}+2\left(S_{P} S_{Q}-D\right) \end{aligned} i=1∑n∥ei∥2=i=1∑n∥ ∥s 1Qi′−s RPi′∥ ∥2=s1SQ i=1∑n∥Qi′∥2−D 2i=1∑nQi′RPi′+sSP i=1∑n∥RPi′∥2=s1SQ−2D+sSP=(sSP −sSQ )2+2(SPSQ−D)(02)其中第二项只与 R \mathbf R R相关，里的 S p S_p Sp， S q S_q Sq 都是平方项，所以令第二项里的 D = ∑ i = 1 n Q i ′ R P i ′ D=\sum_{i=1}^{n} Q_{i}^{\prime} \mathbf R P_{i}^{\prime} D=∑i=1nQi′RPi′ 最大时， R \mathbf R R 取得最优解。下面我们考虑用四元数来代替矩阵来表达旋转，那么为什么用四元数而不是矩阵表达旋转？
①因为直接使用矩阵必须要保证矩阵的正交性等约束，这个约束太强了，会带来很多困难。 ②四元数只需要保证模值为1的约束，简单很多，方便推导。
针对于四元数的讲解，这里就不进行详细的推导与理解了，目前本人还没有编写四元数的相关文章，确实其也是一个比较复杂的东西，不过已经有规划了。后续有专门的文章对四元数进行讲解。这里展示暂时只列举一些四元数的性质。
二、四元数假设空间三维点 P = [ x , y , z ] P=[x,y,z] P=[x,y,z]，用一个虚四元数来表示为 p ˙ = [ 0 , x , y , z ] T \dot{p}=[0, x, y, z]^{T} p˙=[0,x,y,z]T
性质 1 ： \color{blue}性质1： 性质1：用四元数来对三维点进行旋转，旋转用一个单位四元数 p ˙ \dot{p} p˙ 来表示，则 p ˙ \dot{p} p˙ 旋转后的三维点用四元数表示为： p ˙ ′ = q ˙ p ˙ q ˙ − 1 = q ˙ p ˙ q ˙ ∗ (03) \color{Green} \tag{03} \dot{p}^{\prime}=\dot{q} \dot{p} \dot{q}^{-1}=\dot{q} \dot{p} \dot{q}^{*} p˙′=q˙p˙q˙−1=q˙p˙q˙∗(03)四元数 p ′ ˙ \dot{p'} p′˙ 的虚部取出即为旋转后的坐标。其中 q ˙ ∗ \dot{q}^* q˙∗ 表示取 q ˙ \dot{q} q˙ 的共轭。 性质 2 ： \color{blue}性质2： 性质2：三个四元数满足如下条件。直接相乘的形式，表示四元数乘法，中间的 ⋅ · ⋅ 表示向量点乘。 p ˙ ⋅ ( r ˙ q ˙ ∗ ) = ( p ˙ q ˙ ) ⋅ r ˙ (04) \color{Green} \tag{04} \dot{p} \cdot\left(\dot{r} \dot{q}^{*}\right)=(\dot{p} \dot{q}) \cdot \dot{r} p˙⋅(r˙q˙∗)=(p˙q˙)⋅r˙(04)
性质 3 ： \color{blue}性质3： 性质3：假设四元数 r ˙ = [ r 0 , r x , r y , r z ] \dot{r}=\left[r_{0}, r_{x}, r_{y}, r_{z}\right] r˙=[r0,rx,ry,rz]，则有： r ˙ q ˙ = [ 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 ] q ˙ = R q ˙ q ˙ r ˙ = [ 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 ] q ˙ = R ‾ q ˙ (05) \color{Green} \tag{05} \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} r˙q˙=⎣ ⎡r0rxryrz−rxr0rz−ry−ry−rzr0rx−rzry−rxr0⎦ ⎤q˙=Rq˙ q˙r˙=⎣ ⎡r0rxryrz−rxr0−rzry−ryrzr0−rx−rz−ryrxr0⎦ ⎤q˙=Rq˙(05) 其中 R \mathbb{R} R, R ‾ \overline{\mathbb{R}} R 都是 4x4 的对称矩阵。 提示： \color{red}提示： 提示： 其实呢，大家暂时可以不去关心什么四元数，只要知道可以用两个四元数相乘来表示旋转就可以了。
三、理论推导根据上一篇博客的推导，式(02)的 D = ∑ i = 1 n Q i ′ R P i ′ D=\sum_{i=1}^{n} Q_{i}^{\prime} \mathbf R P_{i}^{\prime} D=∑i=1nQi′RPi′ 最大时取得 R \mathbf R R 的最优解。 ( 1 ) ： \color{blue}(1)： (1)：利用前面的性质，我们现在的的代价函数可以做如下变换(先根据性质1，再根据性质2)： ∑ i = 1 n Q i ′ R P i ′ = ∑ i = 1 n ( Q ˙ i ′ ) ⋅ ( q ˙ P ˙ i ′ q ˙ ∗ ) = ∑ i = 1 n ( Q ˙ i ′ ) ⋅ ( ( q ˙ P ˙ i ) ′ q ˙ ∗ ) = ∑ i = 1 n ( Q ˙ i ′ q ˙ ) ⋅ ( q ˙ P ˙ i ′ ) (06) \color{Green} \tag{06} \begin{aligned} \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}^{*}\right) =\sum_{i=1}^{n}\left(\dot{Q}_{i}^{\prime}\right) \cdot\left((\dot{q} \dot{P}_{i})^{\prime} \dot{q}^{*}\right) =\sum_{i=1}^{n}\left(\dot{Q}_{i}^{\prime} \dot{q}\right) \cdot\left(\dot{q} \dot{P}_{i}^{\prime}\right) \end{aligned} i=1∑nQi′RPi′=i=1∑n(Q˙i′)⋅(q˙P˙i′q˙∗)=i=1∑n(Q˙i′)⋅((q˙P˙i)′q˙∗)=i=1∑n(Q˙i′q˙)⋅(q˙P˙i′)(06)
( 2 ) ： \color{blue}(2)： (2)：根据性质3，现在再对上面的符号做一些具体化： Q i ′ = [ Q i , x ′ , Q i , y ′ , Q i , z ′ ] T P i ′ = [ P i , x ′ , P i , y ′ , P i , z ′ ] T (07) \color{Green} \tag{07} 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} Qi′=[Qi,x′,Qi,y′,Qi,z′]T Pi′=[Pi,x′,Pi,y′,Pi,z′]T(07) Q ˙ i ′ q ˙ = [ 0 − Q i , x ′ − Q i , y ′ − Q i , z ′ Q i , x ′ 0 − Q i , z ′ Q i , y ′ Q i , y ′ Q i , z ′ 0 − Q i , x ′ Q i , z ′ − Q i , y ′ Q i , x ′ 0 ] q ˙ = R Q , i ˙ q ˙ (08) \color{Green} \tag{08} \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} Q˙i′q˙=⎣ ⎡0Qi,x′Qi,y′Qi,z′−Qi,x′0Qi,z′−Qi,y′−Qi,y′−Qi,z′0Qi,x′−Qi,z′Qi,y′−Qi,x′0⎦ ⎤q˙=RQ,i˙q˙(08) q ˙ P ˙ i ′ = [ 0 − P i , x ′ − P i , y ′ − P i , z ′ P i , x ′ 0 P i , z ′ − P i , y ′ P i , y ′ − P i , z ′ 0 P i , x ′ P i , z ′ P i , y ′ − P i , x ′ 0 ] q ˙ = R P , i ‾ q ˙ (09) \color{Green} \tag{09} \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} q˙P˙i′=⎣ ⎡0Pi,x′Pi,y′Pi,z′−Pi,x′0−Pi,z′Pi,y′−Pi,y′Pi,z′0−Pi,x′−Pi,z′−Pi,y′Pi,x′0⎦ ⎤q˙=RP,iq˙(09)
( 3 ) ： \color{blue}(3)： (3)：根据性质3带入上式如下： ∑ i = 1 n ( Q ˙ i ′ q ˙ ) ⋅ ( q ˙ P ˙ i ′ ) = = ∑ i = 1 n ( R Q , i q ˙ ) ⋅ ( R P , i ‾ q ˙ ) = = ∑ i = 1 n q ˙ T R Q , i T R P , i ‾ q ˙ = q ˙ T ( ∑ i = 1 n R Q , i T R P , i ‾ ) q ˙ (10) \color{Green} \tag{10} \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} i=1∑n(Q˙i′q˙)⋅(q˙P˙i′)==i=1∑n(RQ,iq˙)⋅(RP,iq˙)==i=1∑nq˙TRQ,iTRP,iq˙=q˙T(i=1∑nRQ,iTRP,i)q˙(10)
( 4 ) ： \color{blue}(4)： (4)：为了方便后续的讨论，令： N = ∑ i = 1 n R Q , i T R P , i ‾ ⇒ q ˙ T ( ∑ i = 1 n R Q , i T R P , i ‾ ) q ˙ = q ˙ T N q ˙ (11) \color{Green} \tag{11} 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} N=i=1∑nRQ,iTRP,i ⇒ q˙T(i=1∑nRQ,iTRP,i)q˙=q˙TNq˙(11) ( 5 ) ： \color{blue}(5)： (5)：这里定义： M = ∑ i = 1 n P i ′ Q i T = [ S x x S x y S x z S y x S y y S y z S z x S z y S z z ] (12) \color{Green} \tag{12} \begin{aligned} M &=\sum_{i=1}^{n} P_{i}^{\prime} Q_{i}^{T} =\left[\begin{array}{lll} S_{x x} & S_{x y} & S_{x z} \\ S_{y x} & S_{y y} & S_{y z} \\ S_{z x} & S_{z y} & S_{z z} \end{array}\right] \end{aligned} M=i=1∑nPi′QiT=⎣ ⎡SxxSyxSzxSxySyySzySxzSyzSzz⎦ ⎤(12)也就是说 S x x = ∑ i = 1 n P i , x Q i , x S_{x x} =\sum_{i=1}^{n} P_{i, x} Q_{i, x} Sxx=∑i=1nPi,xQi,x， S x y = ∑ i = 1 n P i , x Q i , y S_{x y} =\sum_{i=1}^{n} P_{i, x} Q_{i, y} Sxy=∑i=1nPi,xQi,y … ~~\dots …，元素依此类推。 引入 M M M 是为了方便用其元素来表示 N N N. ( 6 ) ： \color{blue}(6)： (6)：将 M M M 的元素带入到 N N N 中结果如下： N = ∑ i = 1 n R Q , i T R P , i ‾ = [ ( S x x + S y y + S z z ) S y z − S z y S z x − S x z S x y − S y x S y z − S z y ( S x x − S y y − S z z ) S x y + S y x S z x + S x z S z x − S x z S x y + S y x ( − S x x + S y y − S z z ) S y z + S z y S x y − S y x S z x + S x z S y z + S z y ( − S x x − S y y + S z z ) ] (13) \color{Green} \tag{13} \begin{aligned} 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_{x x}+S_{y y}+S_{z z}\right) & S_{y z}-S_{z y} & S_{z x}-S_{x z} & S_{x y}-S_{y x} \\ S_{y z}-S_{z y} & \left(S_{x x}-S_{y y}-S_{z z}\right) & S_{x y}+S_{y x} & S_{z x}+S_{x z} \\ S_{z x}-S_{x z} & S_{x y}+S_{y x} & \left(-S_{x x}+S_{y y}-S_{z z}\right) & S_{y z}+S_{z y} \\ S_{x y}-S_{y x} & S_{z x}+S_{x z} & S_{y z}+S_{z y} & \left(-S_{x x}-S_{y y}+S_{z z}\right) \end{array}\right] \end{aligned} N=i=1∑nRQ,iTRP,i=⎣ ⎡(Sxx+Syy+Szz)Syz−SzySzx−SxzSxy−SyxSyz−Szy(Sxx−Syy−Szz)Sxy+SyxSzx+SxzSzx−SxzSxy+Syx(−Sxx+Syy−Szz)Syz+SzySxy−SyxSzx+SxzSyz+Szy(−Sxx−Syy+Szz)⎦ ⎤(13)然后我们对 N N N 进行特征值分解，求得最大特征值对应的特征向量就是待求的用四元数表示的旋转， ( 这部分内容在原 \color{red} 这部分内容在原 这部分内容在原 论文中有讲清楚原由 \color{red}论文中有讲清楚原由 论文中有讲清楚原由)，注意这里旋转的方向是 { P i } → { Q i } \left\{P_{i}\right\} \rightarrow\left\{Q_{i}\right\} {Pi}→{Qi}。 ( 7 ) ： \color{blue}(7)： (7)：假设单位四元数为 q ˙ = w + x i + y j + z k \dot q=w+x \mathbf{i}+y \mathbf{j}+z \mathbf{k} q˙=w+xi+yj+zk，其中 ∥ q ˙ ∥ = x 2 + y 2 + z 2 + w 2 = 1 \|\dot q\|=x^{2}+y^{2}+z^{2}+w^{2}=1 ∥q˙∥=x2+y2+z2+w2=1。那么给定一个单位四元数，可以构造旋转矩阵： R ( q ˙ ) = [ 1 − 2 y 2 − 2 z 2 2 x y − 2 z w 2 x z + 2 y w 2 x y + 2 z w 1 − 2 x 2 − 2 z 2 2 y z − 2 x w 2 x z − 2 y w 2 y z + 2 x w 1 − 2 x 2 − 2 y 2 ] (14) \color{Green} \tag{14} \mathbf R(\dot q)=\left[\begin{array}{ccc} 1-2 y^{2}-2 z^{2} & 2 x y-2 z w & 2 x z+2 y w \\ 2 x y+2 z w & 1-2 x^{2}-2 z^{2} & 2 y z-2 x w \\ 2 x z-2 y w & 2 y z+2 x w & 1-2 x^{2}-2 y^{2} \end{array}\right] R(q˙)=⎣ ⎡1−2y2−2z22xy+2zw2xz−2yw2xy−2zw1−2x2−2z22yz+2xw2xz+2yw2yz−2xw1−2x2−2y2⎦ ⎤(14)另外，也可以先把四元数转换成旋转向量，然后再转换成旋转矩阵(源码中使用的就是这种方式)，推导过程就不在这里进行讲解了，因为涉及到的东西较多： θ = 2 arccos w vec = [ n x , n y , n z ] T = [ x , y , z ] T / sin θ 2 (15) \color{Green} \tag{15} \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} θ=2arccosw vec=[nx,ny,nz]T=[x,y,z]T/sin2θ(15)然后再调用cv::Rodrigues(vec,mR12i)函数，即可以把旋转向量转换成旋转矩阵。
四、总结在进行推导讲解的时候，是先求出平移 t \mathbf{t} t、尺度 s s s。但是： t ∗ = Q ˉ − s R P ˉ s ∗ = S Q S P = ∑ i = 1 n ∥ Q i ′ ∥ 2 ∑ i = 1 n ∥ P i ′ ∥ 2 (16) \color{Green} \tag{16} \mathbf t^{*}=\bar{Q}-s \mathbf R \bar{P} ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~s^{*}=\sqrt{\frac{S_{Q}}{S_{P}}}=\sqrt{\frac{\sum_{i=1}^{n}\left\|Q_{i}^{\prime}\right\|^{2}}{\sum_{i=1}^{n}\left\|P_{i}^{\prime}\right\|^{2}}} t∗=Qˉ−sRPˉ s∗=SPSQ =∑i=1n∥Pi′∥2∑i=1n∥Qi′∥2 (16)很明显 平移 t \mathbf{t} t 依赖尺度 s s s 与 旋转 R \mathbf R R，所以在实际的过程中，应该先求解 t \mathbf{t} t 依赖尺度 s s s 与 旋转 R \mathbf{R} R。所以在实际应用的时候流程一般如下。
( 1 ) ： \color{blue}(1)： (1)：先计算旋转矩阵 R \mathbf{R} R，首先构建： M = ∑ i = 1 n P i ′ Q i T = [ S x x S x y S x z S y x S y y S y z S z x S z y S z z ] (17) \color{Green} \tag{17} \begin{aligned} M &=\sum_{i=1}^{n} P_{i}^{\prime} Q_{i}^{T} =\left[\begin{array}{lll} S_{x x} & S_{x y} & S_{x z} \\ S_{y x} & S_{y y} & S_{y z} \\ S_{z x} & S_{z y} & S_{z z} \end{array}\right] \end{aligned} M=i=1∑nPi′QiT=⎣ ⎡SxxSyxSzxSxySyySzySxzSyzSzz⎦ ⎤(17)然后得到矩阵 N N N，对 N N N 进行特征值分解，求得最大特征值对应的特征向量就是待求的用四元数表示的旋转，再把该四元数转换成旋转矩阵 R \mathbf{R} R。 ( 2 ) ： \color{blue}(2)： (2)：有两方式来计算尺度 s s s，第一种是非对称性尺度，依赖转矩阵 R \mathbf{R} R(ORBSLAM使用)；第二种是对称性尺度，不依赖旋转矩阵(推荐)；其计算方式分别如下： s = D S P = ∑ i = 1 n Q i ′ R P i ′ ∑ i = 1 n ∥ P i ′ ∥ 2 s = S Q S P = ∑ i = 1 n ∥ Q i ′ ∥ 2 ∑ i = 1 n ∥ P i ′ ∥ 2 (18) \color{Green} \tag{18} s=\frac{D}{S_{P}}=\frac{\sum_{i=1}^{n} Q_{i}^{\prime} \mathbf R P_{i}^{\prime}}{\sum_{i=1}^{n}\left\|P_{i}^{\prime}\right\|^{2}}~~~~~~~~~~~~~~~~~~~~~~~~~~~s=\sqrt{\frac{S_{Q}}{S_{P}}}=\sqrt{\frac{\sum_{i=1}^{n}\left\|Q_{i}^{\prime}\right\|^{2}}{\sum_{i=1}^{n}\left\|P_{i}^{\prime}\right\|^{2}}} s=SPD=∑i=1n∥Pi′∥2∑i=1nQi′RPi′ s=SPSQ =∑i=1n∥Pi′∥2∑i=1n∥Qi′∥2 (18)
( 3 ) ： \color{blue}(3)： (3)：有了旋转矩阵 R \mathbf{R} R 以及 尺度 s s s之后，则进一步可以求出平移 t \mathbf{t} t
t = Q ˉ − s R P ˉ (19) \color{Green} \tag{19} \mathbf t=\bar{Q}-s \mathbf R \bar{P} t=Qˉ−sRPˉ(19)
结语： \color{red}结语： 结语：到此为止，关于 Sim3 的理论推导，可以说是讲解完成了，就是对源代码进行分析了。 本文内容来自计算机视觉life ORB-SLAM2 课程课件
