多传感器融合SLAM数学学习历程
多传感器融合SLAM数学学习历程
>>>流形和流形空间（姿态）
https://blog.csdn.net/professor_Xie/article/details/131911894
欧式空间和流形空间的区别和联系?
基本结构：欧式空间是我们熟悉的传统三维空间，其中的点由三个实数（x、y、z）表示，具有直角坐标系。在欧式空间中，可以进行常规的线性运算和加法操作。而流形空间是一种更一般的概念，它在局部上与欧式空间同胚，但在全局范围内可能不是直角坐标系。
维度：欧式空间的维度是固定的，例如三维欧式空间就有三个坐标轴（x、y、z）。而流形空间的维度可以是任意的，取决于流形的定义。例如，SO(3)流形是三维的，而SO(2)流形是二维的。
将姿态定义在流形上比定义在欧式空间上有什么好处？
连续性：姿态定义在流形空间中时，旋转操作的组合和插值都保持了流形的连续性。这意味着在流形空间上进行旋转操作时，不会出现突变或不连续性，从一个姿态平滑地过渡到另一个姿态。
不会出现奇异性：在流形空间上定义姿态可以避免一些奇异性问题。在欧式空间中，例如使用欧拉角时，存在万向锁问题，导致某些方向上的旋转变得不稳定。而在流形空间上，使用四元数或旋转矩阵等表示方式，可以避免这些奇异性问题，从而提高了姿态的稳定性。（欧式空间中姿态表示使用欧拉角）
避免过度参数化：姿态定义在流形空间上通常采用最小的参数化方式，例如四元数、旋转矩阵等。相比之下，在欧式空间中使用欧拉角时，可能会存在多种表示方式表示相同的旋转，导致过度参数化，增加了问题的复杂性。
保持结构特性：在流形空间上定义姿态，比如三维旋转群（SO(3)），可以保持旋转矩阵的正交性和行列式等于1的特性。这保证了旋转操作仍然是合法的旋转。
Example：
>>>李群李代数
视觉slam十四讲
https://zhuanlan.zhihu.com/p/395668394?utm_id=0
李群李代数的定义、相互间关系
李群的指数映射和对数映射
李代数求导、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;
}
#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;
}
>>>卡尔曼滤波
SO(3)流形上的ESKF，相比于四元数形式或欧拉角形式g=更简洁
https://blog.csdn.net/whatiscode/article/details/126242319?spm=1001.2014.3001.5501
SO(3)流形上的ESKF
https://zhuanlan.zhihu.com/p/441182819
>>>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
本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。 如若内容造成侵权/违法违规/事实不符，请联系我的编程经验分享网邮箱：veading@qq.com进行投诉反馈，一经查实，立即删除！