视觉slam十四讲ch4李群和李代数（包含实践部分）

目录

0.前言：

1.李群和李代数

1.1群的定义

1.1.1李群SO3、SE3

1.2李代数的引出

1.3李代数的定义

1.3.1李代数so3

1.3.2李代数se3

2.指数与对数映射

2.1SO3上的指数映射

2.1.1指数映射的性质：

2.2SE3上的指数映射

3.李群、李代数的定义与相互的转换关系

4.李代数求导与扰动模型

4.1BCH公式与近似模型

4.1.1BCH公式

4.1.2BCH近似的意义

4.2SO3上的李代数求导：

4.2.1李代数求导（不重要，因为比较复杂故基本不用，用更简单的扰动模型）

4.2.2扰动模型（左乘）

4.3SE3上的李代数求导

5.结语 

6.实践：Sophus

6.1Sophus基本使用方法

6.2例子：评估轨迹的误差

---

0.前言：

相信很多读者和我一样，在看到这一节的时候不清楚引入李群、李代数的概念有什么用，在slam中，解决当前观测数据对应的相机最接近的位姿这样的问题，将其构建成求解最优的R,t的优化问题，使得误差最小化。

在解决这类问题过程中，旋转矩阵自身是带有约束的，作为优化变量时会引入额外的约束，使优化变得困难，通过引入李群-李代数间的转换关系，将其变为无约束的优化问题。

1.李群和李代数

1.1群的定义

群（Group）是一种集合加上一种运算的代数结构。我们把集合记作A，运算记作·，那么群可以记作G=(A,·)。满足以下条件：

（笔者通俗的理解为一堆矩阵在同一种运算后并满足上述四个条件就称这堆矩阵为群）

1.1.1李群SO3、SE3

旋转矩阵集合和矩阵乘法构成群SO3，变换矩阵和矩阵乘法也构成群SE3。

李群是指具有连续（光滑）性质的群。整数群z是离散的群无连续性质，不能称之为李群。

SO3、SE3在实数空间上是连续的，故都为李群。

1.2李代数的引出

任意旋转矩阵R，满足：

而R是相机的旋转，会随时间连续变化，即为时间的函数：R(t)，仍为旋转矩阵，有： 

对时间求导得：

整理得：

可看出˙R (t )R (t ) T为反对称矩阵。反对称矩阵可以找到一个与之对应的向量，向量也可以变成反对称矩阵：

因为˙R (t )R (t ) T是反对称矩阵，可以找到一个三维向量ϕ (t )∈ R 3与之对应：

等式两边右乘R（t），由于R为正交阵，有：

可以看到，每对旋转矩阵求一次数，只需左乘一个ϕ∧ (t )矩阵即可。为方便讨论，我们设t0 =0，并设此时旋转矩阵为R (0)=I 。按照导数定义，在0附近对函数进行一阶泰勒展开：

  我们看到反映了R的导数性质，故称它在SO(3)原点附近的正切空间上。同时在 =0 附近。设保持为常数。那么前式，有：

此式是关于R的微分方程，且知初始值为R（0）=I，解得：

旋转矩阵R与另一个反对称矩阵通过指数关系发生了关系。这说明当我们知道某个时刻的R时，存在一个向量ϕ，它们满足这个矩阵指数关系。我们后来可以看到，这个与R对应的ϕ正是对应SO3上的李代数so3

1.3李代数的定义

 每个李群都有与之对应的李代数。李代数描述了李群的局部性质。

一般的李代数的定义如下:李代数由一个集合V、一个数域F和一个元运算[,]组成。如果它们满足以下几条性质,则称(V,F,[,])为一个李代数，记作。满足以下条件

 其中二元运算被称为李括号。

1.3.1李代数so3

SO3对应的李代数是定义在上的向量，我们记作。根据前面的推导，每个都可以生成一个反对称矩阵：

在此定义下，两个向量中的李括号为：

故so3为：

so3是一个由三维向量组成的集合，每个向量对应一个反对称矩阵，可以用于表达旋转矩阵的导数。它与SO(3)的关系由指数映射给定：

1.3.2李代数se3

SE3对应的se3位于空间中：

我们把每个se3元素记作，它是一个六维向量。前三维为平移（但含义与变换矩阵中的平移不同，分析见后)，记作；后三维为旋转，记作，实质上是so3元素。同时，我们拓展了 ^ 符号的含义。在se3中，同样使用 ^ 符号，将一个六维向量转换成四维矩阵，但这里不再表示反对称。

我们仍使用 ^ 和∨符号指代“从向量到矩阵”和“从矩阵到向量”的关系，以保持和so3上的一致性。它们依旧是一一对应的。

可以简单地把se3理解成“由一个平移加上一个so3元素构成的向量”(尽管这里的还不直接是平移)。同样，李代数se3也有类似于so3的李括号：

2.指数与对数映射

2.1SO3上的指数映射

，矩阵指数exp(ϕ∧ )如何计算？这在李群和李代数中被称为指数映射。

对so3中的任意元素写成一个泰勒展开，结果仍然是一个矩阵。

但矩阵的无穷次幂没法直接计算。

所以由于是三维向量，我们可以定义它的模长和方向，分别记作和，于是有：ϕ =θa。这里a 是一 个长度为1的方向向量。首先，对于a∧ ，有以下两条性质：

利用这两条性质，可得：

这和罗德里格斯公式一样，这表明so3实际上就是由旋转向量组成的空间，指数映射即罗德里格斯公式。

通过它们，我们把so3中任意一个向量对应到一个位于SO3的旋转矩阵。

反之，SO3中的元素也可以通过对数映射对应到so3中

一般通过第三讲中利用迹的性质分别求解转角和转轴

2.1.1指数映射的性质：

指数映射是一个满射。意味着每个SO3的元素都可以找到一个so3的元素与之对应，但可能存在多个so3中的元素对应同一个SO3的元素。

至少对于旋转角θ，我们知道多转360°和没有转是一样的——它具有周期性。但是，如果我们把旋转角度固定在之间，那么李群和李代数元素是一一对应的。

2.2SE3上的指数映射

 se3上的指数映射形式为

ξ的指数映射左上角的R是我们熟知的SO3中的元素，与se3中的旋转部分ϕ对应。右上角的J可整理为雅可比矩阵：

该式与罗德里格斯公式有些相似，但不完全一样。我们看到，平移部分经过指数映射之后发生了一次以为系数矩阵的线性变换。
         同样地，虽然我们也可以类比推得对数映射，不过根据变换矩阵求so3上的对应向量也有更省事的方式：从左上角的R计算旋转向量，而右上角的满足：

3.李群、李代数的定义与相互的转换关系

（其实最后看懂这张表就行）

4.李代数求导与扰动模型

4.1BCH公式与近似模型

 使用李代数的一大动机是进行优化，而在优化过程中导数是非常必要的信息。下面来考虑一个问题。虽然我们已经清楚了和上的李群与李代数关系。但是，当在SO(3)中完成两个矩阵乘法时，李代数中so3上发生了什么改变呢？反过来说，当so3上做两个李代数的加法时，SO(3)上是否对应着两个矩阵的乘积？如果成立，相当于：
 

如果,为标量，那么显然该式成立；但此处我们计算的是矩阵的指数函数，而非标量的指数。换言之，我们在研究下式是否成立：

很遗憾，该式不成立。

4.1.1BCH公式

两个李代数指数映射乘积的完整形式，由BCH给出：

其中[ ]为李括号。BCH公式告诉我们，当处理两个矩阵指数之积时，它们会产生一些由李括号组成的余项。特别地，考虑SO3上的李代数，当如或为小量时，小量二次以上的项都可以被忽略。此时，BCH拥有线性近似表达：

  
 以第一个近似为例。该式告诉我们，当对个旋转矩阵R2（李代数中为ϕ 2）左乘一个微小旋转矩阵R1（李代数中为ϕ 1）时。

可以近似地看作，在原有的李代数上加上了一项Jl(ϕ2)−1ϕ 1，同理，第二个近似描述了右乘一个微小位移的情况。

本书以左乘为例

它的逆为：

右乘雅可比仅需要对自变量取负号：

这样，我们就可以谈论李群和李代数加法的关系了。

4.1.2BCH近似的意义

假定对某个旋转R，对应的李代数为ϕ。我们给它左乘一个微小旋转，记作∆R，对应的李代数为∆ϕ。那么，在李群上，得到的结果就是∆R·R ，，而在李代数上，根据BCH近似，为（），合并起来，可以写成：

反之，如果我们在李代数上进行加法，让一个加上，那么可以近似为李群上带左右雅可比矩阵的乘法：

这就为之后李代数上做微积分提供了理论基础。

同样地，对于SE3，也有类似的BCH近似:

4.2SO3上的李代数求导：

在SLAM中，要估计一个相机的位置和资态，该位置是由SO3旋转矩阵和SE3变换矩阵得到的，设某时刻相机位姿为T，观察到世界坐标p的点，产生一个观测数据z。那么有：

w为观测噪声，所以计算理想观测值与实际数据的误差：

假设有N个这样的观测，则对相机的位姿估计相当于找最优的T使整体误差最小。

而求解此问题则需要计算J关于T的导数。我们经常构建与位姿有关的函数，然后讨论该函数关于位姿的导数，以调整当前的估计值。

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

4.2.1李代数求导（不重要，因为比较复杂故基本不用，用更简单的扰动模型）

对于SO3，首先对于一个空间点p进行旋转，得到Rp，现在要计算旋转后的坐标对于旋转的导数，非正式的记为：

没有加法，导数无法定义，转而计算：

按照导数定义：

第2行的近似为BCH线性近似，第3行为泰勒展开舍去高阶项后的近似，第4行至第5行将反对称符号看作叉积，交换之后变号。

于是，我们推导出了旋转后的点相对于李代数的导数：

不过，由于这里仍然含有形式比较复杂的，我们不太希望计算它。下面要讲的扰动模型提供了更简单的导数计算方式。

4.2.2扰动模型（左乘）

另一种求导方式是对R进行一次扰动∆R这个扰动可以乘在左边也可以乘在右边，最后结果会有差异，以左扰动为例。设左扰动对应的李代数为。然后，对求导，即：

可见，相比于直接对李代数求导，省去了一个雅可比的计算。这使得扰动模型更为实用。请读者务必理解这里的求导运算，这在位姿估计中具有重要的意义。

4.3SE3上的李代数求导

不介绍求导，直接扰动模型

假设某空间点p经过一次变换T（对应的李代数为），得到Tp。现在，给T左乘一个扰动T=exp（），我们设扰动项的李代数为=[,]转置，那么：

我们把最后的结果定义成一个算符⊙，它把一个齐次坐标的空间点变换成一个4×6的矩阵。此式稍微需要解释的是矩阵求导方面的顺序，假设a,b,x,y都是列向量，有如下规则：

5.结语 

至此，我们介绍了李群和李代数的定义，介绍了李群SO3、SE3和李代数so3、se3，介绍了如何通过它们之间的指数/对数映射来表达位姿的转换，通过BCH对李代数进行扰动，进而求导，这些理论知识帮助我们完成后面的位姿优化，是误差减小。

6.实践：Sophus

6.1Sophus基本使用方法

安装Sophus

安装fmt依赖项：

git clone  https://github.com/fmtlib/fmt.git
cd fmt
mkdir build
cd build
cmake ..
make
sudo make install

下载Sophus

git clone https://github.com/strasdat/Sophus.git
cd Sophus/
mkdir build
cd build
cmake ..
make
sudo make install

编译可能会出问题，我的报错是找不到Sophus：：Sophus

修改一下CMakeLists.txt

target_link_libraries(useSophus ${Sophus_LIBRARIES} fmt)再编译就会成功了

code：

#include 
#include 
#include 
#include 
#include "sophus/se3.hpp"

using namespace std;
using namespace Eigen;

/// 本程序演示sophus的基本用法

int main(int argc, char **argv) {

  // 沿Z轴转90度的旋转矩阵
  Matrix3d R = AngleAxisd(M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix();
  // 或者四元数
  Quaterniond q(R);
  Sophus::SO3d SO3_R(R);              // Sophus::SO3d可以直接从旋转矩阵构造
  Sophus::SO3d SO3_q(q);              // 也可以通过四元数构造
  // 二者是等价的
  cout << "SO(3) from matrix:\n" << SO3_R.matrix() << endl;
  cout << "SO(3) from quaternion:\n" << SO3_q.matrix() << endl;
  cout << "they are equal" << endl;

  // 使用对数映射获得它的李代数
  Vector3d so3 = SO3_R.log();
  cout << "so3 = " << so3.transpose() << endl;
  // hat 为向量到反对称矩阵
  cout << "so3 hat=\n" << Sophus::SO3d::hat(so3) << endl;
  // 相对的，vee为反对称到向量
  cout << "so3 hat vee= " << Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose() << endl;

  // 增量扰动模型的更新
  Vector3d update_so3(1e-4, 0, 0); //假设更新量为这么多
  Sophus::SO3d SO3_updated = Sophus::SO3d::exp(update_so3) * SO3_R;
  cout << "SO3 updated = \n" << SO3_updated.matrix() << endl;

  cout << "*******************************" << endl;
  // 对SE(3)操作大同小异
  Vector3d t(1, 0, 0);           // 沿X轴平移1
  Sophus::SE3d SE3_Rt(R, t);           // 从R,t构造SE(3)
  Sophus::SE3d SE3_qt(q, t);            // 从q,t构造SE(3)
  cout << "SE3 from R,t= \n" << SE3_Rt.matrix() << endl;
  cout << "SE3 from q,t= \n" << SE3_qt.matrix() << endl;
  // 李代数se(3) 是一个六维向量，方便起见先typedef一下
  typedef Eigen::Matrix Vector6d;
  Vector6d se3 = SE3_Rt.log();
  cout << "se3 = " << se3.transpose() << endl;
  // 观察输出，会发现在Sophus中，se(3)的平移在前，旋转在后.
  // 同样的，有hat和vee两个算符
  cout << "se3 hat = \n" << 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_Rt;
  cout << "SE3 updated = " << endl << SE3_updated.matrix() << endl;

  return 0;
}

结果为：

---

6.2例子：评估轨迹的误差

在实际工程中，我们经常需要评估一个算法的估计轨迹与真实轨迹的差异来评价算法的精度。真实轨迹往往通过某些更高精度的系统获得，而估计轨迹则是由待评价的算法计算得到的。第3讲我们演示了如何显示存储在文件中的某条轨迹，本节我们考虑如何计算两条轨迹的误差。考虑一条估计轨迹T.esti,和真实轨迹T.gt，其中i= 1,…,N，那么我们可以定义一些误差指标来描述它们之间的差别。
误差指标有好多种，有绝对轨迹误差：

这实际上是每个位姿李代数的均方根误差。这种误差可以刻画两条轨迹的旋转和平移误差。

若仅考虑平移误差，则可定义绝对平移误差：

相对位姿误差：

也可只取平移部分：

code：

#include 
#include 
#include 
#include 
#include 

using namespace Sophus;
using namespace std;

string groundtruth_file = "/home/shikai/slamshijian/第四讲/example/groundtruth.txt";
string estimated_file = "/home/shikai/slamshijian/第四讲//example/estimated.txt";

typedef vector> TrajectoryType;

void DrawTrajectory(const TrajectoryType >, const TrajectoryType &esti);

TrajectoryType ReadTrajectory(const string &path);

int main(int argc, char **argv) {
  TrajectoryType groundtruth = ReadTrajectory(groundtruth_file);
  TrajectoryType estimated = ReadTrajectory(estimated_file);
  assert(!groundtruth.empty() && !estimated.empty());
  assert(groundtruth.size() == estimated.size());

  // compute rmse
  double rmse = 0;
  for (size_t i = 0; i < estimated.size(); i++) {
    Sophus::SE3d p1 = estimated[i], p2 = groundtruth[i];
    double error = (p2.inverse() * p1).log().norm();
    rmse += error * error;
  }
  rmse = rmse / double(estimated.size());
  rmse = sqrt(rmse);
  cout << "RMSE = " << rmse << endl;

  DrawTrajectory(groundtruth, estimated);
  return 0;
}

TrajectoryType ReadTrajectory(const string &path) {
  ifstream fin(path);
  TrajectoryType trajectory;
  if (!fin) {
    cerr << "trajectory " << path << " not found." << endl;
    return trajectory;
  }

  while (!fin.eof()) {
    double time, tx, ty, tz, qx, qy, qz, qw;
    fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
    Sophus::SE3d p1(Eigen::Quaterniond(qw, qx, qy, qz), Eigen::Vector3d(tx, ty, tz));
    trajectory.push_back(p1);
  }
  return trajectory;
}

void DrawTrajectory(const TrajectoryType >, const TrajectoryType &esti) {
  // create pangolin window and plot the trajectory
  pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
  glEnable(GL_DEPTH_TEST);
  glEnable(GL_BLEND);
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

  pangolin::OpenGlRenderState s_cam(
      pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
      pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
  );

  pangolin::View &d_cam = pangolin::CreateDisplay()
      .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
      .SetHandler(new pangolin::Handler3D(s_cam));


  while (pangolin::ShouldQuit() == false) {
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    d_cam.Activate(s_cam);
    glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

    glLineWidth(2);
    for (size_t i = 0; i < gt.size() - 1; i++) {
      glColor3f(0.0f, 0.0f, 1.0f);  // blue for ground truth
      glBegin(GL_LINES);
      auto p1 = gt[i], p2 = gt[i + 1];
      glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
      glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
      glEnd();
    }

    for (size_t i = 0; i < esti.size() - 1; i++) {
      glColor3f(1.0f, 0.0f, 0.0f);  // red for estimated
      glBegin(GL_LINES);
      auto p1 = esti[i], p2 = esti[i + 1];
      glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
      glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
      glEnd();
    }
    pangolin::FinishFrame();
    usleep(5000);   // sleep 5 ms
  }

}

结果为：