多传感器融合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)//用四元数和平移向量初始化特殊欧式群上的变换矩阵TSophus::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        0cout << "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轴平移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;
}

#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);
}

在这里插入图片描述

误差卡尔曼滤波

名义值变量（8个、23维）–位置、姿态、旋转外参、平移外参、速度、加速度零偏、角速度零偏、重力

P阵：状态协方差矩阵（23*23维）

过程噪声维数Q（4个、12维）–加速度、角速度白噪声、加速度角速度零偏白噪声。斜对角阵

状态转移矩阵Fx（2423维），噪声转移矩阵Fw（2412维）。

R阵：测量噪声协方差矩阵（n*n维）一帧中有n个点云，斜对角阵

观测方程的雅可比矩阵H阵（n*12维）一帧中有n个点云，残差与位置、姿态、旋转外参、平移外参这四个优化变量有关，共12维。

计算卡尔曼滤波增益：如果量测维度小于状态维度，用传统公式（nn维），否则用新公式（1212维）

误差值会随着迭代次数增加而减小，当相邻两次增量之差小于一定阈值就认为是收敛，或者达到最大迭代次数也将停止迭代。用误差的后验去更新位姿并且更新P阵（状态协方差矩阵）

系统模型状态方程

在这里插入图片描述

迭代更新

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

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;   // 斜对称矩阵KK << 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;}
}