第4章 李群李代数

一、概述

1. 李群和李代数的核心思想

   1. 可以理解为专门用于矩阵旋转的东西，符合封结幺逆法则；

   2. 李群可以理解为旋转矩阵，李代数可以理解为旋转向量；

   3. 李群是连续群，李代数可以表出李群的导数，所以李代数表示的是李群的局部性质；

   4. 进而我们可以理解为：旋转向量表达了旋转矩阵的局部（旋转发生那一瞬间的领域内）性质；

   5. 由拉格朗日中值定理可知：导数控制函数。李代数控制李群，(phi)控制(R)；^【1】

      也就是说想要估计出函数值，我们可以研究该函数的导数，用来描述某个点领域内性质。故而我们需要建立对李群的求导模型，通过分析导数的性质来估计出相机在这一时刻（领域内）的位姿。

      但是我们知道群是指只有一个运算的集合（我们选择矩阵乘法），所以李群不对加法封闭^【2】，但是我们知道李代数是建立在向量空间上的，支持加法运算。所以我们需要一种让李群映射到李代数的机制，然后通过对李代数求导，求出李群的导数。不过，对李代数求导后的结果非常复杂，所以我们需要寻找另外一种求导方式^【3】，这就是我们接下来所要介绍的内容。

      【注】

      ^【1】：某个名牌大学考研的复试题——你知道导数的作用是什么吗？

      ^【2】：李群也是一种群。甭跟我扯什么鳄鱼不是鱼、日本人不是人。

      ^【3】：对谁求导不重要，因为我们总可以通过这个导数控制相同的函数。

2. 李群的两种求导模型（都是映射到了李代数空间）

   1. BCH公式线性化（将李群的变化与李代数的变化联系起来）；

   2. 对李代数求导的求导模型；（复杂）

      1. 需要求出左右雅可比矩阵的逆；

   3. 对微扰动求导的扰动模型；（精简）

      1. 不需要求出左右雅可比矩阵的逆；

3. 这两种求导模型都是会有误差存在的

4. 李群和李代数的基础符号

   1. 特殊正交群(SO(3))，特殊欧式群(SE(3))；

   2. 特殊正交群上的李代数(mathfrak{so}(3))，这里我们具象化为三维(phi)向量或者反对称阵(widehat{phi})；

      

   3. 特殊欧式群上的李代数(mathfrak{se}(3))，这里我们具象化为六维(xi)向量或者四维方阵(widehat{xi})；

      

      ( ho)表示三维空间中的平移，(phi)表示三维空间中的旋转。

   4. 反对称矩阵与相应的三维向量：(wedge)和(vee)；

      

   5. 向量(overrightarrow{a})和(overrightarrow{omega})都表示旋转向量的单位方向向量，( heta)表示旋转角，(overrightarrow{phi})表示旋转向量，(R)表示旋转矩阵；

   6. 有时为了格式不显得过于复杂，会省略掉向量标识(overrightarrow{})；

5. Sophus库的使用

   1. (SO(3))和(mathfrak{so}(3))

      // 构造旋转向量、旋转矩阵、四元数
      Eigen::AngleAxisd RV = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1));
      Eigen::Matrix3d RM = RV.toRotationMatrix();
      Eigen::Quaterniond QD(RM);
      // 通过上述旋转表述构造李群
      Sophus::SO3d SO3_RM(RM);
      Sophus::SO3d SO3_QD(QD);
      // 输出SO(3)时，以so(3)形式输出
      cout << "SO(3) from matrix    :" << SO3_RM.log().transpose() << endl;
      cout << "SO(3) from quaternion:" << SO3_QD.log().transpose() << endl;
      
      // 使用对数映射获得它的李代数
      Eigen::Vector3d so3 = SO3_RM.log();
      cout << "so3 = " << so3.transpose() << endl;
      // hat为向量到反对称矩阵
      cout << "so3 hat=" << endl << Sophus::SO3d::hat(so3) << endl;
      // vee为反对称矩阵到向量
      cout << "so3 hat vee= " << Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose() << endl; 
      
      // 增量扰动模型的更新
      Eigen::Vector3d update_so3(1e-4, 0, 0);
      Sophus::SO3d SO3_updated = Sophus::SO3d::exp(update_so3) * SO3_RM;
      cout << "SO3 updated = " << endl << SO3_updated.matrix() << endl;
      

   2. (SE(3))和(mathfrak{se}(3))

      // 构造旋转向量、旋转矩阵、四元数
      Eigen::AngleAxisd RV = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1));
      Eigen::Matrix3d RM = RV.toRotationMatrix();
      Eigen::Quaterniond QD(RM);
      // 通过上述旋转表述构造李群
      Sophus::SO3d SO3_RM(RM);
      Sophus::SO3d SO3_QD(QD);
      // 对SE(3)操作大同小异
      Eigen::Vector3d t(1,0,0);           // 沿X轴平移1
      Sophus::SE3d SE3_RMt(RM, t);           // 从R,t构造SE(3)
      Sophus::SE3d SE3_QDt(QD, t);            // 从q,t构造SE(3)
      cout << "SE3 from RM,t = " << SE3_RMt.log().transpose() << endl;
      cout << "SE3 from QD,t = " << SE3_QDt.log().transpose() << endl;
      // 李代数se(3) 是一个六维向量，方便起见先typedef一下
      typedef Eigen::Matrix<double,6,1> Vector6d;
      Vector6d se3 = SE3_RMt.log();
      cout << "se3 = " << se3.transpose() << endl;
      // 观察输出，会发现在Sophus中，se(3)的平移在前，旋转在后.
      // 同样的，有hat和vee两个算符
      cout << "se3 hat = " << endl << Sophus::SE3d::hat(se3) << endl;
      cout << "se3 hat vee = " << Sophus::SE3d::vee(Sophus::SE3d::hat(se3)).transpose() << endl;
      
      // 最后，演示一下更新
      Vector6d update_se3; //更新量
      update_se3.setZero();
      update_se3(0,0) = 1e-4;
      Sophus::SE3d SE3_updated = Sophus::SE3d::exp(update_se3) * SE3_RMt;
      cout << "SE3 updated = " << endl << SE3_updated.matrix() << endl;
      

二、详述

1. 从物理角度引出

   

   假设有一个三维的向量(overrightarrow{p(0)})绕着旋转轴(overrightarrow{omega})旋转了( heta)角度（旋转所需时间为(t)）到达了(overrightarrow{p(t)})处，由小学二年级就学过的知识知道：(overrightarrow{速度}=overrightarrow{角速度} imesoverrightarrow{（旋转中心，旋转点）})。

   我们不妨设(overrightarrow{p(t)})领域处的速度为(overset{ullet}{overrightarrow{p(t)}})，故而有公式：(overset{ullet}{overrightarrow{p(t)}}=overrightarrow{omega} imesoverrightarrow{p(t)}=widehat{overrightarrow{omega}}cdotoverrightarrow{p(t)})^【1】。这其实是一个关于(overrightarrow{p(t)})的微分方程^【2】，并且当(overrightarrow{omega})是单位向量，即值为(1 mathbf{rad/s})时有( heta=t)，所以不难解出：

   [overrightarrow{p(t)}=e^{widehat{overrightarrow{omega}}cdot{t}}cdot{overrightarrow{p(0)}}，由此我们不难看出overrightarrow{p(0)}到overrightarrow{p(t)}的旋转矩阵：R(t)=e^{widehat{overrightarrow{omega}}cdot{t}}=e^{widehat{overrightarrow{omega}}cdot{ heta}}=e^{widehat{overrightarrow{phi}}}； ]

   所以我们便可的出结论：旋转向量和旋转矩阵存在指数映射关系，且多对一对应（旋转周期性）。

   为了统一化表述，我们称旋转矩阵为李群(SO(3))，旋转向量为(mathfrak{so}(3))，它们之间有指数映射关系。

   【注】

   ^【1】：如果这个时候你仍然以向量为参考，那可能有点儿不太好理解，但要是将视线移至坐标系身上的化，也就是说是坐标系在旋转，那就不难理解：旋转轴是(overrightarrow{omega})，旋转中心是原点。

   ^【2】：

   [egin{aligned} &overset{ullet}{x(t)}=acdot{x(t)}，且x(0)=x_0&Longrightarrow&quad x(t)=e^{at}cdot{x_0}； \\ &overset{ullet}{X(t)}=Acdot{X(t)}，且X(0)=X_0&Longrightarrow&quad X(t)=e^{At}cdot{X_0}； end{aligned} ]

2. 指数与对数映射

   1. 矩阵的泰勒展开

      [e^{widehat{overrightarrow{omega}}cdot{ heta}}=sum_{n=0}^{infty}{frac{1}{n!}(overrightarrow{omega}}{ heta})^n=sum_{n=0}^{infty}{frac{{ heta}^n}{n!}(overrightarrow{omega}})^n ]

   2. 反对称阵的性质

      [egin{cases} widehat{overrightarrow{omega}}cdot{widehat{overrightarrow{omega}}}=overrightarrow{omega}cdot{overrightarrow{omega}^T}-I \\ widehat{overrightarrow{omega}}cdot{widehat{overrightarrow{omega}}cdot{widehat{overrightarrow{omega}}}}=-{widehat{overrightarrow{omega}}} end{cases} ]

   3. 指数映射的推导

      1. (mathfrak{so}(3) Longrightarrow SO(3))的推导

         

      2. (mathfrak{se}(3) Longrightarrow SE(3))的推导

         

         如果你尝试这将上面的(J)泰勒展开，将会得到下面的公式（请务必有印象！）：

         

         而旋转矩阵(R)通过上述的(mathfrak{so}(3) Longrightarrow SO(3))已经给出。

   4. 对数映射的推导

      1. (SO(3)Longrightarrowmathfrak{so}(3))的推导

         

         只有伞兵才会这样推导，之前在《矩阵旋转》中就提到了如何从旋转矩阵到旋转向量，我们只需知道旋转矩阵的迹(mathbf{tr}(R))解出( heta)，并解出特征值为1的特征向量并归一化得到(omega)。

      2. (SE(3)Longrightarrowmathfrak{se}(3))的推导

         由上述(SO(3)Longrightarrowmathfrak{so}(3))我们可以通过旋转(R)求出旋转向量(phi)，而(overrightarrow{t}=J ho)得( ho=J^{-1}overrightarrow{t})；

3. 李群和李代数的对应关系

   

三、李代数的求导和扰动模型（左乘）

1. BCH公式和它的线性近似式

   1. BCH公式的引出

      在高等数学中，我们研究的大部分问题都是标量（也就是纯数值）的问题，所以一定满足下面恒等式

      [ln(e^Acdot{e^B})equiv{A+B}\ ln(e^widehat{a}cdot{e^widehat{b}})equiv{(a+b)^{wedge}} ]

      但是很不幸的是，我们在SLAM中研究的是向量（或者说矩阵），反正不是标量，所以不能满足上式。这个东西在向量空间中的计算很是复杂，但是这个世界上总有些神仙捣鼓这种玩意儿，比如Baker-Campbell-Hausdorff这哥仨就为这个东西的计算给出了BCH公式。

   2. BCH的完全展开

      [ln(e^Acdot{e^B})=A+B+frac{1}{2}[A，B]+frac{1}{12}[A，[A，B]]-frac{1}{12}[B，[A，B]]+cdots ]

      其中([，])表示李括号运算。也就是说，这天底下就没有干干净净的事，矩阵指数之积会产生一堆李括号余项。

   3. BCH线性近似式

      但凡事不能说得太绝对。虽说这BCH公式的完全展开很复杂，但是后面那一堆东西叫做余项啊！也就是说，当(overrightarrow{phi_1})或者(overrightarrow{phi_2})是小量的时候，它们的高阶就是高阶无穷小量了，可以被忽略的！这个时候BCH公式就变得非常‘和蔼可亲’了（近乎线性的表达）

      [ln(e^{widehat{phi_1}}cdot{e^{widehat{phi_2}}})^{vee}approx egin{cases} 左乘模型：J_l^{-1}(phi_2)cdot{phi_1}+phi_2，当phi_1为小量时； \\ 右乘模型：J_r^{-1}(phi_1)cdot{phi_2}+phi_1，当phi_2为小量时； end{cases} ]

      至此，我们将李群(SO(3))（旋转矩阵）和李代数(mathfrak{so}(3))（旋转向量）线性化了。

      可以理解为：旋转矩阵(R_2)左乘了一个微小旋转矩阵(R_1)，相当于旋转向量(phi_2)加上了经过左雅可比逆(J_l^{-1}(phi_2))旋转后的旋转向量(phi_1)。右乘微小矩阵(R_2)亦可模仿理解。

   4. BCH公式的意义

      1. 李群 (Longrightarrow) 李代数

         [egin{cases} 左乘模型：e^{widehat{Deltaphi}}cdot{e^{widehat{phi}}}=e^{【J_l^{-1}(phi)cdot{Deltaphi}+phi】^{wedge}} \\ 右乘模型：e^{widehat{phi}}cdot{e^{widehat{Deltaphi}}}=e^{【J_r^{-1}(phi)cdot{Deltaphi}+phi】^{wedge}} end{cases} ]

      2. 李代数 (Longrightarrow) 李群

         [e^{【phi+Deltaphi】^{wedge}}= egin{cases} 左乘模型：e^{【phi+J_l^{-1}(phi)cdot{J_l(phi){Deltaphi}}】^{wedge}} =e^{【J_l(phi){Deltaphi}】^{wedge}}cdot{e^{widehat{phi}}} \\ 右乘模型：e^{【phi+J_r^{-1}(phi)cdot{J_r(phi){Deltaphi}}】^{wedge}} =e^{widehat{phi}}cdot{e^{【J_r(phi){Deltaphi}】^{wedge}}} end{cases} ]

      3. 李代数解决李群的求导问题

         李群没有加法，所以很难用导数的一般定义去求导数，但是李代数允许加法^【1】。所以如果想要对李群求导数，我们可以运用指数映射法则和BCH公式转换成李代数，然后用定义区求导数（乘除变加减）。

         【注】^【1】：李群是群，不对加法封闭。李代数定义在向量空间，对加法封闭。

   5. 左右雅可比矩阵的计算

      

   6. 在(SE(3))上同样有线性化公式

      

      其中的左右雅可比矩阵表述十分复杂，并且我们以后不会用到它来计算，所以就将其视为一个常数符号。

2. 李代数(SO(3))的求导

   假设一个向量(overrightarrow{p})经过了旋转矩阵(R)的作用，旋转矩阵对应的李代数（旋转向量）为(phi)；

   1. 对李代数求导（求导模型）

      [公式 控制egin{aligned} &frac{part{(Rp)}}{part{phi}} = frac{part{(Rp)}}{part{phi}} = frac{part{(e^{widehat{phi}}cdot{p})}}{part{phi}} \\=&underset{Deltaphi ightarrow0}{lim} frac{(e^{(phi+Deltaphi)^{wedge}}-e^{phi^{wedge}})cdot{p}}{Deltaphi} ，（利用左乘模型得） \\=&underset{Deltaphi ightarrow0}{lim} frac{(e^{【J_l(phi){Deltaphi}】^{wedge}}cdot{e^{widehat{phi}}}-e^{phi^{wedge}})cdot{p}}{Deltaphi} ，（利用矩阵的等价无穷小替换） \\=&underset{Deltaphi ightarrow0}{lim} frac{【J_l(phi){Deltaphi}】^{wedge}cdot{e^{widehat{phi}}p}}{Deltaphi} ，（将反对陈矩阵wedge符号换成叉积 imes，再交换顺序） \\=& -【e^{widehat{phi}}p】^{wedge}J_l(phi) = -(Rp)^{wedge}J_l{(phi)} end{aligned} ]

   2. 对微扰动求导（扰动模型）

      假设这个时候对(R)有一个作用在左边的微小扰动(Delta R)，对应的李代数为(psi)。（左右干扰模型不同！！）

      [egin{aligned} &frac{part{(Rp)}}{part{psi}} = frac{part{(Rp)}}{part{psi}} = frac{part{(e^{widehat{phi}}cdot{p})}}{part{psi}} \\=&underset{Deltaphi ightarrow0}{lim} frac{(e^{psi^{wedge}}e^{phi^{wedge}}-e^{phi^{wedge}})cdot{p}}{psi} ，（利用矩阵的等价无穷小替换） \\=&underset{Deltaphi ightarrow0}{lim} frac{psi^{wedge}cdot{e^{phi^{wedge}}p}}{psi} ，（将反对陈矩阵wedge符号换成叉积 imes，再交换顺序） \\=& -【e^{widehat{phi}}p】^{wedge}=-(Rp)^{wedge} end{aligned} ]

3. 李代数(SE(3))的求导

   

   第2、3行是因为用矩阵的等价无穷小替换；

四、评估轨迹的误差

1. 向量的长度（或大小）

   1. 欧式距离

      在三维及三维以下的向量空间中，向量的长度的平方是坐标值的平方和。

   2. 欧式推广

      在n维坐标系中，我们也可以用这一思想表示向量的长度（或者称之为大小）。

      [overrightarrow{alpha}=[alpha_1，alpha_2，cdots，alpha_n]^T，则||overrightarrow{alpha}||_2^2=alpha_1^2+alpha_2^2+cdots+alpha_n^2 ]

2. 矩阵的左右乘法区别

3. 每个位姿李代数误差

   我们记第(i)时刻的真实轨迹为(T_{(GT，i)})、观测（估计）轨迹为(T_{(Esti，i)})、绝对轨迹误差(T_{(ATE，i)})。

   由于误差的作用使得真实轨迹变换到了估计轨迹，我们的关注点是坐标系的变换，所以有三者的转换公式：(T_{(Esti，i)}=T_{(GT，i)}cdot{T_{(ATE，i)}})，即有(T_{(ATE，i)}=T_{(GT，i)}^{-1}cdot{T_{(Esti，i)}})。

   得到了每个位姿的误差公式，那如何确定误差的大小呢？我们知道李群是很难估计大小的，所以我们可以借助其对应的李代数来确定大小，这就需要用到对数映射公式：(xi=【log(T)】^{vee})。

   所以每个位姿的李代数上的误差为：(【log(T_{(GT，i)}^{-1}cdot{T_{(Esti，i)}})】^{vee})，那么它的大小也就可以用二范数来计算。

   当然我们也可以计算第i时刻的邻域(Delta{t})时间内，真实轨迹（的变化量）与估计轨迹（的变化量）的相对误差：

   [【log（T_{(GT，i)}^{-1}cdot{T_{(GT，i+Delta{t})}}）^{-1}（T_{(Esti，i)}^{-1}cdot{T_{(Esti，i+Delta{t})}}）】^{vee} ]