视觉惯性单目SLAM （二） 算法基础知识

1. 基本概念

• 视觉惯性：Visual-Inertial (VI)
• VI ORB-SLAM：视觉惯性ORB-SLAM
• VI ORB-SLAM输入：
  
  • IMU数据（用B表示，加速度：$\mathbf a_B ； 角速度：\omega_B$；时间间隔：$\bigtriangleup t$）
  • 单目图像
  • 在图像中提取KeyPoints时，对像素坐标进行畸变校正，即此KeyPoint坐标可与投影点坐标进行匹配
• IMU数据（加速度和角速度）的测量除了被传感器噪声影响之外，还被缓慢变化的加速度(Accelerometer)偏差（$b_a$）和陀螺仪(Gyroscope)偏差（$b_g$）
• 加速度：受重力加速度（$\Bbb g_w$）影响，所在在计算运动时，需要减去重力加速度的作用。
• $SO(3)$：The group of rotations about the origin in 3 dimensions
• $SE(3)$：The group of rigid body motions (comprising rotations and translations) in 3 dimensions

2. 向量

• 向量的运处可以由坐标运算来表达；下面的a、b都为列向量。

2.1 向量加减法

$$a\pm b = \sum_{i=0}^3 (a_i \pm b_i)$$

2.2 向量内积/点乘(结果为实数)

$$a \cdot b = a^Tb = \sum_{i=0}^3 a_i b_i = |a||b|cos(a,b)$$

2.3 向量外积/叉乘(结果为向量)

$$a \times b = \begin{bmatrix} i & j & k \\ a_1 & a_2 & a_3 \\ b_1 & b_2 & b_3 \\ \end{bmatrix} = \begin{bmatrix} a_2b_3-a_3b_2 \\ a_3b_1-a_1b_3 \\ a_1b_2-a_2b_1 \\ \end{bmatrix}= \begin{bmatrix} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \\ \end{bmatrix} b$$

2.4 向量欧氏长度(向量2范数)

• $x$：为列向量
• $||x||$：向量欧氏长度(向量2范数)
• $||x|| = (x^Tx)^{1/2}$

3. 特殊矩阵

3.1 旋转矩阵 (特殊正交群)

• R是一个正交矩阵（$RR^T = I$)
• R的行列式为+1 （$det(A) = +1$）
  $$SO(n) = \{R \in \mathbb R^{n \times n} \; | \; RR^T=I, \;det(A) = 1\}$$

3.2 对称矩阵 (Symmetric Matrix)

• 对称矩阵：$A=A^T$
• 若A是实对称矩阵，则有：
  $$A = UDU^T$$
  
  • U：正交矩阵
  • D：实对角矩阵
  • 实对称矩阵：有实特征值；且特征向量正交
• $\color {red}{提取实对称矩阵的特征值：雅可比方法（Jacobi’s method） }$

3.3 反对称矩阵 (Skew-Symmetric Matrix)

• 反对称矩阵：$A=-A^T$
• 若S是实反对称矩阵，则有：
  $$S = UBU^T$$
  
  • B：块对角矩阵，其形式为：
    $$diag(a_1Z, a_2Z, ..., a_mZ, 0, ..., 0), \; where Z = \begin{bmatrix} 0 & 1 \\ -1 & 0 \end{bmatrix}$$
  • S的特征向量都是纯虚数，奇数阶的反对称矩阵是奇异的(不可逆)
• $\color {red}{向量的反对称矩阵}$：若$a = (a_1, a_2, a_3) ^T$是一个3维列向量， 其反对称矩阵为：
  $$[a]_{\times} = \begin{bmatrix} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \\ \end{bmatrix}$$
  
  • $a$：是$3 \times 1$的列向量
  • $[a]_{\times}$：是$3 \times 3$的反对称矩阵
  • 矩阵$[a]_{\times}$：是奇异矩阵（不可逆）
• 向量叉乘与反对称矩阵的关系：
  $$a \times b = a ∧ b = \begin{bmatrix} i & j & k \\ a_1 & a_2 & a_3 \\ b_1 & b_2 & b_3 \\ \end{bmatrix} = \begin{bmatrix} a_2b_3-a_3b_2 \\ a_3b_1-a_1b_3 \\ a_1b_2-a_2b_1 \\ \end{bmatrix}= \begin{bmatrix} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \\ \end{bmatrix} b = [a]_{\times} b = (a^T[b]_{\times})^T$$

$$a \times b = a ∧ b = \begin{bmatrix} a_2b_3-a_3b_2 \\ a_3b_1-a_1b_3 \\ a_1b_2-a_2b_1 \\ \end{bmatrix}= \begin{bmatrix} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \\ \end{bmatrix} b = [a]_{\times} b = (a^T[b]_{\times})^T$$

4. 3D坐标系

4.1 坐标系旋转

• 灰色的为坐标系1 ($V_1 = (3 \;\; 2 \;\; 2)^T$)，黑色的为坐标系2 ($V_2 = (3.54 \; -0.707 \;\; 2)^T$$)
  
• 坐标系2的x轴和y轴相对于坐标系1有旋转，其旋转矩阵为：
  $$R= \begin{bmatrix} 0.707 & 0.707 & 0 \\ -0.707 & 0.707 & 0 \\ 0 & 0 & 1 \\ \end{bmatrix} \\ $$

$$V_2 = RV_1= \begin{bmatrix} 0.707 & 0.707 & 0 \\ -0.707 & 0.707 & 0 \\ 0 & 0 & 1 \\ \end{bmatrix} \begin{bmatrix} 3 \\ 2 \\ 2 \\ \end{bmatrix}= \begin{bmatrix} 3.54 \\ -0.707 \\ 2 \\ \end{bmatrix}$$

• 旋转矩阵$R$的直观理解：

  • $R$的列对应于单位向量

  • 此单位向量的的值为坐标系1的对应轴的单位向量在坐标系2中值
  • $R$的第一列：为坐标系1中点$(1 \quad 0 \quad 0)^T$在坐标系2中的值
  • $R$的第二列：为坐标系1中点$(0 \quad 1 \quad 0)^T$在坐标系2中的值
  • $R$的第三列：为坐标系1中点$(0 \quad 0 \quad 1)^T$在坐标系2中的值

  • 只有满足以下要求的$3 \times 3$矩阵($R$)才能表示坐标轴旋转：

    • R的列向量的长度（2-范数）都为1
    • R的列向量相互正交（即内积为0）
    • R的行列式为1（$detA = 1$）

    • 4.2 坐标系平移

      • 灰色的为坐标系1 ($V_1 = (3 \quad 2 \quad 2)^T$)，黑色的为坐标系2 ($V_2 = (1 \quad 1 \quad 2)^T$
        
      • $V_2 = V_1 + t$： $t$为坐标系1的原点在坐标系2中的位置$(-2 \quad -1 \quad 0)^T$

      4.3 坐标系旋转与平移的组合

      • 灰色的为坐标系1 ($V_1 = (3 \quad 2 \quad 2)^T$)，黑色的为坐标系2 ($V_2 = (1.414 \quad 0 \quad 2)^T$
        
        $$R= \begin{bmatrix} 0.707 & 0.707 & 0 \\ -0.707 & 0.707 & 0 \\ 0 & 0 & 1 \\ \end{bmatrix}, \quad t= \begin{bmatrix} -2.121 \\ 0.707 \\ 0 \\ \end{bmatrix} \\ V_2 = RV_1 + t$$

      4.4 齐次坐标系

      • 上面的旋转和平移需要用一个方程来表示：$V_2 = R V_1 + t$
      • $\color {red}{坐标系的旋转和平移能否用一个矩阵来表示呢？}$
      • $\color {red}{齐次坐标系}$：可把坐标系的旋转和平移用一个矩阵来表示
        $$T_{4 \times 4}=\begin{bmatrix} R_{3 \times 3} & t_{3 \times 1} \\ 0_{1 \times 3} & 1_{1 \times 1} \\ \end{bmatrix} \\ T_{4 \times 4}^{-1}=\begin{bmatrix} R_{3 \times 3}^T & -R_{3 \times 3}^Tt_{3 \times 1} \\ 0_{1 \times 3} & 1_{1 \times 1} \\ \end{bmatrix} \\ \begin{bmatrix} 0.707 & 0.707 & 0 & -2.121 \\ -0.707 & 0.707 & 0 & 0.707 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{bmatrix} \begin{bmatrix} 3 \\ 2 \\ 2 \\ 1 \\ \end{bmatrix}= \begin{bmatrix} 1.414 \\ 0 \\ 2 \\ 1 \\ \end{bmatrix}$$

      5. 李群与李代数（Lie Group and Lie Algebra）

      5.1 引入李群与李代数的原因

      • 三维世界刚体运动的描述：$\color{red}{旋转矩阵、旋转向量、欧拉角、四元数等}$
      • 除了$\color{red}{表示}$之外， 还需要对其进行$\color{red}{优化}$和$\color{red}{估计}$
      • 旋转矩阵自身带有约束（$\color{red}{正交且行列式为1}$），作为优化变量时，会引入额外的约束，使优化变得困难
      • 李代数上可以变为$\color{red}{无约束优化}$

      5.2 特殊正交群与特殊欧氏群

      • 三维旋转矩阵构成了$\color {red}{特殊正交群(Special \; Orthogonal \; group)}$
        $$SO(3) = \{ R \in \mathbb R ^{3 \times 3} \quad | \quad RR^T = I, \quad det(R) = 1 \}$$
      • 三维变换矩阵(旋转+平移)构成了$\color {red}{特殊欧氏群(Special \; Euclidean \; group)欧氏意为刚体变换}$
        $$SE(3) = \left\{ T = \begin{bmatrix} R & t \\ 0^T & 1 \end{bmatrix} \in \mathbb R^{4 \times 4} \quad | \quad R \in SO(3), \quad t \in \mathbb R^3 \right\}$$
      • 群(Group)：是一种集合($A$)加上一种运算的代数结构，此运算满足以下性质(凤姐咬你)：
        
        • 封闭性：$\forall a_1, a_2 \in A, \quad a_1 \cdot a_2 \in A$
        • 结合律：$\forall a_1, a_2, a_3 \in A, \quad (a_1 \cdot a_2) \cdot a_3 = a_1 \cdot (a_2 \cdot a_3)$
        • 幺元：$\exists a_0 \in A, \quad s.t. \forall a \in A, \quad a_0 \cdot a = a \cdot a_0 = a$
        • 逆：$\forall a \in A, \exists a^{-1} \in A, \quad s.t. \quad a \cdot a^{-1} = a_0$
      • 旋转矩阵集合+矩阵乘法：构成群（旋转矩阵群：$SO(3)$(3维特殊正交群)）
      • 变换矩阵集合+矩阵乘法：构成群（变换矩阵群：$SE(3)$(3维特殊欧氏群)）

      • 5.3 李群（Lie Group）(位于矩阵空间)

        • 李群（$SO(3)$)：三维空间旋转矩阵的集合

        • 李群中的旋转矩阵有9个值，所以可以把$SO(3)$看作9维空间
          

          • 上图是SO(3)表面(流形)(surface (manifold))的一部分
          • 绿点：表示单位矩阵
          • 蓝点：绕$x$轴旋转了10度
          • 红点：绕$z$轴旋转了10度

        • 在$SO(3)$中，如何表示单位矩阵附近的R矩阵？
          

          $$R= \begin{bmatrix} 1 & b & c \\ -b & 1 & f \\ -c & -f & 1 \\ \end{bmatrix}$$
          • $b, c, f$：无穷小
          • 此矩阵有3个参数，因此在单位矩阵附近的矩阵看起来像一个三维空间
          • 此矩阵的来由：
            
          • 把R表示为在单位矩阵附近的形式：
            $$R= \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \\ \end{bmatrix} - f \begin{bmatrix} 0 & 0 & 0 \\ 0 & 0 & -1 \\ 0 & 1 & 0 \\ \end{bmatrix} + c \begin{bmatrix} 0 & 0 & 1 \\ 0 & 0 & 0 \\ -1 & 0 & 0 \\ \end{bmatrix} -b \begin{bmatrix} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 0 \\ \end{bmatrix}$$
          • 设$\alpha_1 = -f, \quad \alpha_2 = c, \quad \alpha_3 = -b$，则有：
            $$R = I + \alpha_1G_1 + \alpha_2G_2 + \alpha_3G_3 \quad(\alpha_1,\alpha_2,\alpha_3都无穷小 ) \\ G_1 = \begin{bmatrix} 0 & 0 & 0 \\ 0 & 0 & -1 \\ 0 & 1 & 0 \\ \end{bmatrix} \quad G_2 = \begin{bmatrix} 0 & 0 & 1 \\ 0 & 0 & 0 \\ -1 & 0 & 0 \\ \end{bmatrix} G_3 = \begin{bmatrix} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 0 \\ \end{bmatrix}$$

        • $G_1, G_2, G_3$：叫做单位矩阵附近矩阵的导数分量，即生成矩阵（Generator Matrices）。

        • 李群的性质：

          • 具有连续（光滑）性质的群
          • 既是群，也是流形
          • 直观上看，一个刚体能够连续地在空间中运动，所以$SO(3)$和$SE(3)$都是李群
          • $\color {blue}{局限性：李群只定义了良好的乘法，而没有加法，所以无法进行求极限、求导等操作}$，即无法进行优化

        5.4 正切空间和导数：李代数（Lie Algebra）(位于向量空间)

        • $\color {red}{正切空间：是一个向量空间，其原点在单位矩阵（identity matrix）的位置处}$
        • 向量空间+双线性运算 = 李代数 （Lie algebra）
        • 生成矩阵（Generator Matrices（$G_1、G_2、G_3$））：是此向量空间的基
        • 若$G_1, G_2, G_3$变为非无穷小，则它们创建了一个3维平面，此平面与$SO(3)$正切于单位向量，下图是一个二维表示：
          
          
          • 红色$R(t)$：表示流形表面上的路径
          • 橙色$dR/dt$：表示当它经过单位向量时，此路径对变量t的导数
          • 正切空间：可看作$SO(3)$在单位向量处可能的导数集合
        • 重点：若旋转矩阵R是变量$t$（如：弧度）的函数，则当$t$取一定的值时$R(t) = I$（如绿点处），在此点$dR/dt$必然位于正切空间上，且是生成矩阵（$G_1、G_2、G_3$）的线性组合：
          $$\frac {dR}{dt} \mid_{R=I} = \sum_i^3\alpha_i G_i$$

        • 举例说明：若变量t是绕x轴旋转的弧度值，则有：
          

          $$R = \begin{bmatrix} 1 & 0 & 0 \\ 0 & cos(t) & -sin(t) \\ 0 & sin(t) & cos(t) \\ \end{bmatrix}$$
          • $R$对$t$的导数为：
            $$\frac {dR}{dt} = \begin{bmatrix} 0 & 0 & 0 \\ 0 & -sin(t) & -cos(t) \\ 0 & cos(t) & -sin(t) \\ \end{bmatrix}$$
          • 当t=0时，R=I，则导数为：
            $$\frac {dR}{dt}|_{t=0} = \begin{bmatrix} 0 & 0 & 0 \\ 0 & 0 & -1 \\ 0 & 1 & 0 \\ \end{bmatrix} = G_1$$
          • $G_1$：对应绕 x 轴的旋转
          • $G_2$：对应绕 y 轴的旋转
          • $G_3$：对应绕 z 轴的旋转

        • 李代数：与李群对应的一种结构，位于向量空间。

          • 记作：$so(3)、se(3)$
          • 李代数是李群单位矩阵处的正切空间
        • 李代数的定义：
          
          • $\color {blue}{向量空间+双线性运算（特殊：三维向量+叉乘运算：构成了李代数）}$
          • 组成：向量集合V + 数域F + 二元运算符[ , ] (叫做李括号，表示两个元素的差异)
        • 李代数的通用性质：
          
          • 封闭性：$\forall X, Y \in V, \quad [X, Y] \in V$
          • 双线性：$\forall X, Y， Z \in V, a, b \in F \; 则有： \\ [aX + bY, Z] = a[X, Z] + b[Y, Z] \quad [Z, aX+bY] = a[Z, X] + b[Z, Y]$
          • 自反性：$\forall X in V, \quad [X,X] = 0$
          • 雅可比恒等式：$\forall X, Y, Z in V, \quad [X,[Y,Z]] + [Y, [Z, X]] + [Z,[X,Y]] = 0$
          • 李括号具有反对称性（anit-symmetric）：$\forall X, Y in V, \quad [X,Y] = -[Y,X]$
        • so(3)的性质(旋转向量)：
          
          • $so(3) = \{r \in \mathbb R^3, S = [r]_{\times } \in \mathbb R^{3 \times 3}\} \\ S = [r]_{\times} =\begin{bmatrix} 0 & -r_3 & r_2 \\ r_3 & 0 & -r_1 \\ -r_2 & r_1 & 0 \end{bmatrix}$
        • se(3)的性质（刚体变换：旋转+平移向量）
          
          • $se(3) = \{e= \begin{bmatrix} t \\ r \end{bmatrix} \in \mathbb R^6, \; t \in \mathbb R^3, r \in so(3), \; [e]_{\times} = \begin{bmatrix} [r]_{\times} & t \\ 0^T & 0 \end{bmatrix} \in \mathbb R^{4 \times 4} \}$
          • se(3)组成：3个平移分量+3个旋转分量
          • 其旋转分量与so(3)相同
          • 平移是普通的向量，但不是SE(3)上的平移分量
          • $[e]_{\times}$不是反对称矩阵，只是保留了相同的记法
        • 把李代数理解为向量形式或矩阵形式都可以，但向量形式更加自然些

        5.5 指数映射

        • 设$A$是生成矩阵（导数分量）的线性组合(很明显，$A$为反对称矩阵)：
          $$A = \sum_{i=0}^3 \alpha_i G_i$$
        • $e^x$的泰勒展开式：
          $$e^x = 1 + x + \frac {1}{2}x^2 + \frac {1}{6}x^3 + \cdot \cdot \cdot + \frac {1}{n!}x^n$$
        • $e^A$泰勒展开式：
          $$M= e^A = I + A + \frac {1}{2}A^2 + \frac {1}{6}A^3 + \cdot \cdot \cdot + \frac {1}{n!}A^n$$
          
          • M以这种方式计算，它必定是$SO(3)$的成员，即M为一个旋转矩阵
          • 任何SO(3)中的成员，都能以这种方式进行计算
        • 为什么$M=e^A$必定是$SO(3)$ 的成员？
          $$e^A = \lim_{n \to \infty} \left(I + \frac {1}{n}A \right)^n$$
          
          • 当n无穷大时，$\frac {1}{n}A$变为无穷小，因此$I + \frac {1}{n}A$ 成为$SO(3)$的成员（
          • 因为：$R = I + \alpha_1G_1 + \alpha_2G_2 + \alpha_3G_3 \quad(\alpha_1,\alpha_2,\alpha_3都无穷小 )$
          • 次数越高，$e^A$越接近R，如下图所示：
            
          • 举例说明：
            
        • 结论：
          
          • 指数映射：建立了在单位矩阵处的导数（如速度）与群$（SO(3)或SE(3)）$成员间的关系，它可以不同的方程进行表示：
            $$A = \sum_i \alpha_i G_i (表示在单位矩阵处的速度) \\ \frac {dR}{dt} = AR \\ R(0) = I \\ R(t) = e^{tA} \\ R(1) = e^A$$
        • $\frac {dR}{dt} = AR$的由来：
          
          • 任意旋转矩阵满足：
            $$RR^T = I$$
          • 考虑R随着时间变化：
            $$R(t)R(t)^T = I$$
          • 两侧对时间求导：
            $$\frac {dR}{dt} R(t)^T + R(t) (\frac {dR}{dt})^T = 0$$
          • 整理得：
            $$\frac {dR}{dt} R(t)^T = - \left((\frac {dR}{dt})R(t)^T \right)^T$$
          • 可以看出这是一个反对称矩阵，记：
            $$\frac {dR}{dt} R(t)^T = A$$
          • 两侧右乘R(t)，得：
            $$\frac {dR}{dt} = AR$$
          • 可以看到，对R(t)求导后，左侧多出一个反对称矩阵A
            $$A = \begin{bmatrix} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \end{bmatrix}$$

        5.6 SO(3)的生成与叉乘(Corss Product

        • $v_1 \times v_2 = \begin{bmatrix} a \\ b \\ c \\ \end{bmatrix} \times \begin{bmatrix} d \\ e \\ f \\ \end{bmatrix} = [a]_{\times}b = \begin{bmatrix} 0 & -c & b \\ c & 0 & -a \\ -b & a & 0 \\ \end{bmatrix} \begin{bmatrix} d \\ e \\ f \\ \end{bmatrix} = \begin{bmatrix} bf-ce \\ cd-af \\ ae-bd \\ \end{bmatrix}$

        • $[a]_{\times} = \begin{bmatrix} 0 & -c & b \\ c & 0 & -a \\ -b & a & 0 \\ \end{bmatrix} = aG_1 + bG_2 +cG_3$

        • 这意味着$SO(3)$中旋转矩阵可表示为：$\color {red} {R=e^{[V]_{\times}}}$
        • 其参数为向量$V$中的元素，这3个参数在向量$V$与旋转矩阵$R$间建立了一个漂亮的关系
        • 基于此，旋转矩阵的直观理解：
          
          • 旋转轴：此旋转以向量$V$为旋转轴
          • 旋转角度：为向量$V$的长度
          • 旋转方向：沿着向量方向看，顺时针方向；沿着向量相反的方向看，逆时针方向；当$V=0$时，无旋转轴， 且旋转角度为0，即没有旋转
        • $V与R$的关系的直观解释：
          
          • $[V]_{\times}$：是在单位矩阵处的导数(即：$\frac {dR}{dt} |_{R=I} = A=\sum_i^3 \alpha_i G_i=[V]_{\times}$)，且在单位时间后产生了旋转$R$
          • $[V]_{\times}$：可被解释为产生一个速度向量域（velocity vector field），在空间的每一个点有一个速度向量（velocity vector）
          • 点P处的向量域（vector field)：被定义为：$\color {red} {[V]_{\times}P}$
          • 在旋转轴$V$上的点没有速度，因为$V \times V = 0$
          • 在旋转轴$V$外的点有速度，它与向量$V$的长度与$[V]_{\times}P$的长度成正比
          • 速度域如下图所示（由$[V]_{\times}$建立的速度域形成一个围绕V的圆形旋转）
            

        • 5.7 SE(3)的生成

          • SE(3)表示欧拉变换（刚体变换），在三给空间中形成了6维流形
          • 它有3个平移生成矩阵和3个旋转生成矩阵
            
          • $G_1, G_2, G_3$：分别为沿x,y,z方向的平移生成矩阵
          • $G_4, G_5, G_6$：分别为绕x,y,z轴旋转的旋转生成矩阵
          • 沿x方向的平移+绕z轴的旋转=沿y方向移动旋转中心
            

          5.8 李括号（Lie bracket）

          • 李代数：在群的单位矩阵处的正切空间中的向量
          • 李括号：李括号=李代数+双线性反对称运算
          • 指数映射：把正切空间->(映射到)群中的元素，这样可以抓住群的局部结构
          • $矩阵A、B$为正切空间中的矩阵（如为生成矩阵的线性组合），则有：
            $$e^Ae^B \neq e^Be^A \Rightarrow AB \neq BA$$
          • $AB$与$BA$的差异叫做Commutator(换位子)，即李括号， 表示为[A, B]：
            $$[A, B] = AB - BA$$
          • 李括号是反对称的：
            $$[A, B] = -[B, A]$$
          • 生成矩阵间的关系：
            $$[G_2, G_1] = -[G_1, G_2] = -G_3 \\ [G_3, G_1] = G_2 \\ [G_2, G_3] = G_1$$

          5.9 李代数求导与扰动模型

          • 在SLAM实际应用中，需要对位姿进行估计
          • 李群上只有乘法，没有加法，从而无从定义导数
          • 能否利用李代数上的加法，定义李群元素的导数？基本问题是：当在李代数中做加法时，是否等价于在李群上做乘法：
            $$e^{A_1} e^{A_2} = e^{A_1+A_2}$$ ：此等式当$A_1、A_2$为矩阵时，成立吗？
          • 根据BCH线性近似，可得：

          1）左乘雅可比：
          

          $$J_l = J = \frac {sin \theta}{\theta}I+(1- \frac {sin \theta}{\theta})aa^T + \frac {1-cos \theta}{\theta}[a]_{\times}$$
          $$J_l^{-1}=\frac{\theta}{2} cos \frac {\theta}{2}I + (1-\frac{\theta}{2} cos \frac {\theta}{2})aa^T-\frac{\theta}{2}[a]_{\times}$$

          2）右乘雅可比：
          

          $$J_r(v) = J_l(-v)$$
          3）在李群上左乘小量时，李代数上的加法相差左雅可比的逆：
          $$e^{[\bigtriangleup v]_{\times}} e^{[v]_{\times}} = e^{[v + J_l^{-1}(v) \bigtriangleup v]_{\times}}$$
          4）李代数上进行小量加法时， 相当于李群上左乘一个带左雅可比的量，或右乘一个带右雅可比的量
          $$e^{[v+\bigtriangleup v]_{\times}} = e^{[J_l \; \bigtriangleup v]_{\times}}e^{[v]_{\times}} = e^{[v]_{\times}} e^{[J_r \; \bigtriangleup v]_{\times}}$$
          - 通过BCH线性近似，可以定义李代数上的导数
          - $\color {red}{旋转后的点关于旋转的导数，不严谨地记为}$：
          $$\frac {\partial(Rp)}{\partial R}$$
          - 由于R没有加法，导数无法定义
          - 存在两种解决办法
          - R对应的李代数上加上小量，求相对于小量的变化率（ 导数模型）
          - $\color {red}{对R左乘或右乘一个小量，求相对于小量的李代数的变化率（扰动模型）}$
          - 导数模型
          - 按照定义可得：
          $$\frac {\partial(Rp)}{\partial R} = \frac {\partial (e^{[v]_{\times}}p)}{\partial v} \\ =\lim_{\bigtriangleup v \to 0} \frac {e^{[v+\bigtriangleup v]_{\times}}p - e^{[v]_{\times}}p}{\bigtriangleup v} \\ = - [Rp]_{\times}J_l$$
          - 结果中含有左乘雅可比，比较复杂
          - 扰动模型
          - 左乘小量，令其李代数为0
          $$\frac {\partial(Rp)}{\partial \bigtriangleup v} = \lim_{\bigtriangleup v \to 0} \frac {e^{[\bigtriangleup v]_{\times}} e^{[v]_{\times}}p - e^{[v]_{\times}}p}{\bigtriangleup v} \\ \approx \lim_{\bigtriangleup v \to 0} \frac {(1+[\bigtriangleup v]_{\times}) e^{[v]_{\times}}p - e^{[v]_{\times}}p}{\bigtriangleup v} \\ =\lim_{\bigtriangleup v \to 0} \frac {[\bigtriangleup v]_{\times}Rp}{\bigtriangleup v} = \lim_{\bigtriangleup v \to 0} \frac {-[Rp]_{\times}\bigtriangleup v}{\bigtriangleup v}\\ = - [Rp]_{\times}$$
          - 最终结果更为简洁，所以更实用

          5.10 总结

          • $\color {blue}{反对称矩阵\iff三维向量}$：

            • 由于$A^T = -A$，所以它主对角线元素必为0，而非对角线元素则只有三个自由度。
            • 可以把反对称矩阵对应到一个三维向量
            • 用途：它与叉积兼容，可以直接把矩阵与任意向量的乘积$Ab$写成 $a \times b$，即$Ab = a \times b$。

          • $\color {blue}{SO(3)：3维旋转矩阵}$

            • SO(3)：三维旋转群，其元素为旋转矩阵
            • 每对旋转矩阵求一次导数，只需左乘一个$\color {red}{反对称矩阵}$即可
            • 由于$\color {red}{反对称矩阵}$反映了R的导数性质，故称它在SO(3)的正切空间(tangent space)上

          • $\color {blue}{so(3)：3维旋转向量或 3维反对称矩阵}$

            • so(3)：是一个由三维向量(即$\color {red}{旋转向量}$)组成的集合，每个向量对应到一个反对称矩阵，可以表达旋转矩阵的导数
            • 反映了R的导数性质
            • 旋转轴：旋转向量的方向为旋转轴
            • 旋转弧度：旋转向量的长度为旋转的弧度
            • 描述了李群$SO(3)$中元素的局部性质
            • $[a, b]=AB-BA$ (A、B分别为向量a、b 对应的反对称矩阵)
            • $[A,B] = -[B, A]$
            • $aa^T = AA+||a||^2I_{3 \times 3}$

          • $\color {blue}{指数映射(so(3) \Rightarrow SO(3)) (罗德里格斯公式)}$

            • $R(t) = e^{tA} = e^{[V]_{\times}}，\quad A和[V]_{\times}都为反对称矩阵$
              $1) 设r为旋转向量，则旋转角度(弧度)：\theta = ||r|| \ $
              $2) 把r转换为单位向量：r = \frac{1}{\theta} r$
              

              $$3) e^{[V]_{\times}} =e^{\theta [r]_{\times}} = I + \theta [r]_{\times} + \frac{1}{2!}\theta^2 [r]_{\times}^2 +\cdot\cdot\cdot+ \frac{1}{n!}\theta^n [r]_{\times}^n = rr^T+\left(\theta - \frac {1}{3!} \theta^3 + \frac {1}{5!} \theta^5+\cdot\cdot\cdot\right)[r]_{\times} \\ -\left(1-\frac{1}{2!}\theta^2 + \frac{1}{4!}\theta^4 - \cdot\cdot\cdot \right)[r]_{\times}^2 \\ = [r]_{\times}^2 + I + sin(\theta) [r]_{\times} - cos(\theta) [r]_{\times}^2 \\ = (1-cos(\theta))[r]_{\times}^2 + I + sin(\theta)[r]_{\times} \\ =cos(\theta)I + (1-cos(\theta))rr^T + sin(\theta)[r]_{\times}$$

            • 4) 又根据罗德里格斯（Rodrigues）公式：$R= cos(\theta)I + (1-cos(\theta))rr^T + sin(\theta)[r]_{\times}$

            • (5)所以结论为：
              

              $$R=e^{\theta [r]_{\times}}=e^{[V]_{\times}}$$

            • 每个SO(3)中的元素，都可以找到so(3)一个或多个元素与之对应

          • $\color {blue}{so(3)与SO(3)的关系}$

            • 三维旋转表示： 除了采用旋转矩阵描述外，还可以用旋转向量来描述
            • 旋转向量的长度（模）表示绕轴逆时针旋转的角度（弧度）
            • $旋转向量 \Rightarrow 旋转矩阵$：可以通过$\color {red}{罗德里格斯（Rodrigues）（指数映射）}$变换进行转换
              $$1) 设r为旋转向量，则旋转角度(弧度)：\theta = ||r|| \\ 2) 把r转换为单位向量：r = \frac{1}{\theta} r \\ 3) 则旋转矩阵R为：R= cos(\theta)I + (1-cos(\theta))rr^T + sin(\theta)[r]_{\times} \\ 4) OpenCV实现函数：int\; cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian=0 );$$

            • $旋转矩阵 \Rightarrow 旋转向量$：
              1）弧度：
              

              $$\theta = arccos \left(\frac {tr(R) -1}{2} \right)$$
              2）旋转轴：
              $$Rn = n$$
              3）旋转向量：
              $$v= \theta n$$

            • 旋转向量可以视为旋转矩阵的导数，指导如何在旋转矩阵中进行微积分运算

            • 旋转矩阵：9个自由度，有正交性和行列式值为+1的约束
            • 旋转向量：3个自由度，没有约束
            • 旋转向量与旋转矩阵：只是表达方式的不同，但表达的东西可以是同一个
          • $\color {blue}{指数映射+对数映射关系图}$
            

          6. 雅可比矩阵与海森矩阵

          6.1 雅可比矩阵(Jacobian Matrix)

          • Reduced Jacobian matrix ：降阶雅可比矩阵

          • $\color{red}{雅可比矩阵}$：即一阶导数矩阵($\color{maroon}{向量对向量的一阶微分矩阵}$)

          • $\color{red}{重要性}$：它体现了一个可微方程在给定点的最优线性逼近。因此，雅可比矩阵类似于多元函数的导数。

          • $\color{red}{应用领域}$：用于向量微积分

          • $\color{red}{雅可比矩阵}$：是函数的一阶偏导数以一定方式排列成的矩阵

          • $\color{red}{雅可比矩阵相关的（函数与变量）}$：多个函数(m个）+多个变量(n个) $(映射：R^n \rightarrow R^m)$

          • $\color{red}{定义}$：假设某函数从$R^n$ 映射到 ${R} ^{m}$， 其雅可比矩阵是从 $R^n$ 到 ${R} ^{m}$ 的线性映射，其重要意义在于它表现了一个多变量$\color{blue}{向量函数}$的最佳线性逼近。因此，雅可比矩阵$\color{blue}{类似于单变量函数的导数}$。

          • 举例：假设$F:R^n→R^m$ 是一个从$n$维欧氏空间映射到到$m$维欧氏空间的函数。这个函数由$m$个实函数组成: $y_1(x_1, ..., x_n), ..., y_m(x_1, ...,x_n)$。这些函数的偏导数(如果存在)可以组成一个m行n列的矩阵($m \times n$)，这就是所谓的雅可比矩阵：
            

            $$\begin{bmatrix} \frac {\partial y_1}{\partial x_1} & \cdots & \frac {\partial y_1}{\partial x_n} \\ \vdots & \ddots & \vdots \\ \frac {\partial y_m}{\partial x_1} & \cdots & \frac {\partial y_m}{\partial x_n} \\ \end{bmatrix}$$

          • $\color{red}{符号表示}$：
            

            $$J_F(x_1, \cdots, x_n), \quad 或者\frac {\partial(y_1, \cdots, y_m}{\partial(x_1, \cdots, x_n}$$
            $1）F：为其映射的向量函数$
            $2）(x_1, \cdots, x_n)：为多变量$
            $3）J_F(x_1,\cdots,x_n)$：是一个线性映射（矩阵的本质就是线性变换），表示向量函数F在P点$(x_1, \cdots, x_n)$处的Jacobian矩阵(一阶导数矩阵)

          • $\color{red}{用途}$：

            • 如果$p$是${R} ^{n}$ 中的一点，$F$在$p$点可微分，根据高等微积分， $J_{F(p)}$ 是在这点($p$)的导数。
            • 在此情况下，$J_{F(p)}$这个线性映射即$F$ 在点$p$附近的最优线性逼近，也就是说当 $x$ 足够靠近点$p$时，我们有：
            • $F(x) \approx F(p) + J_{F(p)} \cdot (x-p)$

          6.2 海森矩阵（Hessian Matrix）

          • 海森矩阵用途：被应用于牛顿法解决的大规模优化问题。
          • 海森矩阵变量与函数：一个函数+多个变量 $(映射：R^n \rightarrow R)$。
          • 海森矩阵定义：（Hessian matrix 或 Hessian）是一个多变量实值函数的二阶偏导数组成的方块矩阵，假设有一实数函数$f(x_1, x_2, ..., x_n)$，如果$f$ 所有的二阶偏导数都存在，则海林矩阵H为：
            
          • 其$x=(x_1, x_2, ... , x_n)$
          • 当函数 $f$二阶连续可导时，Hessian矩阵H在临界点$x_0$上是一个 $n\times n$阶的对称矩阵。
            
            • 当H是正定矩阵时，临界点$x_0$是一个局部的极小值。
            • 当H是负定矩阵时，临界点$x_0$ 是一个局部的极大值。
            • H=0,需要更高阶的导数来帮助判断。
            • 在其余情况下，临界点$x_0$不是局部极值。

          6.3 雅可比矩阵与海森矩阵的比较

          矩阵属性	雅可比矩阵 (Jacobian Matrix)	海森矩阵 (Hessian Matrix)
          用途	1)关节型机器人：关节空间微小运动$d\theta$与手部作业空间微小位移dY的关系 2)非线性最小二乘
          yy
          方程	函数向量F由m个实值函数组成: $$\left\{ \begin{array}{c} y_1 = f_1(x_1, \cdots, x_n) \\ y_2 = f_2(x_1, \cdots, x_n) \\ \cdots \\ y_m = f_m(x_1, \cdots, x_n) \end{array} \right.$$ 也可记为：$Y=F(X)$ $Y= (y_1, \cdots, y_m)^T$ $F= (f_2, \cdots, f_m)^T$ $X= (x_1, \cdots, x_n)^T$	只有一个实值函数组成: $\begin{array}{c} y = f(x_1, \cdots, x_n) \end{array}$ $X = (x_1, \cdots, x_n)$
          实值函数个数	$\color {maroon} {m}$ (即m个n元素函数 )	$\color {maroon} {1}$ （即1个n元函数）
          微分方程组	$$\begin{bmatrix} dy_1 \\ \vdots \\ dy_m \end{bmatrix} = \begin{bmatrix} \frac {\partial f_1}{\partial x_1} & \cdots & \frac {\partial f_1}{\partial x_n} \\ \vdots & \ddots & \vdots \\ \frac {\partial f_m}{\partial x_1} & \cdots & \frac {\partial f_m}{\partial x_n} \\ \end{bmatrix} \begin{bmatrix} dx_1 \\ \vdots \\ dx_n \end{bmatrix}$$ $即：dY=J_F(X) dX \\ 方程组是一个关于微分的线性方程组, 其系数矩阵即雅可比矩阵$
          定义	$$J_F(X) = \begin{bmatrix} \frac {\partial f_1}{\partial x_1} & \cdots & \frac {\partial f_1}{\partial x_n} \\ \vdots & \ddots & \vdots \\ \frac {\partial f_m}{\partial x_1} & \cdots & \frac {\partial f_m}{\partial x_n} \\ \end{bmatrix}$$	$$\begin{bmatrix} \frac {\partial^2 f}{\partial x_1 \partial x_1} & \frac {\partial^2 f}{\partial x_1 \partial x_2} & \cdots & \frac {\partial^2 f}{\partial x_1 \partial x_n} \\ \frac {\partial^2 f}{\partial x_2 \partial x_1} & \frac {\partial^2 f}{\partial x_2 \partial x_2} & \cdots & \frac {\partial^2 f}{\partial x_2 \partial x_n} \\ \vdots & \vdots & \ddots & \vdots \\ \frac {\partial^2 f}{\partial x_n \partial x_1} & \frac {\partial^2 f}{\partial x_n \partial x_2} & \cdots & \frac {\partial^2 f}{\partial x_n \partial x_n} \\ \end{bmatrix}$$
          导数阶数	一阶偏导数	二阶偏导数
          矩阵类型	$m \times n$ (一般不为方阵)	$n \times n$ (方阵)
          映射	$$R^n \rightarrow {R} ^{m} \\ J为Y随X变化而变化的线性变换$$