格物致知

但这里有个问题，本身RigidMask是需要图像、场景流、位姿做为输入的，所以DytanVO的主要思想是进行运动分割和位姿解算的联合优化，也就是说将RigidMask输出的优化后的光流分割结果馈送给PoseNet，PoseNet生成的位姿又反过来优化RigidMask的动态物体分割性能，论文里提到迭代3次以后可以收敛，注意光流网络只需要前向推理一次。总体结构如图2所示。具体来说，作者修改了现有的主干网络，以学习更强大的对象特征表示，并在主干网络中部署了两次查看和思考的机制，提升了实例分割的性能。

路径规划是自动驾驶和机器人导航中的一个关键环节，它可以根据对环境信息的掌握程度被划分为两种主要类型：全局路径规划和局部路径规划。这两种规划方式在信息获取、规划策略和应用场景上有所不同。全局路径规划：局部路径规划：在实际应用中，全局路径规划和局部路径规划往往需要结合使用。全局路径规划可以提供一个大致的路径方向，而局部路径规划则负责在接近目标的过程中，根据实时环境信息进行路径的细化和调整。这种组合使用的方法可以提高导航系统的鲁棒性和适应性。通过结合全局和局部算法，仓储机器人能够灵活应对各种环境需求，平衡路径规划

初始化best_score初始化为负无穷大，用来记录当前找到的最佳匹配分数。遍历平移和旋转参数使用三个嵌套的循环，分别遍历水平方向（j_x）、垂直方向（j_y）和旋转方向（j_θ）的参数空间，从而在各个可能的平移和旋转组合中搜索。计算匹配分数在每个平移和旋转组合下，计算一个匹配分数score，它是通过某个函数M_nearest对所有K个匹配点的距离求和得到的。更新最佳匹配如果当前计算的score比best_score更好，则更新best_score，并记录当前的匹配参数match。返回结果。

1. 激光SLAM分类**1. GMapping 系统架构**1.1 **粒子滤波器（Particle Filter）**1.2 **运动模型（Motion Model）**1.3 **传感器模型（Sensor Model）**1.4 **地图更新（Map Update）**1.5 **重采样（Resampling）**1.6 **闭环检测（Loop Closure）****2. GMapping 的特点**2.1 **基于粒子滤波器的 SLAM**2.2 **高效的 2D 建图**2.3 **实时性**

本文介绍了FAST-LIO2：一种快速、鲁棒的、通用的激光雷达惯性里程计框架，FASTLIO2以高效紧耦合的方式迭代卡尔曼滤波器为基础，具有两个关键的新颖之处，可实现快速、稳健和精确的激光雷达的建图和导航。第一该方法是直接将原始点云配准到地图（随后更新地图），而不提取特征，这样可以利用环境中的细微特征，从而提高准确性，这里不采用人工设计的特征提取模块，使其自然适应不同扫描模式的新兴激光雷达；第二个主要创新点是通过。

而ikd-Tree在point-wise和block-wise，通过对结点新加了deleted, treedeleted, pushdown，treesize, invalidnum属性，进而减小了插入，删除，检索，re-insert的时间复杂度，并达到增量更新的目的；并且能够通过设置的参数，检测到二叉树不平衡时，进行重建。，这样做的好处是原本公式中求逆中的矩阵是观测维度的，新公式求逆中的矩阵是状态维度的，需要求逆H的矩阵维度减小了(因为在实际情况中，激光特征点的数量维度要远大于状态量的维度)

提出了一种通过平滑和映射实现紧密耦合的激光雷达惯性里程计框架 LIO-SAM，该框架可实现高精度、实时的移动机器人轨迹估计和地图构建。LIO-SAM 在因子图上制定激光雷达惯性里程计，允许将来自不同来源的大量相对和绝对测量（包括回环）作为因子纳入系统。惯性测量单元 (IMU) 预积分估计的运动消除了点云的倾斜，并为激光雷达里程计优化产生了初始猜测。获得的激光雷达里程计解决方案用于估计 IMU 的偏差。为了确保实时高性能，我们将旧的激光雷达扫描边缘化以进行姿势优化，而不是将激光雷达扫描与全局地图匹配。

分枝定界算法（Branch and Bound）在激光雷达（LiDAR）匹配中是一种用于解决优化问题的方法，尤其是在回环检测和路径规划中。这种方法通过系统地探索所有可能的解决方案空间，并在探索过程中剪枝不具有最优性的分支，从而找到最优解。

总体而言，FAST-LIO2在计算效率上（例如，在大型室外环境中实现高达100 Hz的里程计和制图）、鲁棒性（例如，在旋转速度高达1000度/秒的复杂室内环境中可靠的位姿估计）、多功能性（即，适用于多线旋转和固态激光雷达、无人机和手持平台，以及基于Intel和ARM的处理器）方面表现出色，同时仍然实现了比现有方法更高的精度。FAST-LIO2 进一步优化了数据处理流程，简化了特征提取的步骤，使其能够更加稳定地运行在不同种类的LiDAR设备上，尤其适用于低成本的激光雷达设备，在保证实时性的同时提高了精度。

提出 FAST-LIO，一种计算效率高且稳健的 LiDAR-惯性里程计框架，采用紧耦合迭代卡尔曼滤波器。使用前向和后向传播来预测状态并补偿 LiDAR 扫描中的运动。此外证明并实现了一个等效公式，该公式可以大大降低卡尔曼增益计算的复杂度。FASTLIO 在无人机飞行实验中进行了测试，在具有大旋转速度的室内环境和室外环境中具有挑战性。在所有测试中，我们的方法都产生了精确、实时和可靠的导航结果。

3.1 Fast_lio 编译与运行3.2 Faster-LIO编译与运行3.3 Point-LIO编译与运行3.4 DLO编译与运行3.5 lego_loam编译与运行3.6 A-LOAM编译与运行3.7 LVI-SAM编译与运行。

当偏置更新后，需要重新计算后续IMU数据的预积分，以确保在后续帧中使用最新的偏置进行校正。通过重传播，可以更精确地估计后续帧的状态。在每一帧激光里程计数据到来后，结合IMU预积分量更新状态估计。IMU预积分计算不仅考虑位姿增量，还会根据IMU偏置更新其预积分结果。实现了激光里程计的状态估计，并结合IMU数据进行运动畸变校正。总体来说，代码中的因子图优化和IMU预积分都是高效的，适合实时系统使用。IMU预积分使用IMU的加速度和角速度数据来估计设备在帧间的。是IMU预积分的预测增量，是当前激光帧的位姿观测。

在激光雷达技术中，使用两次回波的原因是为了提高测量的准确性和环境适应性。当激光雷达发射的光束遇到物体时，会发生反射，这个反射回来的信号就是第一次回波。如果光束能够穿透某些物体（如雨雾），它可能会遇到更远的物体并产生第二次回波。通过比较两次回波，激光雷达可以区分并忽略那些不是来自目标物体的反射，比如雨滴或灰尘，从而获得更准确的测量结果。在恶劣天气条件下，如雨雾天气，第一次回波可能是由雨滴反射的，而不是实际的目标物体。这时，第二次回波（如果足够强烈且能够被区分出来）可以提供更准确的距离信息。

该函数是SLAM系统中的关键部分，负责保存关键帧、更新因子图，并使用GTSAM库进行状态估计和优化。

通过设置初始姿态、速度和偏置的先验因子，并将它们添加到优化问题中，可以确保在接收到第一个里程计数据时，系统有一个合理的起点来进行状态估计。函数中用于初始化系统的部分。这个初始化流程是IMU预积分和状态估计过程中非常关键的一步，它确保了系统在接收到新的里程计数据时能够快速且准确地进行状态估计。这个流程是状态估计和传感器融合中常见的实践，特别是在处理IMU数据时，定期重置优化图是保持系统性能和准确性的重要步骤。函数中的核心部分，它负责处理IMU数据，执行状态估计和优化，并在优化后重新传播IMU里程计预积分。

主要实现了激光雷达数据的去畸变、投影和提取的功能，并使用IMU和里程计数据来消除激光雷达点云在运动中的畸变误差。通过将三维点云投影到二维图像平面，便于后续的处理，并对投影后的点云进行提取和下采样，优化了点云的质量和计算效率。最终，这段代码为下游模块提供了处理后的点云数据，适用于激光雷达SLAM或地图构建的应用场景。缓存和处理点云数据：缓存传入的点云消息，并根据不同的激光雷达传感器类型转换点云格式，提取时间戳和其他必要的信息。IMU和里程计信息的去畸变。

这段代码实现了基于LiDAR（激光雷达）点云数据的特征提取，用于SLAM（Simultaneous Localization and Mapping）系统中的前端处理。特征提取的目的是从点云中识别出角点和平面点（面点），为后续的位姿估计和地图构建提供关键特征点。