曾经也曾立志

本文提出了FAST-LIVO2：一种快速、直接的LiDAR-惯性-视觉里程计框架，旨在实现精确且鲁棒的同步定位与建图（SLAM）任务，并在实时、机载机器人应用中展现出巨大潜力。FAST-LIVO2通过误差状态迭代卡尔曼滤波器（ESIKF）高效地融合了IMU、LiDAR和图像测量。为了解决LiDAR和图像测量之间的维度不匹配问题，我们在卡尔曼滤波器中使用了顺序更新策略。

主要在后端优化阶段引入IMU约束，而非在底层与视觉特征联合优化。这种设计在保证实时性和模块化的同时，牺牲了部分动态环境下的精度和鲁棒性。ORB-SLAM3 中的相机图像数据与IMU数据采用的是**松耦合（Loose Coupling）**的方式进行融合定位。ORB-SLAM3 虽然支持IMU，但其核心设计仍以视觉SLAM为主，IMU作为辅助传感器。在局部BA或全局BA中，将IMU预积分误差作为约束项，优化相机位姿和IMU参数（如零偏）。检测到回环时，IMU数据参与全局位姿图优化，减少累积误差。

是两种多传感器数据融合的核心策略，尤其在结合视觉、IMU、激光雷达等传感器时至关重要。在SLAM（同步定位与地图构建）系统中，

在ORB_SLAM3中实现保存和加载地图功能。加载和保存不能同时设置。

将接收到的雷达点云话题转换为pcl点云格式，处理点云后，再次转换为点云话题并发布。

Velodyne雷达旋转方向VLP16雷达旋转方向为顺时针；在Velodyne激光雷达的ROS驱动中，velodyne_pointcloud\rawdata.h中做了一次坐标变换，变换到了x轴指向雷达正前方，y轴指向左侧。

注：以上仅供个人学习使用，如有侵权，请联系！

注：以上仅供个人学习使用，如有侵权，请联系！

原理参见。

记录常用库提供的类、方法等。

从 P3P 的原理可以看出，为了求解 PnP，我们利用了三角形相似性质，求解投影点 a, b, c 在相机坐标系下的 3D 坐标，最后把问题转换成一个 3D 到 3D 的位姿估计问题。带有匹配信息的 3D−3D 位姿求解非常容易，所以这种思路是非常有效的。其他的一些方法，例如 EPnP，亦采用了这种思路。

PnP 问题有很多种求解方法，例如，用 3 对点估计位姿的 P3P、直接线性变换（DLT）、EPnP（Efficient PnP）、UPnP，等等。此外，还能用非线性优化的方式，构建最小二乘问题并迭代求解，也就是Bundle Adjustment。因此，在双目或RGB-D的视觉里程计中，可以直接使用PnP估计相机运动。PnP（Perspective-n-Point）是求解3D到2D点对运动的方法。它描述了当知道n个3D空间点及其投影位置时，如何估计相机的位姿。注：本文内容仅限于学习使用，如有侵权，请联系！

ICP 的求解也分为两种方式：利用线性代数的求解（主要是 SVD），以及利用非线性优化方式的求解（类似于 Bundle Adjustment）。

使用OpenCV 提供的 triangulation 函数进行三角化。

基础矩阵（Fundamental Matrix）F 、本质矩阵（Essential Matrix）E、单应矩阵（Homography）H。整个程序使用opencv提供的算法进行求解。2D−2D: 对极几何及代码示例。

有了匹配好的点对，接下来，要根据点对来估计相机的运动。简单介绍3种情况根据点对来估计相机运动所使用的方法。

ORB 特征亦由关键点和描述子两部分组成。它的关键点称为“Oriented FAST”，是一种改进的 FAST 角点。它的描述子称为 BRIEF（Binary Robust Independent Elementary Feature）。特征点由关键点（Key-point）和描述子（Descriptor）两部分组成。注：以上笔记仅供个人学习使用，如有侵权，请联系！演示如何提取 ORB 特征并进行匹配。

Xsens Mti 300如何修改数据的hz数值。可以去官网直接下载，或者。

图优化，是把优化问题表现成图（Graph）的一种方式。于是，对任意一个上述形式的非线性最小二乘问题，可以构建与之对应的一个图。可以简单地称它为图，也可以用概率图里的定义，称之为贝叶斯图或因子图。在这个程序中，我们从 g2o 派生出了用于曲线拟合的图优化顶点和边：CurveFittingVertex 和CurveFittingEdge，这实质上扩展了g2o的使用方式。定义了顶点和边之后，我们在 main 函数里声明了一个图模型，然后按照生成的噪声数据，往图模型中添加顶点和边，最后调用优化函数进行优化。

Google Ceres 是一个广泛使用的最小二乘问题求解库。在 Ceres 中，只需按照一定步骤定义待解的优化问题，然后交给求解器计算即可。

将演示如何手写高斯牛顿法。

程序主要实现：(1). 根据内参计算一对 RGB-D 图像对应的点云；(2). 根据各张图的相机位姿（也就是外参），把点云加起来，组成地图。

本程序实现去畸变部分的代码。尽管我们可以调用OpenCV的去畸变，但自己实现一遍有助于理解。

演示了如下几个操作：图像读取、显示、像素遍历、复制、赋值等。大部分的注解已写在代码里面。编译该程序时，需要在 CMakeLists.txt 中添加 OpenCV 的头文件，然后把程序链接到库文件上。同时，由于使用了 C++ 11 标准（如 nullptr 和 chrono），还需要设置一下编译器。

单目相机的成像过程。

Sophus 库支持SO(3) 和SE(3)，此外还含有二维运动 SO(2)，SE(2) 以及相似变换 Sim(3) 的内容。它是直接在 Eigen 基础上开发的，不需要安装额外的依赖库。2）当在CMakeLists中遇到ADD_SUBDIRECTORY时，则跳转到该关键字指定的路径下执行相应的CMakeLists。ADD_SUBDIRCTORY：这个指令用来向当前工程中添加存放源文件的子目录，并可以指定中间、目标二进制的存放位置。3）执行之后返回跳转位置的下一条指令，然后继续按顺序执行。

【代码】随手笔记——基于Pangolin库显示机器人运动轨迹。

Eigen库中的四元数Quaterniond相关问题，包括构造、及使用。顺序是(w,x,y,z)，w为实部，后三者为虚部。注：四元数使用之前需要归一化。

注：如果想要深入理解，“所有的变换都相对于最开始的坐标系用左乘”可以从点的操作（point operator）去理解；“所有变换都相对于新得到的坐标系用右乘”可以从坐标系变换（coordinate transformation）去理解。关于齐次变换矩阵的左乘与右乘问题，本质上是所有的变换都相对于最开始的坐标系，还是所有变换都相对于新得到的坐标系的问题。参见https://blog.youkuaiyun.com/jppdss/article/details/131686922。所有变换都相对于新得到的坐标系用右乘。

齐次变换矩阵可以用来表示一个坐标系，旋转矩阵代表的是坐标系{B}三个轴的单位矢量在坐标系{A}中的投影，平移部分代表的是坐标系{B}的原点在坐标系{A}中的坐标。就是坐标系{B}在坐标系{A}中的表示。注：可以认为原始点固定在另一个坐标系下，在点进行平移和旋转时，与之固定的坐标系也随之进行平移和旋转，这样可能更好理解point operator。齐次变换矩阵可以用来描述一个坐标系经过怎样的平移和旋转后变换为另一个坐标系。对坐标系{A}进行相应的平移和旋转后与坐标系{B}重合。内的点进行平移和旋转操作。

/ 对于仿射和射影变换，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可，略。// 旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）// 虽然称为3d，实质上是4＊4的矩阵。// Eigen/Geometry 模块提供了各种旋转和平移的表示。//用matrix()转换成矩阵。// 欧氏变换矩阵使用 Eigen::Isometry。// 也可以直接赋值。// 或者用旋转矩阵。// 欧拉角: 可以。

/ 例如 Vector3d 实质上是 Eigen::Matrix，即三维向量。// 同时，Eigen 通过 typedef 提供了许多内置类型，不过底层仍是Eigen::Matrix。// Matrix3d 实质上是 Eigen::Matrix// 如果不确定矩阵大小，可以使用动态大小的矩阵。// 四则运算就不演示了，直接用±*/即可。