SLAM学习笔记-------------（四）李群与李代数

李群

引出李代数

李代数

指数与对数映射

4.3 李代数求导与扰动模型

4.4 实践：Sophus

4.5相似变换群与李代数

这一讲我们要解决一个问题：

什么样的相机位姿最符合当前观测数据？（也就是什么样的位姿误差最小（从而进一步转换为求导问题））

为啥就牵扯到求导了呢？本章会讲，不过很好理解，可以先了解下

假设机器人在空间内移动，某个时刻相机的位姿是T，它观察到一个在世界坐标系中的一个空间点 p，并在相机上产生了一个观测数据 z，

那么Z = Tp - w（w是噪声）。由于有噪声，所以我们 z 不能精确的表示 Z = Tp

那么理想的观测与实际数据的误差为 e = Z - Tp ，我们当然希望误差最小咯

所以，假设有n个点p与观测值z。我们希望寻找一个最佳的位姿T，使得整体误差最小化。（这个式子里面就T是变量，z和p都是固定数据）

求解函数最小值问题嘛，可不就用到求导了？所以，问题就转换为求目标函数 J 对于变换矩阵 T 的导数。

李群

群：是一种集合加上一种运算的代数结构。如G(A，+ ) （离散数学里面有讲）

群要求这个运算必须满足以下四个条件：

上一讲中我们学了

三维旋转矩阵构成了特数正交群，记作SO(3)
变换矩阵构成了特数欧氏群，记作SE(3)

这两个群都是对乘法封闭的群。

李群是指具有连续（光滑）性质的群。SO(3)和SE(3)都是李群。

（ps：这节你就记住一点：之前我们学的两个特殊的群，一个由R组成，一个由T组成，他们都是李群

引出李代数

上一讲我们学过，旋转矩阵是正交阵。那么，正交阵有个性质（ $AA^{T}=E$ E是单位阵）

所以，我们取旋转矩阵R，则有 $RR^{T}=I$ ( I 代表单位阵）

R是相机的旋转，那么我们把相机的旋转随时间的变化描述为R(t)。故得到了 $R(t)R(t)^{T}=I$ 。

在等式两边对时间 t 求导，得到（R上边的点代表导数，运用了乘法求导法则）

我们把这个式子做以下数学变换（移项，然后历用矩阵转置的性质 $(AB)^T=B^{T}A^{T}$ )，得到如下等式

这个式子说明了，左边那两个相乘是一个反对称矩阵（符合 $A^{T}=-A$ 的就是反对称矩阵）

上一讲我们学过，一个向量对应的反对称矩阵，我们把它记作 A^ 。那么现在我们已知这个反对称矩阵，也可以求出与其对应的向量。记作 $A^{\nu }$ (这上边不是v是一个倒尖）

所以，我们把 $\dot{R(t)}R(t)^{T}$ 这个反对称矩阵所对应的向量记作 $\o (t)$ 。那么，就有如下表示

再对两边同时右乘R(t)。就得到了R(t) 导数的表达式

这个式子很关键，式子左边的R是李群，后面我们会了解到右边的 $\o (t)$ 其实就是对应的李代数。

我们可以看到，对R(t) 每求一次导数，就会在R(t) 前边乘一个 $\o (t)^{\Lambda }$ (想一下，这是不是跟指数函数很像啊 x^5 求导 5x^4）

我们把这个表达式反解出来。

最终，我们就解出了R(t)。（实在搞不懂中间的推导，那就知道结论就好了）

到这，我们可以看到，我们的旋转矩阵和一个反对称矩阵之间建立了指数关系（exp代表指数关系）

下面就由此引出了两个问题

ps：这一小节说的云里雾里，最后推出来了一个结论：R和 $\o$ 建立了指数关系。貌似不知道他想说什么。

注意， R和T其实都是李群， $\o$ 其实就是对应的李代数。这两个空间就靠exp() 建立起了联系。至于这个指数关系具体是什么，后边就说啦

对于某个时刻的R(t)（李群空间），存在一个三维向量φ=（φ1，φ2，φ3）（李代数空间），用来描述R在t时刻的局部的导数

李代数

每一个李群都有与之对应的李代数。

李代数描述了李群的局部性质，准确的说，是李群单位元附近的正切空间。定义如下：

这就是李代数。而我们之前提到的 $\o$ 就是一种李代数。（ $\o$ 是反对称矩阵所对应的三维向量）

我们把 $\o$ 的反对称矩阵，也就是我们之前说的（）。把这个记作 $\Phi$

在这个李代数中，李括号运算为：（倒尖符号上面讲过，是已知反对称矩阵，求其对应的向量）

至此，我们给出了SO(3)对应的李代数so(3) （后边应该是罗马数字表示，我打不出来，用小写so表示）

那不用说了SE(3)也有其对应的李代数se(3)（罗马数字）

这里解释的比较绕（我个人觉得），但是并不难，多看几遍，表述的意思不难，就是复杂一些

现在我们知道了李代数的概念以及他的一些条件

指数与对数映射

既然李代数和李群之间是指数关系，那这个指数关系具体到底是啥呢

虽然我们还不知道指数关系具体是啥，但是可以用泰勒展开

展开后，这式子里，要求它的n次方，也不好求。所以呢，我们给出两条性质。如下（推导过程就不说了）

利用左边这两条性质，就可以把高阶的矩阵相乘化简。

利用这俩性质，再用泰勒展开，就化成了下面这样：（为什么有个 $\theta$ ？看上面那个图，他把 $\o$ 这个向量，转换成了角度和模长。三维向量 φ = θa，a是一个长度为1的方向向量）

再通过一番奇妙的化简（数学就是奇妙），我们得到了下面这个式子

更奇妙的是，这个化简结果和第三讲里的罗德里格斯公式一模一样。

那就有趣了。之前我们用罗德里格斯公式完成了旋转向量（旋转轴+旋转角）与旋转矩阵的转换。

而现在李群与李代数之间的指数映射化简出来还是罗德里格斯公式。

到此，我们就得到了结果。李群和李代数之间的指数关系就是用罗德里格斯公式描述的。

至此，我们完成了李代数到李群的转换

同理，李代数到李群可以，那么李群自然也可以转李代数。指数映射反过来，不就是对数映射嘛

补充一点：之前我们用罗德里格斯公式完成了旋转向量与旋转矩阵的转换。其实呢

李代数空间so(3)就是由旋转向量组成的的空间，其物体意义就是旋转向量。指数映射关系就是罗德里格斯公式，他们在数学上本质是一样的。

SO(3)解决了，SO(E)就不推导了。直接放结论吧（反正推导了也看不懂（是我看不懂Orz。。。）

ps：李代数se(3)中的元素记作 $\xi$ （已经忘了吧。。。

J 又是啥呢？直接放推导结果。（这个 J 蛮重要的，后边还要用到）

指数映射说完了，该反过来对数映射了吧？诶，它根本不用对数映射

到这，我们把李群李代数之间的转化关系讲清楚了，放一个总结图。

4.3 李代数求导与扰动模型

前面说了那么一大堆，都是铺垫，现在总算是到关键的了

（我觉得，看书或者学习计算机方面的东西，如果遇到数学推导太多太难的时候，有一个办法：先记住我们要解决什么问题？然后记住推导的结果，至于过程，那是数学家操心的事。数学家需要严格证明它是可行的，程序员只要它用起来不出错就行（我瞎说的，别信）

那我们要解决什么问题呢？

什么样的相机位姿最符合当前观测数据？（也就是什么样的位姿误差最小（从而进一步转换为求导问题）

看不明白的画，那就记得我们要求导就行了

但是呢，SO(3)和SE(3)都是对乘法封闭的群，他们没有加法，这就使得我们不好定义他们的导数。（导数的定义就是无限个项的加法，泰勒展开嘛）

巧的就是，他们都有对应的李代数啊，李代数是三维向量，三维向量对加法封闭~

所以，我们想，能不能通过他俩的李代数来定义他俩的导数呢？

所以，上边讲了那么一大堆，都是在先搞明白他俩李群与李代数之间的对应关系。关系清楚了，下面开始求导了。（这是我个人见解，不一定正确）

求导之前，我们需要再补充一个知识（到处都是知识盲区~）

BCH公式与近似形式

我们在SO(3)中对两个矩阵做乘法运算的时候，对应的李代数so(3)上是怎样变化的呢？

会是下面这样的加法嘛？（你可能会问，哪里有加法？我咋没看到？看后边，指数运算啊 e^3*e^2=e^(3+2) ）

对这个式子两边取对数，就得到了下面这个式子。所以，我们的工作就是验证下面这个式子成立吗？

直接说结论，不正确。BCH（Baker-Campbell-Hausdorff）公式给出了两个李代数乘积映射的完整形式

这公式很长，看着很复杂，所以我们下边给了一个近似表达。比较简洁一些（ $J_{l}$ 叫左乘雅可比， $J_{r}$ 叫右乘雅可比）

一共给了两个近似表达，后边写了情况 $\o$ 1 最小和 $\o$ 2 的情况，啥意思呢，我们要乘上去的是小量，所以其实就是左乘和右乘的意思

看你的需要，选择左乘还是右乘。（我们以左乘为例子，演示以下怎么算的（我红色箭头指的那个）

所以呢，有了这个东西，我们就完成了乘法的映射。再描述一遍方便理解。

对某个旋转 $R$ ，他就是个李群，它对应的李代数记作 $\o$ ，我们给他左乘一个微小的旋转，记作 $\Delta R$ ，这个 $\Delta R$ 对应的李代数就是 $\Delta \o$

在李群上的乘法（左乘）就是 $\Delta R$ * $R$ ，对应到李代数上就是加上一个东西：（也就是 $\Delta R$ * $R$ =

李群上的乘法就转换成了李代数上的加法了。那么反之，我们在李代数上面直接加法，对应的也就是李群上的乘法了

当然了，SO(3)搞明白了，对于SE(3)上面也有同样的近似

有了这些运算的铺垫，接下来可以谈求导了（终于开始求导了，真不容易啊~~~~

在实际情况中，我们经常会构建与位姿T有关的函数，然后讨论该函数关于T的导数（讨论导数干嘛？比如可以求最小值呀）

所以，我们有两种求导的思路：

用李代数表示位姿，然后根据李代数加法来对李代数求导。称为李代数的求导模型。
对李群进行左乘或者右乘微小的扰动，然后对该扰动求导。称为左扰动、右扰动模型

首先第一种：求导转换成李代数

直接上推导结论，记住推导结果就行了

这就是最后的结果。也就是李代数的求导模型。

这里面带了个 $J_{l}$ 左乘的雅可比，所以这个结果还是比较麻烦（所以不常用）

第二个方法，扰动模型。对李群左乘或者右乘微小的扰动。

这个由于比求导模型少了个雅可比，比较好算些，所以我们常用扰动模型

那同样的，SE(3)也有扰动模型咯

我直接放推导出来的最终结果

就是这个矩阵，我们把这个结果定义成一种看起来简洁的方式，就是右边那样子，用右上角的那个特数符号来表示这种运算。

4.4 实践：Sophus

（我麻了，现在看到代码感觉无比亲切，比数学公式好看一百倍

这个Sophus库呢其实就是Eigen的一个扩展，主要就是扩展了李群李代数的一些运算

代码：

#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
#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轴平移1Sophus::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<double, 6, 1> 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;
}

运行结果：

SO(3) from matrix:
2.22045e-16          -1           01 2.22045e-16           00           0           1
SO(3) from quaternion:
2.22045e-16          -1           01 2.22045e-16           00           0           1
they are equal
so3 =      0      0 1.5708
so3 hat=0 -1.5708       01.5708       0      -0-0       0       0
so3 hat vee=      0      0 1.5708
SO3 updated = 0          -1           01           0     -0.00010.0001 2.03288e-20           1
*******************************
SE3 from R,t=
2.22045e-16          -1           0           11 2.22045e-16           0           00           0           1           00           0           0           1
SE3 from q,t=
2.22045e-16          -1           0           11 2.22045e-16           0           00           0           1           00           0           0           1
se3 =  0.785398 -0.785398         0         0         0    1.5708
se3 hat = 0   -1.5708         0  0.7853981.5708         0        -0 -0.785398-0         0         0         00         0         0         0
se3 hat vee =  0.785398 -0.785398         0         0         0    1.5708
SE3 updated =
2.22045e-16          -1           0      1.00011 2.22045e-16           0           00           0           1           00           0           0           1
*** Finished ***

例子：评估轨迹的误差

我先贴源代码，随后抽空分析代码，加注释

代码：

#include <iostream>
#include <fstream>
#include <unistd.h>
#include <pangolin/pangolin.h>
#include <sophus/se3.hpp>using namespace Sophus;
using namespace std;string groundtruth_file = "./example/groundtruth.txt";
string estimated_file = "./example/estimated.txt";typedef vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> TrajectoryType;void DrawTrajectory(const TrajectoryType &gt, 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 rmsedouble 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 &gt, const TrajectoryType &esti) {// create pangolin window and plot the trajectorypangolin::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 truthglBegin(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 estimatedglBegin(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}}

运行结果：

4.5相似变换群与李代数

同理，带星号，先略过