多传感器融合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/557532018https://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 #include 简单总结
总体脉络梳理
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; // 斜对称矩阵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;
}
}
LI-init / lidar imu init
符号
imu处理
https://blog.csdn.net/SunLHanC/article/details/126700795
实现李代数指数映射部分