ORB-SLAM2_sim3求解

太过爱你忘了你带给我的痛 2023-01-07 01:28 198阅读 0赞

**目录**

1.sim3的简单概述

2.sim3算法介绍

1) 3对匹配的3D点建立坐标系

2) 旋转矩阵计算

3）平移向量计算

4）尺度计算

3.代码解析

1）sim3求解器构造函数Sim3Solver

2）sim3迭代求解Sim3Solver::iterate

--------------------

本篇笔记对 [ORB-SLAM2代码阅读笔记（八）：LoopClosing线程][ORB-SLAM2_LoopClosing] 中提到的sim3进行了分析，以熟悉使用三对匹配点求解位姿变换的方法。

# 1.sim3的简单概述 #

**sim3**简单来说，就是使用3对匹配点来进行相似变换（similarity transformation）的求解，进而解出两个坐标系之间的旋转矩阵、平移向量和尺度。ORB-SLAM2中使用的sim3求解方法来自 Horn 1987, Closed-form solution of absolute orientation using unit quaternions 这篇论文。所以，需要了解一下这篇论文中提出的求解思想，以帮助理解代码。

ORB-SLAM2系统在LoopClosing线程中，当检测到闭环候选帧的时候，就需要对当前关键帧和对应的闭环候选帧之间计算其变换关系。这时需要用**当前关键帧**和其对应的**闭环候选帧**进行sim3求解，这里的sim3求解是对**当前关键帧**和**闭环候选帧**之间匹配的**MapPoint**进行**sim3求解**。通过sim3变换解出当前关键帧和闭环候选帧的匹配MapPoint之间的旋转矩阵R、平移向量t、尺度变换s，也就得到了当前关键帧到闭环关键帧之间的sim3变换gScm。使用这个sim3变换gScm乘上闭环关键帧的sim3位姿gSmw，**mg2oScw=gScm\*gSmw**的乘积**mg2oScw**就是当前关键帧的sim3位姿，之后在闭环校正中就可以使用这个sim3位姿转换为SE3位姿后对当前关键帧进行位姿校正（当然也要对关键帧对应的MapPoints以及其共视的关键帧进行校正）。当然在实际代码中，这个过程还要做**多次投影和优化操作**以确保更高的准确率和精度，这个可以阅读ORB-SLAM2该部分的代码来体会。

以下图为例，A为当前关键帧，B为候选关键帧。

当相机从B处开始运动到A处的时候，检测到B为A的闭环候选帧。此时，考虑到相机从B运动到A的过程中不光会产生旋转和平移的误差，同时也会产生尺度漂移的累积，需要计算A和B之间的sim3变换，来找到A和B之间的sim3变换（包括旋转矩阵R、平移向量t、尺度变换s）,有了这些值之后，就可以**对关键帧A的位姿进行纠正**。

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L21veXUxMjM0NTY3ODk_size_16_color_FFFFFF_t_70][]

# 2.sim3算法介绍 #

Closed-form solution of absolute orientation using unit quaternions论文提供了一种算法，通过两个坐标系之间3对匹配点来确定两个坐标系之间的变换关系。

## 1) 3对匹配的3D点建立坐标系 ##

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L21veXUxMjM0NTY3ODk_size_16_color_FFFFFF_t_70 1][]

如上图所示，在左右两个坐标系中各有3个点，左侧三个点分别是![$r\_l,\_1][r_l_1]，![$r\_l,\_2][r_l_2]，![$r\_l,\_3][r_l_3]，右侧三个点为![$r\_r,\_1$][r_r_1]，![$r\_r,\_2$][r_r_2]，![$r\_\{r,3\}$][r_r_3]。

可以看出，左侧的![$r\_\{l,1\}$][r_l_1 1]和右侧的![$r\_\{r,1\}$][r_r_1 1]都处于坐标系的原点处，则我们可以计算出在X轴、Y轴和Z轴上的单位向量如下：

**X轴：**x轴上的向量为![x\_l=r\_\{l,2\}-r\_\{l,1\}$][x_l_r_l_2_-r_l_1] ，对![x\_l][x_l]除以其模长得到在x轴方向上的单位向量**![\\hat\{x\_l\}=x\_l/\\left \\| x\_l \\right \\|][hat_x_l_x_l_left _ x_l _right]**

图中这种情况下![x\_\{l,1\}][x_l_1]为原点，![r\_\{l,2\}][r_l_2 1]在x轴方向上，所以两个点相减后所得向量在x轴上，对向量除以模长就是x轴上的单位向量。

**Y轴：**y轴上的向量为![y\_l=(r\_\{l,3\}-r\_\{l,1\})-\\left \[ \\left ( r\_\{l,3\}-r\_\{l,1\} \\right )\\cdot \\hat\{x\_l\} \\right \]\\cdot \\hat\{x\_l\}][y_l_r_l_3_-r_l_1_-_left _ _left _ r_l_3_-r_l_1_ _right _cdot _hat_x_l_ _right _cdot _hat_x_l]，对![y\_l][y_l]除以![y\_l][y_l]的模长得到y轴方向上的单位向量![\\hat\{y\_l\}=\\hat\{y\_l\}/\\left \\| y\_l \\right \\|][hat_y_l_hat_y_l_left _ y_l _right]

![r\_\{l,3\}-r\_\{l,1\}][r_l_3_-r_l_1]所得为从![r\_\{l,1\}][r_l_1 2]到![r\_\{l,3\}][r_l_3 1]这两个点之间的向量，![\\left ( r\_\{l,3\}-r\_\{l,1\} \\right )\\cdot\\hat\{x\_l\}][left _ r_l_3_-r_l_1_ _right _cdot_hat_x_l]表示向量在x轴方向上的单位长度，![\\left \[ \\left ( r\_\{l,3\}-r\_\{l,1\} \\right )\\cdot \\hat\{x\_l\} \\right \]\\cdot \\hat\{x\_l\}][left _ _left _ r_l_3_-r_l_1_ _right _cdot _hat_x_l_ _right _cdot _hat_x_l]表示该向量在x轴方向上的向量的长度。则向量减去其在x轴方向的向量就得到了在y轴方向上的向量。

**Z轴：**由于z轴垂直与x轴和y轴，所以z轴所在方向也就是x和y轴方向向量叉乘乘积的方向（两个向量叉乘后的乘积所在的向量方向和这两个向量垂直），则z轴方向上的单位向量为![\\hat\{z\_l\}=\\hat\{x\_l\}\\times \\hat\{y\_l\}][hat_z_l_hat_x_l_times _hat_y_l]

同理，也可推出右侧坐标系各个轴上的单位向量![\\hat\{x\_r\},\\hat\{y\_r\},\\hat\{z\_r\}][hat_x_r_hat_y_r_hat_z_r]。

## 2) 旋转矩阵计算 ##

根据1）中所得的坐标系单位向量，可以得到左右两侧坐标系的各个方向单位向量构成的矩阵：![\\large M\_l=\\left | \\hat\{x\_l\} \\hat\{y\_l\}\\hat\{z\_l\}\\right |][large M_l_left _ _hat_x_l_ _hat_y_l_hat_z_l_right]，![\\large M\_l=\\left | \\hat\{x\_r\}\\hat\{y\_r\} \\hat\{z\_r\} \\right |][large M_l_left _ _hat_x_r_hat_y_r_ _hat_z_r_ _right]

此时，如果左侧坐标系中有一个向量![r\_l][r_l]，则![M\_l^T\\cdot r\_l][M_l_T_cdot r_l]可以计算结果为![r\_l][r_l]在三个坐标轴方向上的向量值。![M\_r][M_r]左乘![M\_l^T\\cdot r\_l][M_l_T_cdot r_l]后所得结果为变换到右侧坐标系中的向量![\\large r\_r=M\_rM\_l^Tr\_l][large r_r_M_rM_l_Tr_l]。

则可以推导出从左侧坐标系旋转到右侧坐标系中的旋转矩阵为：![\\large \{\\color\{Red\} R=M\_r\{M\_l^T\}\}][large _color_Red_ R_M_r_M_l_T]

## 3）平移向量计算 ##

假设左右坐标系中各有![n][]各点，他们在左右两侧坐标系中的测量坐标值为![\\left \\\{ r\_\{l,i\} \\right \\\}][left _ r_l_i_ _right]和![\\left \\\{ r\_\{r,i\} \\right \\\}][left _ r_r_i_ _right]，![i][]的取值从1到![n][]。

此时，一个向量从左侧坐标系到右侧坐标系的变换可表示为：![\\large r\_r = sR(r\_l)+r\_0][large r_r _ sR_r_l_r_0] 其中，**s为尺度变换，![\\large r\_0][large r_0]为平移偏移量。**![\\large R(r\_l)][large R_r_l]表示![\\large r\_l][large r_l]的旋转。

实际当中，两个坐标系之间的变换不会那么容易计算出精确的变换向量，和机器学习中采用方法相同，都是采用优化的方法（最小化误差的方法）来求解的，也就是使用最小二乘法来求解。此时，容易看出这里的误差为：

![\\large e\_i=r\_\{r,i\}-sR\\left ( r\_\{l,i\} \\right )-r\_0.][large e_i_r_r_i_-sR_left _ r_l_i_ _right _-r_0.]

那么，求解的最小二乘问题变成了求解：

![\\large min\\sum\_\{i=1\}^n\\left \\| e\_i \\right \\|^2.][large min_sum_i_1_n_left _ e_i _right _2.]

那么，怎么来求解这个最小误差呢？我们先把要求解的表达式放在这里。**这里我们要求解的是平移向量，也就是上边的![\\large r\_0][large r_0 1]**。看看作者是怎么引入问题求解方法的。

--------------------

计算左侧和右侧坐标系中所有点的质心（其实也就是所有点的中心）：

![\\large \\bar\{r\_l\}=\\frac\{1\}\{n\}\\sum \_\{i=1\}^nr\_\{l,i\}][large _bar_r_l_frac_1_n_sum _i_1_nr_l_i] ， ![\\large \\bar\{r\_r\}=\\frac\{1\}\{n\}\\sum \_\{i=1\}^nr\_\{r,i\}][large _bar_r_r_frac_1_n_sum _i_1_nr_r_i]

则每一个点距离质心的距离为：

![\\large \{r\_\{l,i\}\}'=r\_\{l,i\}-\\bar\{r\_l\}][large _r_l_i_r_l_i_-_bar_r_l] ![\\large \{r\_\{r,i\}\}'=r\_\{r,i\}-\\bar\{r\_r\}][large _r_r_i_r_r_i_-_bar_r_r]

则，我们也可以知道：![\\large \\sum \_\{i=1\}^\{n\}r\_\{l,i\}^\{'\}=0][large _sum _i_1_n_r_l_i_0] ![\\large \\sum \_\{i=1\}^\{n\}r\_\{r,i\}^\{'\}=0][large _sum _i_1_n_r_r_i_0]

根据上边得出的变换公式可得：

![\\large r\_\{r,i\}^\{'\}=sR(r\_\{l,i\}^\{'\})+\{r\_0\}'][large r_r_i_sR_r_l_i_r_0] ![\\large \\Rightarrow][large _Rightarrow] ![\\large \{r\_0\}'=\{r\_\{r,i\}\}'-sR(\{r\_\{l,i\}\}')][large _r_0_r_r_i_-sR_r_l_i]

![\\large \\Rightarrow][large _Rightarrow]![\\large \{r\_0\}'=r\_\{r,i\}-\\bar\{r\_r\}-sR(r\_\{l,i\}-\\bar\{r\_l\})][large _r_0_r_r_i_-_bar_r_r_-sR_r_l_i_-_bar_r_l]

![\\large \\Rightarrow][large _Rightarrow]![\\large \{r\_0\}'=r\_\{r,i\}-\\bar\{r\_r\}-sR(r\_\{l,i\})+sR(\\bar\{r\_l\})][large _r_0_r_r_i_-_bar_r_r_-sR_r_l_i_sR_bar_r_l]

![\\large \\Rightarrow][large _Rightarrow]![\\large \{r\_0\}'=r\_0-\\bar\{r\_r\}+sR(\\bar\{r\_l\})][large _r_0_r_0-_bar_r_r_sR_bar_r_l]

此时，优化函数如下：

![\\large \\sum \_\{i=1\}^\{n\}\\left \\| e\_i \\right \\|^2=\\sum \_\{i=1\}^n\\left \\| \{r\_\{r,i\}\}'-\\left \[ sR(\{r\_\{l,i\}\}')+\{r\_0\}' \\right \] \\right \\|^2][large _sum _i_1_n_left _ e_i _right _2_sum _i_1_n_left _ _r_r_i_-_left _ sR_r_l_i_r_0_ _right _ _right _2]

![\\large =\\sum \_\{i=1\}^\{n\}\\left \\| \{r\_\{r,i\}\}'-sR(\{r\_\{l,i\}\}') \\right \\|^2-2\{r\_0\}'\\cdot \\sum \_\{i=1\}^\{n\}\\left \[ \{r\_\{r,i\}\}'-sR(\{r\_\{l,i\}\}') \\right \]+n\\left \\| \{r\_0\}' \\right \\|^2][large _sum _i_1_n_left _ _r_r_i_-sR_r_l_i_ _right _2-2_r_0_cdot _sum _i_1_n_left _ _r_r_i_-sR_r_l_i_ _right _n_left _ _r_0_ _right _2]

对上边的式子，容易计算出中间部分![\\large 2\{r\_0\}'\\cdot \\sum \_\{i=1\}^\{n\}\\left \[ \{r\_\{r,i\}\}'-sR(\{r\_\{l,i\}\}') \\right \]=0][large 2_r_0_cdot _sum _i_1_n_left _ _r_r_i_-sR_r_l_i_ _right _0]

此时剩下第一部分和第三部分，第一部分和![\\large \{r\_0\}'][large _r_0]没关系，第三部分不可能为负值。所以，优化函数在![\\large \{r\_0\}'=0][large _r_0_0]的情况下取得最小值。

带入上式：![\\large \{r\_0\}'=r\_0-\\bar\{r\_r\}+sR(\\bar\{r\_l\})][large _r_0_r_0-_bar_r_r_sR_bar_r_l]中，可得平移向量![\\large t][large t]:

![\\large \{\\color\{Red\} t=r\_0=\\bar\{r\_r\}-sR(\\bar\{r\_l\})\}][large _color_Red_ t_r_0_bar_r_r_-sR_bar_r_l]

## 4）尺度计算 ##

由于![\\large \{r\_0\}'=0][large _r_0_0]，所以可得误差函数：

![\\large =\\sum \_\{i=1\}^n\\left \\| \{r\_\{r,i\}\}'-\\left \[ sR(\{r\_\{l,i\}\}') \\right \] \\right \\|^2][large _sum _i_1_n_left _ _r_r_i_-_left _ sR_r_l_i_ _right _ _right _2]

![\\large =\\sum \_\{i=1\}^n\\left \\| \{r\_\{r,i\}\}' \\right \\|^2-2s\\sum \_\{i=1\}^n\{r\_\{r,i\}\}'\\cdot R(\{r\_\{l,i\}\}')+s^2\\sum \_\{i=1\}^\{n\}\\left \\| R(\{r\_\{l,i\}\}') \\right \\|^2][large _sum _i_1_n_left _ _r_r_i_ _right _2-2s_sum _i_1_n_r_r_i_cdot R_r_l_i_s_2_sum _i_1_n_left _ R_r_l_i_ _right _2]

我们进一步可以把这个形式写成如下形式：

令：![\\large \\sum \_\{i=1\}^\{n\}\\left \\| e\_i \\right \\|^2=S\_r-2sD+s^2S\_l][large _sum _i_1_n_left _ e_i _right _2_S_r-2sD_s_2S_l]

![\\large =(s\\sqrt\{S\_l\}-D/\\sqrt\{S\_l\})^2+(S\_rS\_l-D^\{2\})/S\_l][large _s_sqrt_S_l_-D_sqrt_S_l_2_S_rS_l-D_2_S_l]

此时，要是误差取最小值，则必须第一项为0，此时

![\\large \{\\color\{Red\} \{\\color\{Red\} \}s=D/S\_l=\\left ( \\sum \_\{i=1\}^n\{r\_\{r,i\}\}'\\cdot R(\{r\_\{l,i\}\}') \\right )/\\sum \_\{i=1\}^\{n\}\\left \\| R(\{r\_\{l,i\}\}') \\right \\|^2\}][large _color_Red_ _color_Red_ _s_D_S_l_left _ _sum _i_1_n_r_r_i_cdot R_r_l_i_ _right _sum _i_1_n_left _ R_r_l_i_ _right _2]

根据对称性可得：![\\large r\_r=sR(r\_l)+r\_0][large r_r_sR_r_l_r_0] ![\\large r\_l=\\bar\{s\}\\bar\{R\}(r\_r)+\\bar\{r\_0\}][large r_l_bar_s_bar_R_r_r_bar_r_0]

我们期望计算出来的值为：![\\large \\bar\{s\}=1/s][large _bar_s_1_s]

![\\large \\bar\{r\_0\}=-\\frac\{1\}\{s\}R^\{-1\}(r\_0)][large _bar_r_0_-_frac_1_s_R_-1_r_0]

![\\large \\bar\{R\}=R^\{-1\}][large _bar_R_R_-1]

但是，此时![\\large \\bar\{s\}=1/s\\neq\\left ( \\sum \_\{i=1\}^n\{r\_\{r,i\}\}'\\cdot R(\{r\_\{l,i\}\}') \\right )/\\sum \_\{i=1\}^\{n\}\\left \\| R(\{r\_\{l,i\}\}') \\right \\|^2\}][large _bar_s_1_s_neq_left _ _sum _i_1_n_r_r_i_cdot R_r_l_i_ _right _sum _i_1_n_left _ R_r_l_i_ _right _2]

**当已知两个系统中的一个系统的坐标比另一个系统的坐标精度高得多时，上述两个不对称结果中的一个可能是合适的。**

**如果两组测量中的误差相似，则对误差项使用对称表达式更为合理：**

**![\\large e\_i=\\frac\{1\}\{\\sqrt\{S\}\}\{r\_\{r,i\}\}'-\\sqrt\{s\}R(\{r\_\{l,i\}\}')][large e_i_frac_1_sqrt_S_r_r_i_-_sqrt_s_R_r_l_i]**

则上边式子变为：![\\large \\sum \_\{i=1\}^\{n\}\\left \\| e\_i \\right \\|^2=\\frac\{1\}\{s\}S\_r-2D+sS\_r=(\\sqrt\{s\}S\_l-\\frac\{1\}\{\\sqrt\{s\}\}S\_r)^2+2(S\_lS\_r-D)][large _sum _i_1_n_left _ e_i _right _2_frac_1_s_S_r-2D_sS_r_sqrt_s_S_l-_frac_1_sqrt_s_S_r_2_2_S_lS_r-D]

该式求最小值，可计算得:![\\large \{\\color\{Red\} s=(\\sum \_\{i=1\}^n\\left \\| \{r\_\{r,i\}\}' \\right \\|^2/\\sum \_\{i=1\}^n\\left \\| \{r\_\{l,i\}\}' \\right \\|^2)^\\frac\{1\}\{2\}\}][large _color_Red_ s_sum _i_1_n_left _ _r_r_i_ _right _2_sum _i_1_n_left _ _r_l_i_ _right _2_frac_1_2]

这种对称结果的一个有点是，它允许人们在不需要知道旋转的情况下确定尺度。重要的是，旋转的确定不受我们选择比例因子的三个值之一的影响。在每种情况下，当D尽可能大时，误差能取得最小值。也就是说，我们必须选择使![\\large \{\\color\{Red\} \\sum \_\{i=1\}^n\{r\_\{r,i\}\}'\\cdot R\\left ( \{r\_\{l,i\}\}' \\right )\}][large _color_Red_ _sum _i_1_n_r_r_i_cdot R_left _ _r_l_i_ _right]尽可能大的旋转值。

**注意：这里的意思是说，只有在![\\large D=\{\\color\{Red\} \\sum \_\{i=1\}^n\{r\_\{r,i\}\}'\\cdot R\\left ( \{r\_\{l,i\}\}' \\right )\}][large D_color_Red_ _sum _i_1_n_r_r_i_cdot R_left _ _r_l_i_ _right]取得最大值的情况下，误差才会最小，所以为了求得最小误差，我们应该去求最大的![\\large D][large D].**

--------------------

现在，我们把问题变成了求最大的![\\large D][large D]。

旋转可以使用多种方式进行表达，此处，引入了四元数来表达。

一个四元数表达如下：![\\large \\qq][large _qq]![\\large \\overset\{\\circ \}\{q\}= q\_0+iq\_x+jq\_y+kq\_z][large _overset_circ _q_ q_0_iq_x_jq_y_kq_z]

其中![\\large q\_0][large q_0]为实部，并且满足：![\\large i^2=-1,][large i_2_-1]![\\large j^2=-1,][large j_2_-1]![\\large k^2=-1;][large k_2_-1]

![\\large ij=k,][large ij_k] ![\\large jk=i,][large jk_i] ![\\large ki=j;][large ki_j]

![\\large ji=-k,][large ji_-k] ![\\large kj=-i,][large kj_-i]![\\large ik=-j;][large ik_-j]

此时两个四元数的点乘积为：![\\large \\overset\{\\circ\}\{p\}\\cdot \\overset\{\\circ\}\{q\}=p\_\{0\}q\_\{0\}+p\_\{x\}q\_\{x\}+p\_yq\_y+p\_zq\_z][large _overset_circ_p_cdot _overset_circ_q_p_0_q_0_p_x_q_x_p_yq_y_p_zq_z]

定义![\\large \\overset\{\\circ\}\{q\}][large _overset_circ_q]的共轭向量为：![\\large \\overset\{\\circ\}\{q\}^\\ast =q\_0-iq\_x-jq\_y-kq\_z][large _overset_circ_q_ast _q_0-iq_x-jq_y-kq_z]

则有：![\\large \\overset\{\\circ\}\{q\}\\cdot \\overset\{\\circ\}\{q\}^\\ast =q\_0^2+q\_x^2+q\_y^2+q\_z^2=\\overset\{\\circ\}\{q\}\\cdot \\overset\{\\circ\}\{q\}][large _overset_circ_q_cdot _overset_circ_q_ast _q_0_2_q_x_2_q_y_2_q_z_2_overset_circ_q_cdot _overset_circ_q]

--------------------

此时，![\\large D=\{\\color\{Red\} \\sum \_\{i=1\}^n\{r\_\{r,i\}\}'\\cdot R\\left ( \{r\_\{l,i\}\}' \\right )\}][large D_color_Red_ _sum _i_1_n_r_r_i_cdot R_left _ _r_l_i_ _right]

可以用单位四元数表达为如下形式：

![\\large \\sum \_\{i=1\}^n\\left ( \\overset\{\\circ\}\{q\} \\overset\{\\circ\}\{\{r\}'\}\_\{l,i\} \\overset\{\\circ\}\{q\}^\*\\right )\\cdot \\overset\{\\circ\}\{\{r\}'\_\{r,i\}\}.][large _sum _i_1_n_left _ _overset_circ_q_ _overset_circ_r_l_i_ _overset_circ_q_right _cdot _overset_circ_r_r_i_.]

对该式可改写为： ![\\large \\sum \_\{i=1\}^n\\left ( \\overset\{\\circ\}\{q\} \\overset\{\\circ\}\{\{r\}'\}\_\{l,i\} \\right )\\cdot \\left ( \\overset\{\\circ\}\{\{r\}'\_\{r,i\}\} \\overset\{\\circ\}\{q\}\\right ).][large _sum _i_1_n_left _ _overset_circ_q_ _overset_circ_r_l_i_ _right _cdot _left _ _overset_circ_r_r_i_ _overset_circ_q_right _.]

此时，假设![\\large \{r\_\{l,i\}\}'=\\left ( \{x\_\{l,i\}\}',\{y\_\{l,i\}\}',\{z\_\{l,i\}\}' \\right )^T][large _r_l_i_left _ _x_l_i_y_l_i_z_l_i_ _right _T] ![\\large \{r\_\{r,i\}\}'=\\left ( \{x\_\{r,i\}\}',\{y\_\{r,i\}\}',\{z\_\{r,i\}\}' \\right )^T][large _r_r_i_left _ _x_r_i_y_r_i_z_r_i_ _right _T]

则![\\large M=\\sum \_\{i=1\}^n\{r\_\{l,i\}\}'\{r\_\{r,i\}\}'^T][large M_sum _i_1_n_r_l_i_r_r_i_T]，可将![\\large M][large M]定义为以下形式

![\\large M=\\begin\{bmatrix\} S\_\{xx\} & S\_\{xy\} & S\_\{xz\}\\\\ S\_\{yx\} & S\_\{yy\} & S\_\{yz\}\\\\ S\_\{zx\} & S\_\{zy \}& S\_\{zz\} \\end\{bmatrix\}][large M_begin_bmatrix_ S_xx_ _ S_xy_ _ S_xz_ S_yx_ _ S_yy_ _ S_yz_ S_zx_ _ S_zy _ S_zz_ _end_bmatrix]

定义了M就是为了用其中的元素来表示N,N定义如下：

![\\large N=\\begin\{bmatrix\} (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\{bmatrix\}][large N_begin_bmatrix_ _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_bmatrix]

此时，对N矩阵进行特征值分解求解最大特征值，该特征值对应的特征向量就是待求的四元数。

四元数转欧拉角:![\\large \{\\color\{Red\} q=cos\\frac\{\\Theta \}\{2\}+nsin\\frac\{\\Theta \}\{2\}\}][large _color_Red_ q_cos_frac_Theta _2_nsin_frac_Theta _2]

根据四元数可以求出旋转矩阵R、平移向量t和尺度s。

# 3.代码解析 #

在Closed-form solution of absolute orientation using unit quaternions这篇论文的结论部分，作者坦言该论文中的算法表达相对复杂一些，但是好处是一些程序库中已经实现了该算法的sim3求解，所以对我们使用者来说，只要调用库函数就行了，这样就简单了好多。

ORB-SLAM2中sim3求解流程相关代码位于LoopClosing::ComputeSim3()中。主要的求解函数如下：

## 1）sim3求解器构造函数Sim3Solver ##

Sim3Solver函数用于针对每一个和当前关键帧存在闭环关系的闭环候选关键帧之间构造sim3求解器。求解器中，主要是确定两个关键帧中匹配的MapPoint的对应关系，构造好匹配的MapPoint点对列表，并设置Ransac相关参数用于后续iterate中求解使用。

Sim3Solver::Sim3Solver(KeyFrame *pKF1, KeyFrame *pKF2, const vector<MapPoint *> &vpMatched12, const bool bFixScale):
        mnIterations(0), mnBestInliers(0), mbFixScale(bFixScale)
    {
        mpKF1 = pKF1;
        mpKF2 = pKF2;
     
        vector<MapPoint*> vpKeyFrameMP1 = pKF1->GetMapPointMatches();
     
        mN1 = vpMatched12.size();
     
        mvpMapPoints1.reserve(mN1);
        mvpMapPoints2.reserve(mN1);
        mvpMatches12 = vpMatched12;
        mvnIndices1.reserve(mN1);
        mvX3Dc1.reserve(mN1);
        mvX3Dc2.reserve(mN1);
     
        cv::Mat Rcw1 = pKF1->GetRotation();
        cv::Mat tcw1 = pKF1->GetTranslation();
        cv::Mat Rcw2 = pKF2->GetRotation();
        cv::Mat tcw2 = pKF2->GetTranslation();
     
        mvAllIndices.reserve(mN1);
     
        size_t idx=0;
        for(int i1=0; i1<mN1; i1++)
        {
            if(vpMatched12[i1])
            {
                MapPoint* pMP1 = vpKeyFrameMP1[i1];
                MapPoint* pMP2 = vpMatched12[i1];
     
                if(!pMP1)
                    continue;
     
                if(pMP1->isBad() || pMP2->isBad())
                    continue;
                //获取MapPoint中记录的能看到该MapPoint的keyframe中对应的关键点的index
                int indexKF1 = pMP1->GetIndexInKeyFrame(pKF1);
                int indexKF2 = pMP2->GetIndexInKeyFrame(pKF2);
     
                if(indexKF1<0 || indexKF2<0)
                    continue;
     
                const cv::KeyPoint &kp1 = pKF1->mvKeysUn[indexKF1];
                const cv::KeyPoint &kp2 = pKF2->mvKeysUn[indexKF2];
     
                const float sigmaSquare1 = pKF1->mvLevelSigma2[kp1.octave];
                const float sigmaSquare2 = pKF2->mvLevelSigma2[kp2.octave];
     
                mvnMaxError1.push_back(9.210*sigmaSquare1);
                mvnMaxError2.push_back(9.210*sigmaSquare2);
     
                mvpMapPoints1.push_back(pMP1);
                mvpMapPoints2.push_back(pMP2);
                mvnIndices1.push_back(i1);
                //pMP1对应的相机坐标
                cv::Mat X3D1w = pMP1->GetWorldPos();
                mvX3Dc1.push_back(Rcw1*X3D1w+tcw1);
     
                cv::Mat X3D2w = pMP2->GetWorldPos();
                //pMP2对应的相机坐标
                mvX3Dc2.push_back(Rcw2*X3D2w+tcw2);
     
                mvAllIndices.push_back(idx);
                idx++;
            }
        }
        //相机内参
        mK1 = pKF1->mK;
        mK2 = pKF2->mK;
        //FromCameraToImage函数计算从相机坐标到像素坐标的投影点，mvP1im1为投影点列表。
        FromCameraToImage(mvX3Dc1,mvP1im1,mK1);
        FromCameraToImage(mvX3Dc2,mvP2im2,mK2);
     
        SetRansacParameters();
    }

其中调用的函数FromCameraToImage代码如下：

/**
     * 计算从相机坐标到像素坐标的投影
    */
    void Sim3Solver::FromCameraToImage(const vector<cv::Mat> &vP3Dc, vector<cv::Mat> &vP2D, cv::Mat K)
    {
        const float &fx = K.at<float>(0,0);
        const float &fy = K.at<float>(1,1);
        const float &cx = K.at<float>(0,2);
        const float &cy = K.at<float>(1,2);
     
        vP2D.clear();
        //vP3Dc为相机坐标，vP2D为要投影到的像素点坐标
        vP2D.reserve(vP3Dc.size());
     
        for(size_t i=0, iend=vP3Dc.size(); i<iend; i++)
        {
            const float invz = 1/(vP3Dc[i].at<float>(2));
            const float x = vP3Dc[i].at<float>(0)*invz;
            const float y = vP3Dc[i].at<float>(1)*invz;
     
            vP2D.push_back((cv::Mat_<float>(2,1) << fx*x+cx, fy*y+cy));
        }
    }

## 2）sim3迭代求解Sim3Solver::iterate ##

iterate函数是开始进行sim3求解的函数。遍历1）中构造好的每个求解器，调用求解器中的iterate函数进行求解。ORB-SLAM2中迭代5次进行求解。

cv::Mat Sim3Solver::iterate(int nIterations, bool &bNoMore, vector<bool> &vbInliers, int &nInliers)
    {
        bNoMore = false;
        vbInliers = vector<bool>(mN1,false);
        nInliers=0;
     
        if(N<mRansacMinInliers)
        {
            bNoMore = true;
            return cv::Mat();
        }
     
        vector<size_t> vAvailableIndices;
     
        cv::Mat P3Dc1i(3,3,CV_32F);
        cv::Mat P3Dc2i(3,3,CV_32F);
     
        int nCurrentIterations = 0;
        while(mnIterations<mRansacMaxIts && nCurrentIterations<nIterations)
        {
            nCurrentIterations++;
            mnIterations++;
     
            vAvailableIndices = mvAllIndices;
     
            // Get min set of points
            // 每次随机获取3对匹配点
            for(short i = 0; i < 3; ++i)
            {
                int randi = DUtils::Random::RandomInt(0, vAvailableIndices.size()-1);
     
                int idx = vAvailableIndices[randi];
     
                mvX3Dc1[idx].copyTo(P3Dc1i.col(i));
                mvX3Dc2[idx].copyTo(P3Dc2i.col(i));
     
                vAvailableIndices[randi] = vAvailableIndices.back();
                vAvailableIndices.pop_back();
            }
            //进行sim3求解
            ComputeSim3(P3Dc1i,P3Dc2i);
     
            CheckInliers();
     
            if(mnInliersi>=mnBestInliers)
            {
                mvbBestInliers = mvbInliersi;
                mnBestInliers = mnInliersi;
                mBestT12 = mT12i.clone();
                mBestRotation = mR12i.clone();
                mBestTranslation = mt12i.clone();
                mBestScale = ms12i;
     
                if(mnInliersi>mRansacMinInliers)
                {
                    nInliers = mnInliersi;
                    for(int i=0; i<N; i++)
                        if(mvbInliersi[i])
                            vbInliers[mvnIndices1[i]] = true;
                    return mBestT12;
                }
            }
        }
     
        if(mnIterations>=mRansacMaxIts)
            bNoMore=true;
     
        return cv::Mat();
    }

/**
     * 计算sim3
    */
    void Sim3Solver::ComputeSim3(cv::Mat &P1, cv::Mat &P2)
    {
        // Custom implementation of:
        // Horn 1987, Closed-form solution of absolute orientataion using unit quaternions
     
        // Step 1: Centroid and relative coordinates
     
        cv::Mat Pr1(P1.size(),P1.type()); // Relative coordinates to centroid (set 1)
        cv::Mat Pr2(P2.size(),P2.type()); // Relative coordinates to centroid (set 2)
        cv::Mat O1(3,1,Pr1.type()); // Centroid of P1
        cv::Mat O2(3,1,Pr2.type()); // Centroid of P2
     
        ComputeCentroid(P1,Pr1,O1);
        ComputeCentroid(P2,Pr2,O2);
     
        // Step 2: Compute M matrix
     
        cv::Mat M = Pr2*Pr1.t();
     
        // Step 3: Compute N matrix
     
        double N11, N12, N13, N14, N22, N23, N24, N33, N34, N44;
     
        cv::Mat N(4,4,P1.type());
     
        N11 = M.at<float>(0,0)+M.at<float>(1,1)+M.at<float>(2,2);
        N12 = M.at<float>(1,2)-M.at<float>(2,1);
        N13 = M.at<float>(2,0)-M.at<float>(0,2);
        N14 = M.at<float>(0,1)-M.at<float>(1,0);
        N22 = M.at<float>(0,0)-M.at<float>(1,1)-M.at<float>(2,2);
        N23 = M.at<float>(0,1)+M.at<float>(1,0);
        N24 = M.at<float>(2,0)+M.at<float>(0,2);
        N33 = -M.at<float>(0,0)+M.at<float>(1,1)-M.at<float>(2,2);
        N34 = M.at<float>(1,2)+M.at<float>(2,1);
        N44 = -M.at<float>(0,0)-M.at<float>(1,1)+M.at<float>(2,2);
     
        N = (cv::Mat_<float>(4,4) << N11, N12, N13, N14,
                                     N12, N22, N23, N24,
                                     N13, N23, N33, N34,
                                     N14, N24, N34, N44);
     
     
        // Step 4: Eigenvector of the highest eigenvalue
     
        cv::Mat eval, evec;
     
        cv::eigen(N,eval,evec); //evec[0] is the quaternion of the desired rotation
     
        cv::Mat vec(1,3,evec.type());
        (evec.row(0).colRange(1,4)).copyTo(vec); //extract imaginary part of the quaternion (sin*axis)
     
        // Rotation angle. sin is the norm of the imaginary part, cos is the real part
        double ang=atan2(norm(vec),evec.at<float>(0,0));
     
        vec = 2*ang*vec/norm(vec); //Angle-axis representation. quaternion angle is the half
     
        mR12i.create(3,3,P1.type());
     
        cv::Rodrigues(vec,mR12i); // computes the rotation matrix from angle-axis
     
        // Step 5: Rotate set 2
     
        cv::Mat P3 = mR12i*Pr2;
     
        // Step 6: Scale
     
        if(!mbFixScale)
        {
            double nom = Pr1.dot(P3);
            cv::Mat aux_P3(P3.size(),P3.type());
            aux_P3=P3;
            cv::pow(P3,2,aux_P3);
            double den = 0;
     
            for(int i=0; i<aux_P3.rows; i++)
            {
                for(int j=0; j<aux_P3.cols; j++)
                {
                    den+=aux_P3.at<float>(i,j);
                }
            }
     
            ms12i = nom/den;
        }
        else
            ms12i = 1.0f;
     
        // Step 7: Translation
     
        mt12i.create(1,3,P1.type());
        mt12i = O1 - ms12i*mR12i*O2;
     
        // Step 8: Transformation
     
        // Step 8.1 T12
        mT12i = cv::Mat::eye(4,4,P1.type());
     
        cv::Mat sR = ms12i*mR12i;
     
        sR.copyTo(mT12i.rowRange(0,3).colRange(0,3));
        mt12i.copyTo(mT12i.rowRange(0,3).col(3));
     
        // Step 8.2 T21
     
        mT21i = cv::Mat::eye(4,4,P1.type());
     
        cv::Mat sRinv = (1.0/ms12i)*mR12i.t();
     
        sRinv.copyTo(mT21i.rowRange(0,3).colRange(0,3));
        cv::Mat tinv = -sRinv*mt12i;
        tinv.copyTo(mT21i.rowRange(0,3).col(3));
    }

[ORB-SLAM2_LoopClosing]: https://blog.csdn.net/moyu123456789/article/details/91374510
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L21veXUxMjM0NTY3ODk_size_16_color_FFFFFF_t_70]: /images/20221119/42b316d831454407adbcfbe9bfcd45dd.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L21veXUxMjM0NTY3ODk_size_16_color_FFFFFF_t_70 1]: https://img-blog.csdnimg.cn/20190621115532129.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L21veXUxMjM0NTY3ODk=,size_16,color_FFFFFF,t_70
[r_l_1]: https://private.codecogs.com/gif.latex?%24r_l%2C_1
[r_l_2]: https://private.codecogs.com/gif.latex?%24r_l%2C_2
[r_l_3]: https://private.codecogs.com/gif.latex?%24r_l%2C_3
[r_r_1]: https://private.codecogs.com/gif.latex?%24r_r%2C_1%24
[r_r_2]: https://private.codecogs.com/gif.latex?%24r_r%2C_2%24
[r_r_3]: https://private.codecogs.com/gif.latex?%24r_%7Br%2C3%7D%24
[r_l_1 1]: https://private.codecogs.com/gif.latex?%24r_%7Bl%2C1%7D%24
[r_r_1 1]: https://private.codecogs.com/gif.latex?%24r_%7Br%2C1%7D%24
[x_l_r_l_2_-r_l_1]: https://private.codecogs.com/gif.latex?x_l%3Dr_%7Bl%2C2%7D-r_%7Bl%2C1%7D%24
[x_l]: https://private.codecogs.com/gif.latex?x_l
[hat_x_l_x_l_left _ x_l _right]: https://private.codecogs.com/gif.latex?%5Chat%7Bx_l%7D%3Dx_l/%5Cleft%20%5C%7C%20x_l%20%5Cright%20%5C%7C
[x_l_1]: https://private.codecogs.com/gif.latex?x_%7Bl%2C1%7D
[r_l_2 1]: https://private.codecogs.com/gif.latex?r_%7Bl%2C2%7D
[y_l_r_l_3_-r_l_1_-_left _ _left _ r_l_3_-r_l_1_ _right _cdot _hat_x_l_ _right _cdot _hat_x_l]: https://private.codecogs.com/gif.latex?y_l%3D%28r_%7Bl%2C3%7D-r_%7Bl%2C1%7D%29-%5Cleft%20%5B%20%5Cleft%20%28%20r_%7Bl%2C3%7D-r_%7Bl%2C1%7D%20%5Cright%20%29%5Ccdot%20%5Chat%7Bx_l%7D%20%5Cright%20%5D%5Ccdot%20%5Chat%7Bx_l%7D
[y_l]: https://private.codecogs.com/gif.latex?y_l
[hat_y_l_hat_y_l_left _ y_l _right]: https://private.codecogs.com/gif.latex?%5Chat%7By_l%7D%3D%5Chat%7By_l%7D/%5Cleft%20%5C%7C%20y_l%20%5Cright%20%5C%7C
[r_l_3_-r_l_1]: https://private.codecogs.com/gif.latex?r_%7Bl%2C3%7D-r_%7Bl%2C1%7D
[r_l_1 2]: https://private.codecogs.com/gif.latex?r_%7Bl%2C1%7D
[r_l_3 1]: https://private.codecogs.com/gif.latex?r_%7Bl%2C3%7D
[left _ r_l_3_-r_l_1_ _right _cdot_hat_x_l]: https://private.codecogs.com/gif.latex?%5Cleft%20%28%20r_%7Bl%2C3%7D-r_%7Bl%2C1%7D%20%5Cright%20%29%5Ccdot%5Chat%7Bx_l%7D
[left _ _left _ r_l_3_-r_l_1_ _right _cdot _hat_x_l_ _right _cdot _hat_x_l]: https://private.codecogs.com/gif.latex?%5Cleft%20%5B%20%5Cleft%20%28%20r_%7Bl%2C3%7D-r_%7Bl%2C1%7D%20%5Cright%20%29%5Ccdot%20%5Chat%7Bx_l%7D%20%5Cright%20%5D%5Ccdot%20%5Chat%7Bx_l%7D
[hat_z_l_hat_x_l_times _hat_y_l]: https://private.codecogs.com/gif.latex?%5Chat%7Bz_l%7D%3D%5Chat%7Bx_l%7D%5Ctimes%20%5Chat%7By_l%7D
[hat_x_r_hat_y_r_hat_z_r]: https://private.codecogs.com/gif.latex?%5Chat%7Bx_r%7D%2C%5Chat%7By_r%7D%2C%5Chat%7Bz_r%7D
[large M_l_left _ _hat_x_l_ _hat_y_l_hat_z_l_right]: https://private.codecogs.com/gif.latex?%5Clarge%20M_l%3D%5Cleft%20%7C%20%5Chat%7Bx_l%7D%20%5Chat%7By_l%7D%5Chat%7Bz_l%7D%5Cright%20%7C
[large M_l_left _ _hat_x_r_hat_y_r_ _hat_z_r_ _right]: https://private.codecogs.com/gif.latex?%5Clarge%20M_l%3D%5Cleft%20%7C%20%5Chat%7Bx_r%7D%5Chat%7By_r%7D%20%5Chat%7Bz_r%7D%20%5Cright%20%7C
[r_l]: https://private.codecogs.com/gif.latex?r_l
[M_l_T_cdot r_l]: https://private.codecogs.com/gif.latex?M_l%5ET%5Ccdot%20r_l
[M_r]: https://private.codecogs.com/gif.latex?M_r
[large r_r_M_rM_l_Tr_l]: https://private.codecogs.com/gif.latex?%5Clarge%20r_r%3DM_rM_l%5ETr_l
[large _color_Red_ R_M_r_M_l_T]: https://private.codecogs.com/gif.latex?%5Clarge%20%7B%5Ccolor%7BRed%7D%20R%3DM_r%7BM_l%5ET%7D%7D
[n]: https://private.codecogs.com/gif.latex?n
[left _ r_l_i_ _right]: https://private.codecogs.com/gif.latex?%5Cleft%20%5C%7B%20r_%7Bl%2Ci%7D%20%5Cright%20%5C%7D
[left _ r_r_i_ _right]: https://private.codecogs.com/gif.latex?%5Cleft%20%5C%7B%20r_%7Br%2Ci%7D%20%5Cright%20%5C%7D
[i]: https://private.codecogs.com/gif.latex?i
[large r_r _ sR_r_l_r_0]: https://private.codecogs.com/gif.latex?%5Clarge%20r_r%20%3D%20sR%28r_l%29&plus;r_0
[large r_0]: https://private.codecogs.com/gif.latex?%5Clarge%20r_0
[large R_r_l]: https://private.codecogs.com/gif.latex?%5Clarge%20R%28r_l%29
[large r_l]: https://private.codecogs.com/gif.latex?%5Clarge%20r_l
[large e_i_r_r_i_-sR_left _ r_l_i_ _right _-r_0.]: https://private.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Clarge%20e_i%3Dr_%7Br%2Ci%7D-sR%5Cleft%20%28%20r_%7Bl%2Ci%7D%20%5Cright%20%29-r_0.
[large min_sum_i_1_n_left _ e_i _right _2.]: https://private.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Clarge%20min%5Csum_%7Bi%3D1%7D%5En%5Cleft%20%5C%7C%20e_i%20%5Cright%20%5C%7C%5E2.
[large r_0 1]: https://private.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Clarge%20r_0
[large _bar_r_l_frac_1_n_sum _i_1_nr_l_i]: https://private.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Clarge%20%5Cbar%7Br_l%7D%3D%5Cfrac%7B1%7D%7Bn%7D%5Csum%20_%7Bi%3D1%7D%5Enr_%7Bl%2Ci%7D
[large _bar_r_r_frac_1_n_sum _i_1_nr_r_i]: https://private.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Clarge%20%5Cbar%7Br_r%7D%3D%5Cfrac%7B1%7D%7Bn%7D%5Csum%20_%7Bi%3D1%7D%5Enr_%7Br%2Ci%7D
[large _r_l_i_r_l_i_-_bar_r_l]: https://private.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Clarge%20%7Br_%7Bl%2Ci%7D%7D%27%3Dr_%7Bl%2Ci%7D-%5Cbar%7Br_l%7D
[large _r_r_i_r_r_i_-_bar_r_r]: https://private.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Clarge%20%7Br_%7Br%2Ci%7D%7D%27%3Dr_%7Br%2Ci%7D-%5Cbar%7Br_r%7D
[large _sum _i_1_n_r_l_i_0]: https://private.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Clarge%20%5Csum%20_%7Bi%3D1%7D%5E%7Bn%7Dr_%7Bl%2Ci%7D%5E%7B%27%7D%3D0
[large _sum _i_1_n_r_r_i_0]: https://private.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Clarge%20%5Csum%20_%7Bi%3D1%7D%5E%7Bn%7Dr_%7Br%2Ci%7D%5E%7B%27%7D%3D0
[large r_r_i_sR_r_l_i_r_0]: https://private.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Clarge%20r_%7Br%2Ci%7D%5E%7B%27%7D%3DsR%28r_%7Bl%2Ci%7D%5E%7B%27%7D%29&plus;%7Br_0%7D%27
[large _Rightarrow]: https://private.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Clarge%20%5CRightarrow
[large _r_0_r_r_i_-sR_r_l_i]: https://private.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Clarge%20%7Br_0%7D%27%3D%7Br_%7Br%2Ci%7D%7D%27-sR%28%7Br_%7Bl%2Ci%7D%7D%27%29
[large _r_0_r_r_i_-_bar_r_r_-sR_r_l_i_-_bar_r_l]: https://private.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Clarge%20%7Br_0%7D%27%3Dr_%7Br%2Ci%7D-%5Cbar%7Br_r%7D-sR%28r_%7Bl%2Ci%7D-%5Cbar%7Br_l%7D%29
[large _r_0_r_r_i_-_bar_r_r_-sR_r_l_i_sR_bar_r_l]: https://private.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Clarge%20%7Br_0%7D%27%3Dr_%7Br%2Ci%7D-%5Cbar%7Br_r%7D-sR%28r_%7Bl%2Ci%7D%29&plus;sR%28%5Cbar%7Br_l%7D%29
[large _r_0_r_0-_bar_r_r_sR_bar_r_l]: https://private.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Clarge%20%7Br_0%7D%27%3Dr_0-%5Cbar%7Br_r%7D&plus;sR%28%5Cbar%7Br_l%7D%29
[large _sum _i_1_n_left _ e_i _right _2_sum _i_1_n_left _ _r_r_i_-_left _ sR_r_l_i_r_0_ _right _ _right _2]: https://private.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Clarge%20%5Csum%20_%7Bi%3D1%7D%5E%7Bn%7D%5Cleft%20%5C%7C%20e_i%20%5Cright%20%5C%7C%5E2%3D%5Csum%20_%7Bi%3D1%7D%5En%5Cleft%20%5C%7C%20%7Br_%7Br%2Ci%7D%7D%27-%5Cleft%20%5B%20sR%28%7Br_%7Bl%2Ci%7D%7D%27%29&plus;%7Br_0%7D%27%20%5Cright%20%5D%20%5Cright%20%5C%7C%5E2
[large _sum _i_1_n_left _ _r_r_i_-sR_r_l_i_ _right _2-2_r_0_cdot _sum _i_1_n_left _ _r_r_i_-sR_r_l_i_ _right _n_left _ _r_0_ _right _2]: https://private.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Clarge%20%3D%5Csum%20_%7Bi%3D1%7D%5E%7Bn%7D%5Cleft%20%5C%7C%20%7Br_%7Br%2Ci%7D%7D%27-sR%28%7Br_%7Bl%2Ci%7D%7D%27%29%20%5Cright%20%5C%7C%5E2-2%7Br_0%7D%27%5Ccdot%20%5Csum%20_%7Bi%3D1%7D%5E%7Bn%7D%5Cleft%20%5B%20%7Br_%7Br%2Ci%7D%7D%27-sR%28%7Br_%7Bl%2Ci%7D%7D%27%29%20%5Cright%20%5D&plus;n%5Cleft%20%5C%7C%20%7Br_0%7D%27%20%5Cright%20%5C%7C%5E2
[large 2_r_0_cdot _sum _i_1_n_left _ _r_r_i_-sR_r_l_i_ _right _0]: https://private.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Clarge%202%7Br_0%7D%27%5Ccdot%20%5Csum%20_%7Bi%3D1%7D%5E%7Bn%7D%5Cleft%20%5B%20%7Br_%7Br%2Ci%7D%7D%27-sR%28%7Br_%7Bl%2Ci%7D%7D%27%29%20%5Cright%20%5D%3D0
[large _r_0]: https://private.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Clarge%20%7Br_0%7D%27
[large _r_0_0]: https://private.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Clarge%20%7Br_0%7D%27%3D0
[large t]: https://private.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Clarge%20t
[large _color_Red_ t_r_0_bar_r_r_-sR_bar_r_l]: https://private.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Clarge%20%7B%5Ccolor%7BRed%7D%20t%3Dr_0%3D%5Cbar%7Br_r%7D-sR%28%5Cbar%7Br_l%7D%29%7D
[large _sum _i_1_n_left _ _r_r_i_-_left _ sR_r_l_i_ _right _ _right _2]: https://private.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Clarge%20%3D%5Csum%20_%7Bi%3D1%7D%5En%5Cleft%20%5C%7C%20%7Br_%7Br%2Ci%7D%7D%27-%5Cleft%20%5B%20sR%28%7Br_%7Bl%2Ci%7D%7D%27%29%20%5Cright%20%5D%20%5Cright%20%5C%7C%5E2
[large _sum _i_1_n_left _ _r_r_i_ _right _2-2s_sum _i_1_n_r_r_i_cdot R_r_l_i_s_2_sum _i_1_n_left _ R_r_l_i_ _right _2]: https://private.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Clarge%20%3D%5Csum%20_%7Bi%3D1%7D%5En%5Cleft%20%5C%7C%20%7Br_%7Br%2Ci%7D%7D%27%20%5Cright%20%5C%7C%5E2-2s%5Csum%20_%7Bi%3D1%7D%5En%7Br_%7Br%2Ci%7D%7D%27%5Ccdot%20R%28%7Br_%7Bl%2Ci%7D%7D%27%29&plus;s%5E2%5Csum%20_%7Bi%3D1%7D%5E%7Bn%7D%5Cleft%20%5C%7C%20R%28%7Br_%7Bl%2Ci%7D%7D%27%29%20%5Cright%20%5C%7C%5E2
[large _sum _i_1_n_left _ e_i _right _2_S_r-2sD_s_2S_l]: https://private.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Clarge%20%5Csum%20_%7Bi%3D1%7D%5E%7Bn%7D%5Cleft%20%5C%7C%20e_i%20%5Cright%20%5C%7C%5E2%3DS_r-2sD&plus;s%5E2S_l
[large _s_sqrt_S_l_-D_sqrt_S_l_2_S_rS_l-D_2_S_l]: https://private.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Clarge%20%3D%28s%5Csqrt%7BS_l%7D-D/%5Csqrt%7BS_l%7D%29%5E2&plus;%28S_rS_l-D%5E%7B2%7D%29/S_l
[large _color_Red_ _color_Red_ _s_D_S_l_left _ _sum _i_1_n_r_r_i_cdot R_r_l_i_ _right _sum _i_1_n_left _ R_r_l_i_ _right _2]: https://private.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Clarge%20%7B%5Ccolor%7BRed%7D%20%7B%5Ccolor%7BRed%7D%20%7Ds%3DD/S_l%3D%5Cleft%20%28%20%5Csum%20_%7Bi%3D1%7D%5En%7Br_%7Br%2Ci%7D%7D%27%5Ccdot%20R%28%7Br_%7Bl%2Ci%7D%7D%27%29%20%5Cright%20%29/%5Csum%20_%7Bi%3D1%7D%5E%7Bn%7D%5Cleft%20%5C%7C%20R%28%7Br_%7Bl%2Ci%7D%7D%27%29%20%5Cright%20%5C%7C%5E2%7D
[large r_r_sR_r_l_r_0]: https://private.codecogs.com/gif.latex?%5Clarge%20r_r%3DsR%28r_l%29&plus;r_0
[large r_l_bar_s_bar_R_r_r_bar_r_0]: https://private.codecogs.com/gif.latex?%5Clarge%20r_l%3D%5Cbar%7Bs%7D%5Cbar%7BR%7D%28r_r%29&plus;%5Cbar%7Br_0%7D
[large _bar_s_1_s]: https://private.codecogs.com/gif.latex?%5Clarge%20%5Cbar%7Bs%7D%3D1/s
[large _bar_r_0_-_frac_1_s_R_-1_r_0]: https://private.codecogs.com/gif.latex?%5Clarge%20%5Cbar%7Br_0%7D%3D-%5Cfrac%7B1%7D%7Bs%7DR%5E%7B-1%7D%28r_0%29
[large _bar_R_R_-1]: https://private.codecogs.com/gif.latex?%5Clarge%20%5Cbar%7BR%7D%3DR%5E%7B-1%7D
[large _bar_s_1_s_neq_left _ _sum _i_1_n_r_r_i_cdot R_r_l_i_ _right _sum _i_1_n_left _ R_r_l_i_ _right _2]: https://private.codecogs.com/gif.latex?%5Clarge%20%5Cbar%7Bs%7D%3D1/s%5Cneq%5Cleft%20%28%20%5Csum%20_%7Bi%3D1%7D%5En%7Br_%7Br%2Ci%7D%7D%27%5Ccdot%20R%28%7Br_%7Bl%2Ci%7D%7D%27%29%20%5Cright%20%29/%5Csum%20_%7Bi%3D1%7D%5E%7Bn%7D%5Cleft%20%5C%7C%20R%28%7Br_%7Bl%2Ci%7D%7D%27%29%20%5Cright%20%5C%7C%5E2%7D
[large e_i_frac_1_sqrt_S_r_r_i_-_sqrt_s_R_r_l_i]: https://private.codecogs.com/gif.latex?%5Clarge%20e_i%3D%5Cfrac%7B1%7D%7B%5Csqrt%7BS%7D%7D%7Br_%7Br%2Ci%7D%7D%27-%5Csqrt%7Bs%7DR%28%7Br_%7Bl%2Ci%7D%7D%27%29
[large _sum _i_1_n_left _ e_i _right _2_frac_1_s_S_r-2D_sS_r_sqrt_s_S_l-_frac_1_sqrt_s_S_r_2_2_S_lS_r-D]: https://private.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Clarge%20%5Csum%20_%7Bi%3D1%7D%5E%7Bn%7D%5Cleft%20%5C%7C%20e_i%20%5Cright%20%5C%7C%5E2%3D%5Cfrac%7B1%7D%7Bs%7DS_r-2D&plus;sS_r%3D%28%5Csqrt%7Bs%7DS_l-%5Cfrac%7B1%7D%7B%5Csqrt%7Bs%7D%7DS_r%29%5E2&plus;2%28S_lS_r-D%29
[large _color_Red_ s_sum _i_1_n_left _ _r_r_i_ _right _2_sum _i_1_n_left _ _r_l_i_ _right _2_frac_1_2]: https://private.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Clarge%20%7B%5Ccolor%7BRed%7D%20s%3D%28%5Csum%20_%7Bi%3D1%7D%5En%5Cleft%20%5C%7C%20%7Br_%7Br%2Ci%7D%7D%27%20%5Cright%20%5C%7C%5E2/%5Csum%20_%7Bi%3D1%7D%5En%5Cleft%20%5C%7C%20%7Br_%7Bl%2Ci%7D%7D%27%20%5Cright%20%5C%7C%5E2%29%5E%5Cfrac%7B1%7D%7B2%7D%7D
[large _color_Red_ _sum _i_1_n_r_r_i_cdot R_left _ _r_l_i_ _right]: https://private.codecogs.com/gif.latex?%5Clarge%20%7B%5Ccolor%7BRed%7D%20%5Csum%20_%7Bi%3D1%7D%5En%7Br_%7Br%2Ci%7D%7D%27%5Ccdot%20R%5Cleft%20%28%20%7Br_%7Bl%2Ci%7D%7D%27%20%5Cright%20%29%7D
[large D_color_Red_ _sum _i_1_n_r_r_i_cdot R_left _ _r_l_i_ _right]: https://private.codecogs.com/gif.latex?%5Clarge%20D%3D%7B%5Ccolor%7BRed%7D%20%5Csum%20_%7Bi%3D1%7D%5En%7Br_%7Br%2Ci%7D%7D%27%5Ccdot%20R%5Cleft%20%28%20%7Br_%7Bl%2Ci%7D%7D%27%20%5Cright%20%29%7D
[large D]: https://private.codecogs.com/gif.latex?%5Clarge%20D
[large _qq]: https://private.codecogs.com/gif.latex?%5Clarge%20%5Cqq
[large _overset_circ _q_ q_0_iq_x_jq_y_kq_z]: https://private.codecogs.com/gif.latex?%5Clarge%20%5Coverset%7B%5Ccirc%20%7D%7Bq%7D%3D%20q_0&plus;iq_x&plus;jq_y&plus;kq_z
[large q_0]: https://private.codecogs.com/gif.latex?%5Clarge%20q_0
[large i_2_-1]: https://private.codecogs.com/gif.latex?%5Clarge%20i%5E2%3D-1%2C
[large j_2_-1]: https://private.codecogs.com/gif.latex?%5Clarge%20j%5E2%3D-1%2C
[large k_2_-1]: https://private.codecogs.com/gif.latex?%5Clarge%20k%5E2%3D-1%3B
[large ij_k]: https://private.codecogs.com/gif.latex?%5Clarge%20ij%3Dk%2C
[large jk_i]: https://private.codecogs.com/gif.latex?%5Clarge%20jk%3Di%2C
[large ki_j]: https://private.codecogs.com/gif.latex?%5Clarge%20ki%3Dj%3B
[large ji_-k]: https://private.codecogs.com/gif.latex?%5Clarge%20ji%3D-k%2C
[large kj_-i]: https://private.codecogs.com/gif.latex?%5Clarge%20kj%3D-i%2C
[large ik_-j]: https://private.codecogs.com/gif.latex?%5Clarge%20ik%3D-j%3B
[large _overset_circ_p_cdot _overset_circ_q_p_0_q_0_p_x_q_x_p_yq_y_p_zq_z]: https://private.codecogs.com/gif.latex?%5Clarge%20%5Coverset%7B%5Ccirc%7D%7Bp%7D%5Ccdot%20%5Coverset%7B%5Ccirc%7D%7Bq%7D%3Dp_%7B0%7Dq_%7B0%7D&plus;p_%7Bx%7Dq_%7Bx%7D&plus;p_yq_y&plus;p_zq_z
[large _overset_circ_q]: https://private.codecogs.com/gif.latex?%5Clarge%20%5Coverset%7B%5Ccirc%7D%7Bq%7D
[large _overset_circ_q_ast _q_0-iq_x-jq_y-kq_z]: https://private.codecogs.com/gif.latex?%5Clarge%20%5Coverset%7B%5Ccirc%7D%7Bq%7D%5E%5Cast%20%3Dq_0-iq_x-jq_y-kq_z
[large _overset_circ_q_cdot _overset_circ_q_ast _q_0_2_q_x_2_q_y_2_q_z_2_overset_circ_q_cdot _overset_circ_q]: https://private.codecogs.com/gif.latex?%5Clarge%20%5Coverset%7B%5Ccirc%7D%7Bq%7D%5Ccdot%20%5Coverset%7B%5Ccirc%7D%7Bq%7D%5E%5Cast%20%3Dq_0%5E2&plus;q_x%5E2&plus;q_y%5E2&plus;q_z%5E2%3D%5Coverset%7B%5Ccirc%7D%7Bq%7D%5Ccdot%20%5Coverset%7B%5Ccirc%7D%7Bq%7D
[large _sum _i_1_n_left _ _overset_circ_q_ _overset_circ_r_l_i_ _overset_circ_q_right _cdot _overset_circ_r_r_i_.]: https://private.codecogs.com/gif.latex?%5Clarge%20%5Csum%20_%7Bi%3D1%7D%5En%5Cleft%20%28%20%5Coverset%7B%5Ccirc%7D%7Bq%7D%20%5Coverset%7B%5Ccirc%7D%7B%7Br%7D%27%7D_%7Bl%2Ci%7D%20%5Coverset%7B%5Ccirc%7D%7Bq%7D%5E*%5Cright%20%29%5Ccdot%20%5Coverset%7B%5Ccirc%7D%7B%7Br%7D%27_%7Br%2Ci%7D%7D.
[large _sum _i_1_n_left _ _overset_circ_q_ _overset_circ_r_l_i_ _right _cdot _left _ _overset_circ_r_r_i_ _overset_circ_q_right _.]: https://private.codecogs.com/gif.latex?%5Clarge%20%5Csum%20_%7Bi%3D1%7D%5En%5Cleft%20%28%20%5Coverset%7B%5Ccirc%7D%7Bq%7D%20%5Coverset%7B%5Ccirc%7D%7B%7Br%7D%27%7D_%7Bl%2Ci%7D%20%5Cright%20%29%5Ccdot%20%5Cleft%20%28%20%5Coverset%7B%5Ccirc%7D%7B%7Br%7D%27_%7Br%2Ci%7D%7D%20%5Coverset%7B%5Ccirc%7D%7Bq%7D%5Cright%20%29.
[large _r_l_i_left _ _x_l_i_y_l_i_z_l_i_ _right _T]: https://private.codecogs.com/gif.latex?%5Clarge%20%7Br_%7Bl%2Ci%7D%7D%27%3D%5Cleft%20%28%20%7Bx_%7Bl%2Ci%7D%7D%27%2C%7By_%7Bl%2Ci%7D%7D%27%2C%7Bz_%7Bl%2Ci%7D%7D%27%20%5Cright%20%29%5ET
[large _r_r_i_left _ _x_r_i_y_r_i_z_r_i_ _right _T]: https://private.codecogs.com/gif.latex?%5Clarge%20%7Br_%7Br%2Ci%7D%7D%27%3D%5Cleft%20%28%20%7Bx_%7Br%2Ci%7D%7D%27%2C%7By_%7Br%2Ci%7D%7D%27%2C%7Bz_%7Br%2Ci%7D%7D%27%20%5Cright%20%29%5ET
[large M_sum _i_1_n_r_l_i_r_r_i_T]: https://private.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Clarge%20M%3D%5Csum%20_%7Bi%3D1%7D%5En%7Br_%7Bl%2Ci%7D%7D%27%7Br_%7Br%2Ci%7D%7D%27%5ET
[large M]: https://private.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Clarge%20M
[large M_begin_bmatrix_ S_xx_ _ S_xy_ _ S_xz_ S_yx_ _ S_yy_ _ S_yz_ S_zx_ _ S_zy _ S_zz_ _end_bmatrix]: https://private.codecogs.com/gif.latex?%5Cdpi%7B120%7D%20%5Clarge%20M%3D%5Cbegin%7Bbmatrix%7D%20S_%7Bxx%7D%20%26%20S_%7Bxy%7D%20%26%20S_%7Bxz%7D%5C%5C%20S_%7Byx%7D%20%26%20S_%7Byy%7D%20%26%20S_%7Byz%7D%5C%5C%20S_%7Bzx%7D%20%26%20S_%7Bzy%20%7D%26%20S_%7Bzz%7D%20%5Cend%7Bbmatrix%7D
[large N_begin_bmatrix_ _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_bmatrix]: https://private.codecogs.com/gif.latex?%5Clarge%20N%3D%5Cbegin%7Bbmatrix%7D%20%28S_%7Bxx%7D&plus;S_%7Byy%7D&plus;S_%7Bzz%7D%29%20%26S_%7Byz%7D-S_%7Bzy%7D%20%26S_%7Bzx%7D-S_%7Bxz%7D%20%26S_%7Bxy%7D-S_%7Byx%7D%20%5C%5C%20S_%7Byz%7D-S_%7Bzy%7D%26%20%28S_%7Bxx%7D-S_%7Byy%7D-S_%7Bzz%7D%29%20%26S_%7Bxy%7D&plus;S_%7Byx%7D%20%26%20S_%7Bzx%7D&plus;S_%7Bxz%7D%5C%5C%20S_%7Bzx%7D-S_%7Bxz%7D%26%20S_%7Bxy%7D&plus;S_%7Byx%7D%20%26%20%28-S_%7Bxx%7D&plus;S_%7Byy%7D-S_%7Bzz%7D%29%20%26S_%7Byz%7D&plus;S_%7Bzy%7D%5C%5C%20S_%7Bxy%7D-S_%7Byx%7D%26%20S_%7Bzx%7D&plus;S_%7Bxz%7D%26S_%7Byz%7D&plus;S_%7Bzy%7D%20%26%20%28-S_%7Bxx%7D-S_%7Byy%7D&plus;S_%7Bzz%7D%29%20%5Cend%7Bbmatrix%7D
[large _color_Red_ q_cos_frac_Theta _2_nsin_frac_Theta _2]: https://private.codecogs.com/gif.latex?%5Clarge%20%7B%5Ccolor%7BRed%7D%20q%3Dcos%5Cfrac%7B%5CTheta%20%7D%7B2%7D&plus;nsin%5Cfrac%7B%5CTheta%20%7D%7B2%7D%7D