多传感器融合SLAM数学学习历程

本文介绍: 多传感器融合SLAM数学学习历程。

多传感器融合SLAM数学学习历程

>>> 流形和流形空间（姿态）

https://blog.csdn.net/professor_Xie/article/details/131911894

fast-lio 带着问题看知识
在这里插入图片描述

欧式空间和流形空间的区别和联系?

基本结构：欧式空间是我们熟悉的传统三维空间，其中的点由三个实数（x、y、z）表示，具有直角坐标系。在欧式空间中，可以进行常规的线性运算和加法操作。而流形空间是一种更一般的概念，它在局部上与欧式空间同胚，但在全局范围内可能不是直角坐标系。

维度：欧式空间的维度是固定的，例如三维欧式空间就有三个坐标轴（x、y、z）。而流形空间的维度可以是任意的，取决于流形的定义。例如，SO(3)流形是三维的，而SO(2)流形是二维的。

将姿态定义在流形上比定义在欧式空间上有什么好处？

连续性：姿态定义在流形空间中时，旋转操作的组合和插值都保持了流形的连续性。这意味着在流形空间上进行旋转操作时，不会出现突变或不连续性，从一个姿态平滑地过渡到另一个姿态。

不会出现奇异性：在流形空间上定义姿态可以避免一些奇异性问题。在欧式空间中，例如使用欧拉角时，存在万向锁问题，导致某些方向上的旋转变得不稳定。而在流形空间上，使用四元数或旋转矩阵等表示方式，可以避免这些奇异性问题，从而提高了姿态的稳定性。（欧式空间中姿态表示使用欧拉角）

避免过度参数化：姿态定义在流形空间上通常采用最小的参数化方式，例如四元数、旋转矩阵等。相比之下，在欧式空间中使用欧拉角时，可能会存在多种表示方式表示相同的旋转，导致过度参数化，增加了问题的复杂性。

保持结构特性：在流形空间上定义姿态，比如三维旋转群（SO(3)），可以保持旋转矩阵的正交性和行列式等于1的特性。这保证了旋转操作仍然是合法的旋转。

李代数就是SO(3)流形在原点处同胚的欧式空间；

在这里插入图片描述
Example：

流形与李代数解释：

利用最小二乘原理，我们在做位姿估计的时候需要对目标函数做约束。如果直接估计R，R通常不满足特殊正交群。这种带有约束的优化问题很难解决，因此引入了李群的概念，旋转矩阵满足李群的性质。
李群是一种流形结构，他是连续光滑的，没有尖椎。这样的优势是容易在上面做微分和积分。旋转矩阵只对乘法封闭，在对旋转矩阵做微
分的时候通常需要考虑加法操作，李群的微分流形上不具备向量空间的运算。由于流形的连续光滑特性，在流形的局部的正切空间满
足向量空间的特性，这个局部的正切空间叫做李代数，这个正切空间和流形空间在局部是同胚的，流形上的每个点的切空间是唯一
的。由于这种切空间是局部的同构，我们需要以微小的变化对旋转矩阵做积分来获取估计矩阵，这种微小的变化导致我们引入扰动。
旋转矩阵并不满足结合律，因此扰动分为左扰动和右扰动。李群李代数的映射关系是通过指数和对数映射。矩阵的指数的乘法和指数
的矩阵相加并不相等（标量是相等的），因此引入BCH公式进行近似。在估计的时候，切空间会发生变换，尤其是迭代的时候。我们
定义⺓元处为全局的，当前的切空间为局部的，全局与局部的转化是通过伴随矩阵进行的。




–	–


–	–

>>> 李群李代数

视觉slam十四讲

https://zhuanlan.zhihu.com/p/395668394?utm_id=0
https://zhuanlan.zhihu.com/p/557532018

https://zhuanlan.zhihu.com/p/145898307

李群李代数的定义、相互间关系
李群的指数映射和对数映射
李代数求导、BCH近似、扰动模型
Sophus例程源码解析

在这里插入图片描述

在这里插入图片描述
******* SO3群加法不封闭，乘法封闭

由于在李群SO3 和SE3 上对于加法运算是不封闭的（两个旋转矩阵相加，不再是旋转矩阵！），因此如果需要在李群上进行后续的优化处理，需要加上不少约束的条件，比如旋转矩阵的正交性，行列式为1等，给优化过程增添了不少麻烦。
所以才引出后面的李代数
而李代数是由向量组成，具有良好的加法运算，因此可以使用李代数来解决目标函数对于位姿进行求导的问题。

在这里插入图片描述

群Group是一种集合加上一种运算的代数结构

特殊正交群是李群
在这里插入图片描述

指数与对数映射
在这里插入图片描述

李群李代数之间的关系，和旋转向量旋转矩阵之间的转换关系很类似，指数映射就是罗德里格斯公式，

旋转矩阵的导数可以由旋转向量来指定，指导着如何在旋转矩阵中进行微积分运算。在这里插入图片描述

李代数求导
李代数扰动
》》》》》
矩阵乘法变加法，变扰动

在这里插入图片描述

应用：
李代数求导

在这里插入图片描述
优化问题关键是求导，求导主要思路由两种：

用李代数来表示姿态，然后根据李代数的加法运算来对李代数进行求导。
对李群左乘或者右乘一个微小的扰动，然后对该扰动进行求导，称作左扰动和右扰动模型。

在这里插入图片描述

代码
code应用

// Sophus::SO3d可以直接从旋转矩阵构造

  // 沿Z轴转90度的旋转矩阵
  // 三维旋转向量可以用角轴来表示，利用Eigen库的AngleAxisd()角轴（旋转向量）函数绕着单位方向向量alpha = z轴（0，0，1）旋转模长为theta = 90° = pi/2，
  // 则phi = theta alpha 再调用toRotationMatrix()函数从角轴表示的旋转向量转换为旋转矩阵
  Matrix3d R = AngleAxisd(M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix();
  // 调用Eigen的四元数，使用旋转矩阵初始化四元数
  Quaterniond q(R);
  // SO(3) 特殊正交群
  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; //对R进行一次左扰动后的旋转矩阵
  cout << "*******************************" << endl;

/ 取李群特殊欧式群SE(3)上变换矩阵的对数映射，也就是得到六维向量（前三维为平移部分，后三维为旋转向量也就是so(3)的元素）的李代数se(3)
  Vector6d se3 = SE3_Rt.log();
  
在三维空间刚体运动中，旋转矩阵构成特殊正交群SO(3)，变换矩阵构成特殊欧式群SE(3)。


/ 对SE(3)操作大同小异
  Vector3d t(1, 0, 0);           // 沿X轴平移1
  //用旋转矩阵和平移向量初始化特殊欧式群上的变换矩阵T 
  Sophus::SE3d SE3_Rt(R, t);           // 从R,t构造SE(3)
  //用四元数和平移向量初始化特殊欧式群上的变换矩阵T
  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;
  // 取李群特殊欧式群SE(3)上变换矩阵的对数映射，也就是得到六维向量（前三维为平移部分，后三维为旋转向量也就是so(3)的元素）的李代数se(3)
  Vector6d se3 = SE3_Rt.log();
  cout << "se3 = " << se3.transpose() << endl;
  // 观察输出，会发现在Sophus中，se(3)的平移在前，旋转在后.
  // 同样的，有hat和vee两个算符
  // hat(): 取向量的反对称矩阵                               
  //                                                          0       -theta_3     theta_2    
  // theta=[theta_1, theta_2, theta_3]^T   hat{theta}=    theta_3        0       -theta_1
  //                                                      -theta_2     theta_1        0
  cout << "se3 hat = n" << Sophus::SE3d::hat(se3) << endl;
  // 取李代数反对称矩阵后，再用vee()函数从矩阵转换为向量，并转置为行向量
  cout << "se3 hat vee = " << Sophus::SE3d::vee(Sophus::SE3d::hat(se3)).transpose() << endl;

两条轨迹误差

首先需要准备数据，十四讲源码中已经给出了真实轨迹 groundtruth.txt 和估计轨迹 estimate.txt，下面读取两条轨迹数据后，通过Sophus计算轨迹间误差，然后显示在Pangolin窗口中。

轨迹误差转换为
计算每个位姿李代数的均方根误差（Root-Mean-Squared Error，RMSE），可以刻画两条估计的旋转和平移误差。

在这里插入图片描述

// 读取两个轨迹
TrajectoryType ReadTrajectory(const string &path) {
  ifstream fin(path); //打开轨迹数据文件
  TrajectoryType trajectory;
  if (!fin) {
    cerr << "trajectory " << path << " not found." << endl;
    return trajectory;
  }
  while (!fin.eof()) {
    //轨迹数据txt文件中每一行表示由时间戳、平移向量、旋转四元数构成的一个相机wi位姿
    double time, tx, ty, tz, qx, qy, qz, qw;
    fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
    //使用轨迹数据中的旋转四元数和平移向量来初始化一个特殊欧式群SE(3)
    Sophus::SE3d p1(Eigen::Quaterniond(qw, qx, qy, qz), Eigen::Vector3d(tx, ty, tz));
    //将使用李群表示的当前相机位姿存入轨迹变量中
    trajectory.push_back(p1);
  }
  return trajectory;
}


/定义一个特殊欧式群SE(3)的vector容器，并取个别名
typedef vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> TrajectoryType;
int main(int argc, char **argv) {
  //真实轨迹路径
  string groundtruth_file = "../../example/groundtruth.txt";
  //估计轨迹路径
  string estimated_file = "../../example/estimated.txt";
  //读取轨迹数据
  TrajectoryType groundtruth = ReadTrajectory(groundtruth_file);
  TrajectoryType estimated = ReadTrajectory(estimated_file);
  //检查读取的轨迹数据是否正常
  assert(!groundtruth.empty() && !estimated.empty());
  assert(groundtruth.size() == estimated.size());

  // 计算每个位姿李代数的均方根误差 RMSE (Root Mean Squared Error)，也就是绝对轨迹误差 ATE (Absolute Trajectory Error)
  double rmse = 0;
  for (size_t i = 0; i < estimated.size(); i++) {
    //获取轨迹中每个相机当前的估计位姿和真实位姿数据
    Sophus::SE3d p1 = estimated[i], p2 = groundtruth[i];
    //根据每个真实数据和估计数据之间的位姿李代数的均方根误差公式RMSE（也就是绝对轨迹误差计算公式ATE）
    //真实位姿的变换矩阵T的逆与估计位姿的变换矩阵相乘，取对数映射计算位姿李代数并归一化
    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;
}

李群 SO(3) SE(3)

旋转矩阵构建 Sophus::SO3d SO3® Sophus::SE3d SE3(R,t)
四元数构建 Sophus::SO3d SO3(q) Sophus::SO3d SO3(q,t)
输出 SO3.matrix() SE3.matrix()
对数映射 Vector3d so3=SO3.log() Vecotr6d se3=SE3.log()
指数映射 SO3d::exp(so3) SE3d::exp(se3)
向量到反对称矩阵 SO3d::hat(so3) SE3d::hat(se3)
反对称矩阵到向量 SO3d::vee(hat) SE3d::vee(hat)
在这里插入图片描述

#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轴平移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<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;
}

#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "sophus/so3.hpp"

using namespace std;
using namespace Eigen;

int main()
{
    //  定义旋转矩阵（绕Z轴旋转90°）
    Matrix3d R = AngleAxisd(M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix();
    //  定义四元数
    Quaterniond q(R);
    //  从旋转矩阵进行构建SO(3)
    Sophus::SO3<double> SO3_R(R);
    //  从四元数进行构建SO(3)
    Sophus::SO3d SO3_q(q);
    //  输出李群内容
    cout << "SO(3)n" << SO3_R.matrix() << endl;
    //  对数映射，获取李代数
    Vector3d so3 = SO3_R.log();
    cout << "so(3)n" << so3.transpose() << endl;
    //  从向量到反对称矩阵
    Matrix3d hat = Sophus::SO3d::hat(so3);
    cout << "so(3) hat n" << hat << endl;
    //  从反对称矩阵到向量
    cout << "so3 hat vee n" << Sophus::SO3d::vee(hat).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;
    
    return 0;
}

简单总结

总体脉络梳理

https://zhuanlan.zhihu.com/p/367721074
》》
》》》
》》》》
》》》》》》

SLAM中位姿变化，左乘模型 → BCH近似李代数加法

SLAM整体误差，求导变为扰动模型

在这里插入图片描述

|
|
|




–	–


–	–


–	–

>>> 卡尔曼滤波

KF、EKF、ESKF的区别与联系

https://blog.csdn.net/liu3612162/article/details/114634670

KF：

在这里插入图片描述

EKF：

扩展卡尔曼滤波（EKF，Extended Kalman Filter）
在这里插入图片描述

ESKF：

Error-State卡尔曼滤波（ESKF）

直接法滤波与间接法滤波
直接法滤波：模型系统方程直接描述系统状态，不存在转化过程。一般使用的KF与EKF都属于直接法滤波。
间接法滤波：模型系统方程描述系统误差，需要通过转换得到系统状态。

ESKF（Error-State Kalman Filter）是一种典型的间接法滤波，其预测和更新过程都是针对系统的误差状态，再将修正后误差状态修正系统状态
在这里插入图片描述

对于SLAM,对于LIO系统

我们以IMU做预测，雷达和点云地图匹配（匹配得到位置和姿态）做观测为例，来展开模型（系统模型如下图）。

在这种融合系统中，虽然我们最终想要的融合结果是位置、速度、姿态，
但是实际在kalman中作为状态量的是他们的误差值，这种以误差作为状态量的滤波方法较多ESKF(Error State Kalman Filter)。

之所以使用误差状态，是因为它有比直接使用位姿作为状态的方法（一般称这种为名义状态）更多的优势，具体这个优势是啥，这个说来话长，不在此展开了，先把融合系统搞明白再说。

在这里插入图片描述

ESKF(误差估计卡尔曼滤波)
ESKF是误差状态卡尔曼滤波（Error State Kalman Filter），KF系列算法可以适用于感知融合，定位，数据处理与关联等多种任务，而ESKF的初衷是为了完成定位这项任务的，这种考虑多状态的KF，在SLAM的惯导系统是比较多用的，在这里拉出来多谈一下，至于AKF（自适应卡尔曼滤波，Sage-Husa算法），IEKF（迭代扩展卡尔曼滤波）等，就不多说了。

学习KF系列算法前首先要弄明白系统的需求是什么，比如在SLAM中到底是用它来做数据融合，还是用它来做目标定位，甚至是其他的一些后端优化。

ESKF的工作基本原理：为了求解机器人/汽车的位姿，而IMU测量数据（包括加速度计，陀螺仪等硬件）带有大量噪声，直接使用IMU数据对时间积分，是会产生明显的漂移。因此，为了更好的获取结果，需要融合其他传感器数据，例如GPS，Lidar等，从而修正数据，得到收敛性更好的结果。ESKF的核心思想是把系统状态分为三类：

1: True State（Ground Truth）：真值，指系统实际的运行状态，一般我们会在系统中假定某种输入为真值。（这一条在非线性KF系列滤波通用）；
2: Nominal State：名义状态，指系统的变化情况或变化趋势。例如IMU的各轴向加速度对时间的积分可以认为是各轴向距离上的变化，一般在系统中我们认为它是非线性（这一条也是我们在非线性KF中的强关注点）；
3: Error State：误差状态，真值与名义状态的差值，这个值也是非线性变化的。例如在IMU、激光、毫米波的系统中，我们可以假定激光的数据为真值，用其他两种来做融合，结果与激光做RMSE，这就可以认为是一种误差状态。
那么我们可以将任意一个状态变化过程，分割为名义变化过程和误差项，即真值为名义变化过后的结果与误差值的叠加。由此给出ESKF的步骤如下：

对IMU数据进行积分，获得名义状态X，注意这个X并没有加入额外的考虑，所以必然引入了累计误差；
利用KF算法估计Error State，包括状态更新和测量更新，这个过程是考虑了噪声的，而且由于这个误差状态的方程式近似线性的，直接使用KF就可以；
利用Error State修正Nominal State，获取“真值”；
重置Error State，等待下一次更新；

在这里插入图片描述
总结
ESKF将状态建模为流型中的状态量（非随机变量）加上正切空间中的误差量（均值为零的高斯分布）。
通过广义加减法建立误差量的运动模型和观测模型。
对误差量使用KF/EKF算法进行滤波。
相对传统KF/EKF，在更新步骤后多出一个修正步骤。

https://github.com/infinity1096/eskf_localization

SO(3)流形上的ESKF，相比于四元数形式或欧拉角形式g=更简洁
[https://blog.csdn.net/whatiscode/article/details/126242319?spm=1001.2014.3001.5501]

(https://blog.csdn.net/whatiscode/article/details/126242319?spm=1001.2014.3001.5501)

SO(3)流形上的ESKF
https://zhuanlan.zhihu.com/p/441182819

eskf会用到李群李代数 BCH右乘
在这里插入图片描述

在这里插入图片描述

在Fast -lio LI-init 使用了ESIKF

https://blog.csdn.net/qq_42731705/article/details/129427615

在这里插入图片描述

|
|
|
|




–	–


–	–


–	–


–	–

>>> LIO系统

https://blog.csdn.net/whatiscode/article/details/126371351?spm=1001.2014.3001.5501

可参考LI-init标定算法，借鉴了fast-lio2
https://blog.csdn.net/whatiscode/article/details/126252173?spm=1001.2014.3001.5501

在这里插入图片描述

LI-INIT LIdar-imu-init
数学介绍，和fast lio有区别
但是可以学习公式
在这里插入图片描述

Fast lio

论文及公式精读blog
FAST-LIO论文精读及公式推导
https://zhuanlan.zhihu.com/p/617068992

FAST-LIO公式推导适合新手
https://blog.csdn.net/qq_42688495/article/details/126732388

符号

在这里插入图片描述

// 指数映射   李代数转李群 Exp 罗德里格斯公式
static Eigen::Matrix3d Exp(const Eigen::Vector3d& r){
    Eigen::Matrix3d expr;
    double theta = r.norm();
    if(theta < 1e-7){
        expr = Eigen::Matrix3d::Identity();
    }
    else{
        Eigen::Matrix3d skew = get_skew_symmetric(r / theta);
        expr = Eigen::Matrix3d::Identity() + sin(theta) * skew + (1 - cos(theta)) * skew * skew;
    }
    return expr;
}
// 对数映射
static Eigen::Vector3d Log(const Eigen::Matrix3d& R){
    double theta = (R.trace() > 3 - 1e-6) ? 0 : acos((R.trace() - 1) / 2);
    Eigen::Vector3d r(R(2,1) - R(1,2), R(0,2) - R(2,0), R(1,0) - R(0,1));
    return fabs(theta) < 0.001 ? (0.5 * r) : (0.5 * theta / sin(theta) * r);
}

在这里插入图片描述

系统模型状态方程

在这里插入图片描述

迭代更新

迭代卡尔曼滤波中的状态更新过程可以看做优化问题。
在这里插入图片描述

imu推导

https://blog.csdn.net/SunLHanC/article/details/126700795

实现李代数指数映射部分

src/LiDAR_IMU_Init/include/so3_math.h

src/FAST_LIO/include/so3_math.h

template<typename T, typename Ts>
Eigen::Matrix<T, 3, 3> Exp(const Eigen::Matrix<T, 3, 1> &ang_vel, const Ts &dt)
{
    T ang_vel_norm = ang_vel.norm();    // 三轴角速度的二范数，就是角速度的标量，表示旋转的有多快。
    Eigen::Matrix<T, 3, 3> Eye3 = Eigen::Matrix<T, 3, 3>::Identity(); // 得到三维单位阵

    if (ang_vel_norm > 0.0000001)       // 确实是在旋转的。
    {
        Eigen::Matrix<T, 3, 1> r_axis = ang_vel / ang_vel_norm; // 等价于normalized()得到一个模长为1的向量，这里是旋转轴的方向向量。
        Eigen::Matrix<T, 3, 3> K;   // 斜对称矩阵K

        K << SKEW_SYM_MATRX(r_axis);

        T r_ang = ang_vel_norm * dt;    // 旋转角

        /// Roderigous Tranformation. 推导过程：
        return Eye3 + std::sin(r_ang) * K + (1.0 - std::cos(r_ang)) * K * K;    // 罗德里格斯公式，将轴角转为旋转矩阵
        
    }
    else
    {
        return Eye3;
    }
}