《第4讲 李群和李代数 》读书笔记

本文是《视觉SLAM十四讲》第4讲的个人读书笔记，为防止后期记忆遗忘写的。

本节目标

1. 总结李群与李代数的概念，知道为什么要引出它们。掌握 SO(3), SE(3) 与对应李代数的表示方式。
2. 回忆 BCH 公式以及近似的意义。总结在李代数上的扰动模型。
3. 使用 Sophus 对李代数进行运算。

这一节的知识点串联思路

在 SLAM 中,除了表示位姿之外,我们还要对它们进行估计和优化。因为在 SLAM 中位姿是未知的,而我们需要解决什么样的相机位姿最符合当前观测数据这样的问题。一种典型的方式是把它构建成一个优化问题,求解最优的 R, t,使得误差最小化。而估计、优化是需要进行迭代计算的。

但是，旋转矩阵自身是带有约束的(正交且行列式为 1)。它们作为优化变量时,会引入额外的约束,使优化变得困难。通过李群——李代数间的转换关系,我们希望把位姿估计变成无约束的优化问题,简化求解方式。所以引入了李群——李代数。

在优化和估计中，其实就是求最小误差，也就是求的最小值．那么，就一定需要涉及到求导　　来求最小值的问题.而BCH 公式以及近似是推理基础，最终通过对不引入额外的约束的李群——李代数进行求导或者扰动模型近似来完成优化问题所需要的求导部分.

4.1 李群李代数基础

三维旋转矩阵构成了特殊正交群 SO(3),而变换矩阵构成了特殊欧氏群 SE(3):

我们发现它们对加法是不封闭的。换句话说,对于任意两个旋转矩阵 R 1 , R 2 ,它们按照矩阵加法的定义,和不再是一个旋转矩阵。这两种矩阵并没有良好定义的加法,但是相对的,它们只有一种较好的运算:乘法。SO(3) 和 SE(3) 关于乘法是封闭的:

群(Group)是一种集合加上一种运算的代数结构。在群里，元素运算具有封闭性、结合律、幺元、逆的特点（参考离散数学）。变换矩阵和矩阵乘法也构成群(因此才能称它们为旋转矩阵群和变换矩阵群)。其他常见的群包括整数的加法 (Z, +),去掉 0 后的有理数的乘法(幺元为 1)(Q\0, ·) 等等。

李群是指具有连续(光滑)性质的群。像整数群 Z 那样离散的群没有连续性质,所以不是李群。而 SO(n) 和 SE(n),它们在实数空间上是连续的。我们能够直观地想象一个刚体能够连续地在空间中运动,所以它们都是李群。

在对进行变换推导后，得到                          

每对旋转矩阵求一次导数,只需左乘一个 φ ∧ (t) 矩阵即可。当我们知道某个时刻的 R 时,存在一个向量 φ,它们满足这个矩阵指
数关系。那么给定某时刻的 R,我们就能求得一个 φ,它描述了 R 在局部的导数关系。φ 正是对应到 SO(3) 上的李代数 so(3);矩阵指数 exp(φ ∧ ) 如何计算?这正是李群与李代数间的指数/对数映射。

SO(3) 对应的李代数是定义在 R 3上的向量,我们记作 φ。每个 φ 都可以生成一个反对称矩阵: 就说 SO(3) 对应的李代数的元素是 3 维向量或者 3 维反对称矩阵,不加区别:

总结下：那么给定某时刻的 R,我们就能求得一个 φ,它描述了 R 在局部的导数关系。我通过对φ向量求出它对应的反对称矩阵φ ∧，通过便可以求出R（t）。

Se(3) 对应的李代数是定义在 R 6上的向量,该向量是由平移和旋转两部分向量组成。

我们拓展了样使用∧符号的含义，在 se(3) 中,∧符号,将一个六维向量转换成四维矩阵,但这里不再表示反对称。我们仍使用∧和∨符号来指代“从向量到矩阵”和“从矩阵到向量”的关系,以保持和 so(3) 上的一致性。                                                                                                                          。

4.2 指数与对数映射

现在来考虑第二个问题:exp(φ ∧ ) 是如何计算的?它是一个矩阵的指数,在李群和李代数中,称为指数映射(Exponential Map)。泰勒展开：

由于 φ 是三维向量,我们可以定义它的模长和它的方向,分别记作 θ 和 a,于是有 φ = θa。这里 a 是一个长度为 1 的方向向量。

类比SO(3),总结SE(3)，得如下运算转换关系。

4.3 李代数求导与扰动模型

当 李代数so(3) 上做两个李代数的加法时,李群SO(3) 上不是对应着两个矩阵的乘积。反之亦然。也就是：     都不成立。由 Baker-Campbell-Hausdorff 公式(BCH 公式) 给出。

分析两因数的情况

   BCH 近似

不妨设某个时刻小萝卜的位姿为 T 。它观察到了一个世界坐标位于 p 的点,产生了一个观测数据 z。那么,由坐标变换关系知:误差：．假设一共有 N 个这样的路标点和观测,于是就有 N 个上式。那么,对小萝卜的位姿估计,相当于是寻找一个最优的 T ,使得整体误差最小化:

然而,SO(3), SE(3) 上并没有良好定义的加法,它们只是群。如果我们把 T 当成一个普通矩阵来处理优化,那就必须对它加以约束。而从李代数角度来说,由于李代数由向量组成,具有良好的加法运算。因此,使用李代数解决求导问题的思路分为两种:

1. 用李代数表示姿态,然后对根据李代数加法来对李代数求导。

2. 对李群左乘或右乘微小扰动,然后对该扰动求导,称为左扰动和右扰动模型。

李代数求导（具体求导过程详见书本）结果：　不过,由于这里仍然含有形式比较复杂的 J l ,我们不太希望计算它。而下面要讲的扰动模型则提供了更简单的导数计算方式。

扰动模型(左乘)

扰动模型相比于直接对李代数求导,省去了一个雅可比 J l 的计算。这使得扰动模型更为实用。

SE(3) 上的李代数求导类似，得到

我们已经介绍了李群李代数上的微分运算。之后的章节中,我们将应用这些知识去解决实际问题。（所以，本节内容相当重要）

4.4 实践:Sophus

在第三讲中,我们看到 Eigen 提供了几何模块,但没有提供李代数的支持。一个较好的李代数库是 Strasdat 维护的 Sophus 库 。它是直接在 Eigen 基础上开发的,我们不需要要安装额外的依赖库。

#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "sophus/so3.h"
#include "sophus/se3.h"using namespace std;
using namespace Sophus;
using namespace Eigen;int main( int argc, char** argv )
{
/****************初始化操作　
****************///沿Z轴转90度的旋转矩阵//旋转矩阵Ｒ　Matrix3d 实质上是 Matrix<double, 3, 3>,　Quaterniond是四元数类//旋转向量使用 AngleAxis(角度*向量）, 它底层不直接是Matrix，但运算可以当作矩阵Matrix3d R = AngleAxisd(M_PI/2, Vector3d(0,0,1)).toRotationMatrix();Quaterniond q(R);/********************************************旋转矩阵\旋转向量\四元数--->SO(3)
**********************************************/SO3 SO3_R(R);               // SO(3)可以直接从旋转矩阵构造SO3 SO3_v( 0, 0, M_PI/2 );  // 亦可从旋转向量构造SO3 SO3_q( q );　　　　　　　　　　　　　　// 或者四元数//     // 上述表达方式都是等价的．输出SO(3)时，以so(3)形式输出cout<<"SO(3) from matrix: "<<SO3_R<<endl;cout<<"SO(3) from vector: "<<SO3_v<<endl;cout<<"SO(3) from quaternion :"<<SO3_q<<endl;/********************************************SO(3)--->标准旋转向量
**********************************************/// 使用对数映射获得它的李代数,Vector3d 实质上是 Matrix<double, 3, 1>Vector3d so3 = SO3_R.log();cout<<"so3 = "<<so3.transpose()<<endl;
/********************************************SO(3)(向量)<--->反对称矩阵
**********************************************/// hat 为向量到反对称矩阵cout<<"so3 hat=\n"<<SO3::hat(so3)<<endl;// 相对的，vee为反对称到向量cout<<"so3 hat vee= "<<SO3::vee( SO3::hat(so3) ).transpose()<<endl; // transpose纯粹是为了输出美观一些/********************************************增量扰动模型的更新(左乘法扰动）
**********************************************/Vector3d update_so3(1e-4, 0, 0); //假设更新量为这么多SO3 SO3_updated = SO3::exp(update_so3)*SO3_R;cout<<"SO3 updated = "<<SO3_updated<<endl;cout<<"************我是分割线*************"<<endl;// 对SE(3)操作大同小异/********************************************
旋转矩阵,平移向量（或者四元数\平移向量）--->SE(3)
**********************************************/Vector3d t(1,0,0);          // 沿X轴平移1SE3 SE3_Rt(R, t);           // 从R,t构造SE(3)SE3 SE3_qt(q,t);            // 从q,t构造SE(3)cout<<"SE3 from R,t= "<<endl<<SE3_Rt<<endl;cout<<"SE3 from q,t= "<<endl<<SE3_qt<<endl;
/********************************************se(3)--->标准向量表示
**********************************************/// 李代数se(3) 是一个六维向量，方便起见先typedef一下typedef Matrix<double,6,1> Vector6d;Vector6d se3 = SE3_Rt.log();cout<<"se3 = "<<se3.transpose()<<endl;// 观察输出，会发现在Sophus中，se(3)的平移在前，旋转在后./********************************************se(3)(６维向量)<--->反对称矩阵（类比的说法）
**********************************************/// 同样的，有hat和vee两个算符cout<<"se3 hat = "<<endl<<SE3::hat(se3)<<endl;cout<<"se3 hat vee = "<<SE3::vee( SE3::hat(se3) ).transpose()<<endl;/********************************************增量扰动模型的更新(左乘法扰动）
**********************************************/// 最后，演示一下更新Vector6d update_se3; //更新量update_se3.setZero();update_se3(0,0) = 1e-4d;SE3 SE3_updated = SE3::exp(update_se3)*SE3_Rt;cout<<"SE3 updated = "<<endl<<SE3_updated.matrix()<<endl;return 0;
}