7.5松耦合LIO系统
前文中介绍激光slam系统、惯性导航系统和组合导航的原理。接下来我们来回顾一下他们之间的组成与区别。
首先激光slam系统，是同步定位与地图构建，以激光雷达为核心，通过采集环境三维点云特征，实现无先验地图下的实时定位+环境建图，是无GPS场景下机器人自主导航的核心方案。主要是通过激光雷达获取环境三维点云（也可以选择IMU、轮速计来进行辅助传感），软件核心模块是前端点云配准帧间匹配、实现相邻点云的对齐（可以先找到最近邻（暴力最近邻、栅格和体素法、二分树与k-d树、四叉树和八叉树），之后进行拟合（直线拟合、平面拟合））进行基础点云处理，处理完之后进行点云匹配（使用点到点ICP、点到线ICP、似然场法的匹配方法），之后利用栅格地图的方法建立子地图，可使用关键帧或者是体素的方法将子地图拼接构建成局部地图，并使用回环检测来进行地图优化。最终输出自身位姿。
惯性导航系统 以IMU（惯性测量单元）为核心 ，通过惯性测量 + 航位推算 实现自主定位，不依赖任何外部环境特征 / GPS ，是纯自主、无外部依赖 的导航方式。主要是通过IMU获取姿态（陀螺仪测角速度）、位置（加速度计测得加速度），无法构建地图。主要是借助卫星导航、imu、和机器人自身位姿来得到机器人在世界坐标系中的位置，利用误差状态卡尔曼滤波器来实现组合导航。
组合导航是将激光slam系统和惯性导航系统相结合，激光雷达和IMU作为核心传感器，轮速里程计、GPS、视觉相机作为辅助传感器。基础模块是激光 SLAM 全套算法（配准 / 优化 / 回环 / 建图）+ 惯导航位推算算法，核心融合模块为
标定 / 同步模块 ：时间同步（PTP/NTP）、空间外参标定（手眼标定 / 离线标定）、IMU 内参标定（零偏 / 温漂补偿）；地图 / 导航模块：融合定位结果 + 建图，输出可直接用于机器人运动规划的位姿 / 地图。
在本篇中主要来学习松耦合的组合导航方法。
使用IMU来指导激光匹配的预测方向，可以看成是激光雷达与IMU进行耦合的系统，称为LIO系统或LINS系统。在松耦合系统中，仍然有一个状态估计器来计算车辆自身的状态。IMU和轮速为这个估计器提供惯性和速度方面的观测源，点云配对结果为这个系统提供位姿数据的观测源。 松耦合估计器中的卡尔曼滤波部分和点云配准部分相对来说是解耦的（不相关联的）。可以选择一个前面章节的里程计位姿作为观测源，在点云配准时也可以使用状态估计器的预测输出，作为初始位姿估计进行配准。但是由于观测源和点云结构受到干扰，反过来也会影响状态估计器。
松耦合对应的是紧耦合LIO系统，直接融合激光/IMU的原始数据，而非融合各自的定位结果，就是把点云的残差方程直接作为观测方程，相当于直接把点云匹配的误差公式作为约束，让优化器自己找最优位姿。这种做法相当于在滤波器或者优化算法中内置一个ICP或NDT，由于ICP和NDT要更新（地图上的点的位姿会更新），ICP和NDT会迭代更新他们的最近邻，相应滤波器也会进行迭代。
本章主要使用卡尔曼滤波配合增量式NDT里程计实现一个松耦合系统。
7.5.1坐标系说明
由于我们是将惯性导航和激光slam结合到一起，所以现在有三个坐标系：世界坐标系（W）、IMU坐标系（I）、雷达坐标系（L）。我们要将雷达数据和IMU数据构成松耦合估计器，IMU和雷达坐标之间存在一个转换关系。为了方便，我们将不同数据集的
记录在配置文件中，在每次启动时获得这个外参数。
在点云配准中，由于IMU的运动模型（例如基于误差ESKF的预测模型）都是在IMU坐标系下推导的比较繁琐，所以我们选择通过外参将点云转换到IMU坐标系下，整个系统是以IMU为中心来实现定位和建图效果的，地图也是在IMU坐标系下描述的。设雷达扫描到的某个点为,那么IMU坐标系下这个点为
7.5.2松耦合LIO系统的运动与预测方程
整个松耦合系统都在IMU坐标系中，状态变量（观测值）的运动方程为:
同时激光雷达的输出位姿，可直接视为对状态变量的观测，对R的观测与对GNSS的观测相似：
写成抽象的的形式，平移部分应为
7.5.3松耦合LIO系统的数据准备
松耦合LIO系统的代码实现主要分为三个部分：第一部分，需要将IMU数据与雷达数据同步。雷达通常使用10Hz的频率，而IMU通常使用更高的100Hz的频率。我们希望统一处理两个雷达数据之间的那10个IMU数据。第二部分，需要处理雷达的运动补偿，而运动补偿需要有雷达测量时间内的位姿数据来源，正好可以用ESKF对每个IMU数据的预测值。第三部分，应该从ESKF中拿到预测的位姿数据，交给里程计算法，再将里程计配准之后的位姿放入ESKF中。在本书中作者给出了一个数据转换类以支持运行本书的各种数据集。
第一步，统一数据格式，CloudConvert类负责将各种格式的点云转化为PCL格式的点云，该类会处理点云的单点时间参数、跳点比例等参数，实际得到的点云通常要比传感器输出的点云小很多，具体代码如下：
cpp
/**
* @file cloud_convert.h/cpp
* @brief 激光雷达点云预处理模块核心类
* @note 该模块是激光-惯导紧耦合SLAM系统的前置环节，负责多型号雷达的点云格式统一、原始数据清洗，为后续LO/LIO算法提供标准化输入
*/
/**
* @class CloudConvert
* @brief 多型号激光雷达点云预处理与格式转换类
* @details 核心功能：将不同厂商/型号的激光雷达原始点云（大疆Avia、Velodyne 32线、Ouster 64线）统一转换为标准化的FullCloud格式；
* 附加功能：支持点云跳点滤波、时间戳标准化、扫描线数配置；
* 设计关系：该类由MessageSync（消息同步类）持有，作为IMU与雷达消息同步后的首个处理环节，预处理后的点云会交付给LO/LIO算法模块
* @author （可补充作者信息）
* @date （可补充日期信息）
*/
class CloudConvert {
public:
// Eigen库内存对齐宏，确保Eigen矩阵/向量的内存布局符合SIMD优化要求，避免内存访问错误
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/**
* @enum LidarType
* @brief 支持的激光雷达型号枚举
* @details 枚举值与工业主流激光雷达型号一一对应，用于区分不同雷达的点云格式解析逻辑
*/
enum class LidarType {
AVIA = 1, ///< 大疆Livox Avia固态激光雷达（非重复扫描，小视场角，高刷新率）
VELO32, ///< Velodyne 32线机械旋转激光雷达（经典车载雷达，中远距离，360°视场）
OUST64, ///< Ouster 64线数字激光雷达（高分辨率，低噪声，工业/车载场景常用）
};
/**
* @brief 默认构造函数
* @details 初始化成员变量为默认值（默认雷达类型为AVIA，跳点为1，扫描线数为6）
*/
CloudConvert() = default;
/**
* @brief 默认析构函数
* @details 无动态分配内存，无需自定义析构逻辑
*/
~CloudConvert() = default;
/**
* @brief 处理大疆Livox Avia雷达的自定义点云消息
* @param[in] msg 原始Avia雷达消息（livox_ros_driver::CustomMsg类型，包含时间戳、点云坐标、反射率、线号等）
* @param[out] pcl_out 输出的标准化FullCloud格式点云（引用传递，避免拷贝）
* @note Avia雷达的消息格式为自定义类型，需单独解析时间戳和点云属性
*/
void Process(const livox_ros_driver::CustomMsg::ConstPtr &msg, FullCloudPtr &pcl_out);
/**
* @brief 处理标准ROS点云消息（适配Velodyne/Ouster雷达）
* @param[in] msg 原始ROS标准点云消息（sensor_msgs::PointCloud2类型）
* @param[out] pcl_out 输出的标准化FullCloud格式点云（引用传递，避免拷贝）
* @note 该接口为Velodyne 32线/Ouster 64线雷达的统一处理入口，内部会根据lidar_type_自动分发到对应处理器
*/
void Process(const sensor_msgs::PointCloud2::ConstPtr &msg, FullCloudPtr &pcl_out);
/**
* @brief 从YAML配置文件加载雷达预处理参数
* @param[in] yaml YAML配置文件的路径（如config/lidar_params.yaml）
* @details 加载的参数包括：雷达型号、跳点数量、扫描线数、时间戳缩放因子；
* 若配置文件中无对应参数，将使用成员变量的默认值
*/
void LoadFromYAML(const std::string &yaml);
private:
/**
* @brief 大疆Avia雷达点云解析与预处理具体实现
* @param[in] msg 原始Avia自定义消息
* @details 核心逻辑：1. 解析自定义消息中的每个点的x/y/z/反射率/时间戳/线号；
* 2. 根据point_filter_num_进行跳点滤波；
* 3. 标准化时间戳（转换为秒级）；
* 4. 填充到cloud_full_/cloud_out_中
*/
void AviaHandler(const livox_ros_driver::CustomMsg::ConstPtr &msg);
/**
* @brief Ouster 64线雷达点云解析与预处理具体实现
* @param[in] msg 原始ROS标准点云消息
* @details 核心逻辑：1. 将sensor_msgs::PointCloud2转换为PCL点云；
* 2. 解析每个点的x/y/z/反射率/时间戳/扫描线信息；
* 3. 跳点滤波+时间戳标准化；
* 4. 转换为FullCloud格式
*/
void Oust64Handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
/**
* @brief Velodyne 32线雷达点云解析与预处理具体实现
* @param[in] msg 原始ROS标准点云消息
* @details 核心逻辑与Oust64Handler一致，差异仅在于点云字段解析（如Velodyne的反射率范围、扫描线编号规则）
*/
void VelodyneHandler(const sensor_msgs::PointCloud2::ConstPtr &msg);
FullPointCloudType cloud_full_; ///< 临时存储原始解析后的完整点云（未滤波）
FullPointCloudType cloud_out_; ///< 临时存储滤波后的输出点云（最终赋值给pcl_out）
LidarType lidar_type_ = LidarType::AVIA; ///< 当前使用的雷达型号，默认值为大疆Avia
int point_filter_num_ = 1; ///< 点云跳点滤波参数，1表示无跳点（保留所有点），2表示每2个点保留1个（降采样）
int num_scans_ = 6; ///< 雷达扫描线数（仅对Avia有效，Avia默认6线），Velodyne/Ouster会自动解析线数
float time_scale_ = 1e-3; ///< 雷达点时间字段转秒的缩放因子，1e-3表示原始时间单位为毫秒（ms），乘以1e-3转为秒（s）
};第二步，将IMU数据与点云进行同步，该类接收ROS数据包中原始的IMU消息与激光雷达消息，通过Sync函数将它们组装成一个MeasureGroup，再传递给回调函数。后续的松耦合、紧耦合算法只需考虑如何处理MeasureGroup对象，不必再"操心"数据准备、同步的实现代码。代码如下：
cpp
/**
* @file message_sync.h/cpp
* @brief 激光雷达与IMU数据时间同步模块核心实现
* @note 该模块是激光-惯导紧耦合SLAM系统的核心前置模块，负责：
* 1. 接收不同类型雷达的原始点云消息，调用CloudConvert完成格式标准化；
* 2. 缓存IMU和雷达数据，通过时间戳对齐实现"雷达帧+对应时间段IMU数据"的同步；
* 3. 同步完成后通过回调函数将数据组交付给后续LO/LIO算法模块；
* 核心设计：采用缓冲队列+时间戳匹配的方式，解决IMU（高频）与雷达（低频）的时间异步问题
*/
/**
* @struct MeasureGroup
* @brief 激光-IMU同步后的数据组结构体
* @details 存储单帧雷达点云及其采集时间段内的所有IMU数据，是后续LO/LIO算法的输入单元；
* 设计目的：将异步的IMU和雷达数据封装为时间对齐的"数据快照"，保证算法处理时的时间一致性
*/
struct MeasureGroup {
/**
* @brief 默认构造函数
* @details 初始化雷达点云指针（避免空指针访问），其他成员变量初始化为默认值
*/
MeasureGroup() { this->lidar_.reset(new FullPointCloudType()); };
double lidar_begin_time_ = 0; ///< 当前雷达帧的采集起始时间（秒级，与IMU时间戳同基准）
double lidar_end_time_ = 0; ///< 当前雷达帧的采集结束时间（秒级，用于匹配对应时间段的IMU数据）
FullCloudPtr lidar_ = nullptr; ///< 标准化后的单帧雷达点云（由CloudConvert转换得到）
std::deque<IMUPtr> imu_; ///< 匹配到当前雷达帧的IMU数据队列（雷达采集时间段内的所有IMU读数）
};
/**
* @class MessageSync
* @brief 激光雷达与IMU数据时间同步核心类
* @details 核心功能：
* 1. 接收IMU高频数据（通常200-500Hz）和雷达低频点云数据（通常10-20Hz），分别缓存到对应队列；
* 2. 对不同型号雷达（Livox Avia/Velodyne/Ouster）的点云消息做预处理（调用CloudConvert）；
* 3. 基于时间戳对齐规则，将"单帧雷达点云 + 该帧采集时段内的所有IMU数据"封装为MeasureGroup；
* 4. 同步完成后通过回调函数将MeasureGroup传递给LO/LIO算法模块；
* 异常处理：检测到时间戳回环（新数据时间早于历史数据）时，清空缓冲队列避免数据错乱
*/
class MessageSync {
public:
/// 同步完成后的回调函数类型定义，参数为同步好的激光-IMU数据组
using Callback = std::function<void(const MeasureGroup &)>;
/**
* @brief 构造函数
* @param cb 同步完成后的回调函数（由LO/LIO算法模块注册，用于接收同步数据）
* @details 初始化回调函数，并创建CloudConvert实例用于雷达点云预处理
*/
MessageSync(Callback cb) : callback_(cb), conv_(new CloudConvert) {}
/**
* @brief 从YAML配置文件初始化模块
* @param[in] yaml YAML配置文件路径（如config/sync_params.yaml）
* @details 核心初始化逻辑：
* 1. 调用CloudConvert::LoadFromYAML加载雷达预处理参数；
* 2. 加载时间同步阈值、缓冲队列最大长度等同步相关参数（若有）
*/
void Init(const std::string &yaml);
/**
* @brief 处理单条IMU数据（IMU消息回调入口）
* @param[in] imu 待处理的IMU数据指针（包含角速度、加速度、时间戳等核心信息）
* @details 核心逻辑：
* 1. 检查IMU时间戳是否回环（新数据时间 < 上一次IMU时间），若回环则清空IMU缓冲队列；
* 2. 更新最新IMU时间戳，将当前IMU数据加入缓冲队列；
* 注意：IMU数据仅缓存不处理，同步逻辑在Sync()中触发
*/
void ProcessIMU(IMUPtr imu) {
double timestamp = imu->timestamp_;
// 检测IMU时间戳回环（如系统时钟跳变、传感器重启），清空缓冲避免数据错乱
if (timestamp < last_timestamp_imu_) {
LOG(WARNING) << "imu loop back, clear buffer";
imu_buffer_.clear();
}
last_timestamp_imu_ = timestamp;
imu_buffer_.emplace_back(imu);
}
/**
* @brief 处理标准ROS格式雷达点云消息（适配Velodyne/Ouster）
* @param[in] msg ROS标准点云消息指针（sensor_msgs::PointCloud2）
* @details 核心逻辑：
* 1. 检测雷达时间戳回环，若回环则清空雷达缓冲队列；
* 2. 调用CloudConvert将原始点云转换为标准化FullCloud格式；
* 3. 将标准化点云和对应时间戳加入缓冲队列；
* 4. 触发Sync()函数尝试完成激光-IMU时间同步；
* 适用场景：Velodyne 32线/Ouster 64线等输出标准ROS点云的雷达
*/
void ProcessCloud(const sensor_msgs::PointCloud2::ConstPtr &msg) {
// 检测雷达时间戳回环，清空缓冲队列
if (msg->header.stamp.toSec() < last_timestamp_lidar_) {
LOG(ERROR) << "lidar loop back, clear buffer";
lidar_buffer_.clear();
}
FullCloudPtr cloud(new FullPointCloudType());
// 调用CloudConvert完成点云格式转换（标准化）
conv_->Process(msg, cloud);
lidar_buffer_.push_back(cloud);
time_buffer_.push_back(msg->header.stamp.toSec());
last_timestamp_lidar_ = msg->header.stamp.toSec();
// 触发同步逻辑
Sync();
}
/**
* @brief 处理Livox自定义格式雷达点云消息（适配大疆Avia）
* @param[in] msg Livox自定义点云消息指针（livox_ros_driver::CustomMsg）
* @details 核心逻辑：
* 1. 检测雷达时间戳回环，若回环则清空雷达缓冲队列；
* 2. 调用CloudConvert解析Livox自定义消息并转换为标准化FullCloud格式；
* 3. 过滤空点云（避免无效同步）；
* 4. 将有效点云和对应时间戳加入缓冲队列；
* 5. 触发Sync()函数尝试完成激光-IMU时间同步；
* 适配场景：大疆Livox Avia固态雷达（输出自定义ROS消息）
*/
void ProcessCloud(const livox_ros_driver::CustomMsg::ConstPtr &msg) {
// 检测雷达时间戳回环，清空缓冲队列
if (msg->header.stamp.toSec() < last_timestamp_lidar_) {
LOG(WARNING) << "lidar loop back, clear buffer";
lidar_buffer_.clear();
}
last_timestamp_lidar_ = msg->header.stamp.toSec();
FullCloudPtr ptr(new FullPointCloudType());
// 调用CloudConvert解析Livox自定义消息并标准化
conv_->Process(msg, ptr);
// 过滤空点云，避免后续同步逻辑处理无效数据
if (ptr->empty()) {
return;
}
lidar_buffer_.emplace_back(ptr);
time_buffer_.emplace_back(last_timestamp_lidar_);
// 触发同步逻辑
Sync();
}
private:
/**
* @brief 核心同步逻辑：匹配雷达帧与对应时间段的IMU数据
* @return bool 同步成功返回true，失败返回false
* @details 核心算法：
* 1. 遍历雷达缓冲队列，以雷达帧的时间戳为基准，从IMU缓冲队列中提取"雷达采集时段内的所有IMU数据"；
* 2. 若IMU数据覆盖雷达帧的完整时间范围，则封装为MeasureGroup并调用回调函数；
* 3. 清理已完成同步的历史数据（减少缓冲队列内存占用）；
* 关键设计：采用"雷达帧驱动"的同步策略，仅当有新雷达帧时触发同步
*/
bool Sync();
Callback callback_; ///< 同步完成后的回调函数（传递MeasureGroup给LO/LIO算法）
std::shared_ptr<CloudConvert> conv_ = nullptr; ///< 点云格式转换实例（处理不同雷达的格式标准化）
std::deque<FullCloudPtr> lidar_buffer_; ///< 雷达点云缓冲队列（存储待同步的标准化点云）
std::deque<IMUPtr> imu_buffer_; ///< IMU数据缓冲队列（存储待同步的IMU原始数据）
double last_timestamp_imu_ = -1.0; ///< 上一次处理的IMU时间戳（秒级，用于检测时间戳回环）
double last_timestamp_lidar_ = 0; ///< 上一次处理的雷达时间戳（秒级，用于检测时间戳回环）
std::deque<double> time_buffer_; ///< 雷达点云时间戳缓冲队列（与lidar_buffer_一一对应）
bool lidar_pushed_ = false; ///< 雷达帧是否已完成同步的标志位（避免重复同步）
MeasureGroup measures_; ///< 临时存储待回调的同步数据组（避免频繁创建对象）
double lidar_end_time_ = 0; ///< 当前雷达帧的采集结束时间（用于匹配IMU时间范围）
};该类的核心价值是解决 IMU 与雷达的时间异步问题，通过缓冲队列 + 时间戳匹配，输出时间对齐的 "雷达帧 + IMU 数据组"。
7.5.4松耦合LIO系统的主要流程
松耦合LIO系统主要持有一个ESKF对象，一个MessageSync对象，处理同步之后的点云数据和IMU数据。
cpp
/**
* @file loosely_lio.h/cpp
* @brief 松耦合激光-惯性里程计（Loosely-Coupled LIO）核心实现类
* @note 核心背景：
* 1. 该类实现《7.5节》的松耦合LIO算法，是首个融合IMU和激光雷达的里程计框架，后续紧耦合LIO将复用本框架结构；
* 2. 核心依赖：基于《第3章》的ESKF（误差状态卡尔曼滤波）做状态预测，基于《7.3节》的增量NDT里程计做观测更新；
* 3. 松耦合核心逻辑：IMU通过ESKF独立预测位姿，激光雷达通过增量NDT计算位姿后作为观测值更新ESKF，而非直接融合原始数据；
* 整体流程：IMU初始化 → 消息同步 → IMU预测 → 点云去畸变 → 增量NDT配准 → ESKF观测更新 → 位姿输出/地图构建
*/
/**
* @class LooselyLIO
* @brief 松耦合激光-惯性里程计（LIO）核心类
* @details 核心功能：
* 1. 接收激光雷达（标准ROS格式/Livox自定义格式）和IMU原始数据，通过MessageSync完成时间同步；
* 2. 实现IMU静止初始化，估计IMU初始零偏和重力向量；
* 3. 基于ESKF完成IMU数据的位姿预测，输出预测期间的IMU状态序列；
* 4. 利用IMU预测的状态对激光点云做运动畸变去除；
* 5. 调用增量NDT里程计完成点云配准，将配准结果作为观测值更新ESKF；
* 6. 支持点云去畸变前后的PCD保存、Pangolin UI可视化；
* 松耦合特征：IMU和激光各自独立计算位姿，仅在ESKF层面融合结果，而非原始数据级融合
*/
class LooselyLIO {
public:
// Eigen库内存对齐宏，确保Eigen矩阵（如SE3、NavStated）的内存布局符合SIMD优化，避免内存访问错误
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
/**
* @struct Options
* @brief 松耦合LIO的运行配置选项结构体
* @details 存储非核心算法参数，用于控制程序的辅助功能（如数据保存、可视化），不影响核心里程计算法逻辑
*/
struct Options {
/**
* @brief 默认构造函数
* @details 初始化配置项为默认值：不保存去畸变点云、启用UI可视化
*/
Options() {}
bool save_motion_undistortion_pcd_ = false; ///< 是否保存激光点云运动畸变去除前后的PCD文件（用于调试/分析）
bool with_ui_ = true; ///< 是否启用Pangolin UI可视化（实时显示位姿、点云、IMU状态）
};
/**
* @brief 构造函数
* @param options 运行配置选项（控制PCD保存、UI可视化等）
* @details 初始化配置项，后续需调用Init()完成算法核心参数加载和模块初始化
*/
LooselyLIO(Options options);
/**
* @brief 默认析构函数
* @details 无动态分配的核心资源（模块均为智能指针管理），无需自定义析构逻辑
*/
~LooselyLIO() = default;
/**
* @brief 从YAML配置文件初始化松耦合LIO
* @param[in] config_yaml YAML配置文件路径（如config/loosely_lio.yaml）
* @return bool 初始化成功返回true，失败返回false
* @details 核心初始化逻辑：
* 1. 调用LoadFromYAML加载算法核心参数（IMU内参、激光-IMU外参、NDT参数、ESKF参数等）；
* 2. 初始化MessageSync（消息同步器）、增量NDT里程计、ESKF、IMU初始化模块；
* 3. 若启用UI，初始化Pangolin可视化窗口；
* 4. 校验参数合法性（如外参是否存在、IMU内参是否合理）
*/
bool Init(const std::string &config_yaml);
/**
* @brief 标准ROS格式激光点云回调函数（Velodyne/Ouster雷达）
* @param[in] msg 标准ROS点云消息指针（sensor_msgs::PointCloud2）
* @details 核心逻辑：将点云消息转发给MessageSync模块，触发激光-IMU数据同步；
* 是激光雷达数据进入松耦合LIO的入口之一
*/
void PCLCallBack(const sensor_msgs::PointCloud2::ConstPtr &msg);
/**
* @brief Livox自定义格式激光点云回调函数（大疆Avia雷达）
* @param[in] msg Livox自定义点云消息指针（livox_ros_driver::CustomMsg）
* @details 核心逻辑：将Livox自定义点云消息转发给MessageSync模块，触发激光-IMU数据同步；
* 是Livox Avia雷达数据进入松耦合LIO的入口
*/
void LivoxPCLCallBack(const livox_ros_driver::CustomMsg::ConstPtr &msg);
/**
* @brief IMU数据回调函数
* @param[in] msg_in IMU数据指针（包含角速度、加速度、时间戳）
* @details 核心逻辑：将IMU原始数据转发给MessageSync模块，缓存待同步；
* 是IMU数据进入松耦合LIO的唯一入口
*/
void IMUCallBack(IMUPtr msg_in);
/**
* @brief 程序结束清理函数
* @details 核心逻辑：
* 1. 若启用UI，关闭Pangolin可视化窗口；
* 2. 保存未写入的PCD文件（若开启save_motion_undistortion_pcd_）；
* 3. 释放消息同步器、NDT里程计等模块资源
*/
void Finish();
private:
/**
* @brief 处理同步后的激光-IMU数据组（松耦合LIO核心流程入口）
* @param[in] meas 同步后的MeasureGroup（单帧激光点云 + 对应时间段IMU数据）
* @details 松耦合LIO完整流程（核心函数）：
* 1. 若IMU未初始化，调用TryInitIMU完成IMU静止初始化；
* 2. 调用Predict()基于IMU数据完成ESKF状态预测；
* 3. 调用Undistort()利用IMU预测状态去除激光点云运动畸变；
* 4. 调用Align()执行增量NDT配准，将结果作为观测更新ESKF；
* 5. 若启用UI，更新可视化内容；
* 6. 计数帧数，保存去畸变点云（若开启配置）
*/
void ProcessMeasurements(const MeasureGroup &meas);
/**
* @brief 从YAML配置文件加载算法核心参数
* @param[in] yaml YAML配置文件路径
* @return bool 加载成功返回true，失败返回false
* @details 加载的核心参数：
* 1. IMU内参（零偏、噪声、随机游走）、激光-IMU外参TIL_；
* 2. 增量NDT参数（体素大小、配准阈值、迭代次数）；
* 3. ESKF参数（状态噪声、观测噪声、更新频率）；
* 4. IMU初始化参数（静止判定阈值、初始化时长）
*/
bool LoadFromYAML(const std::string &yaml);
/**
* @brief 尝试完成IMU静止初始化
* @details 核心逻辑：
* 1. 检测IMU数据是否满足静止条件（角速度/加速度方差低于阈值）；
* 2. 若满足，估计IMU初始零偏、重力向量、初始姿态；
* 3. 初始化完成后，设置imu_need_init_为false，启动ESKF预测；
* 注意：IMU初始化是LIO运行的前提，未初始化时不执行位姿预测和配准
*/
void TryInitIMU();
/**
* @brief 利用IMU数据通过ESKF预测位姿状态
* @details 核心逻辑：
* 1. 从measures_中提取同步后的IMU数据序列；
* 2. 基于ESKF的预测方程，积分IMU数据得到机器人位姿、速度、IMU零偏等状态；
* 3. 将预测期间的所有IMU状态存入imu_states_，用于后续点云去畸变；
* 松耦合特征：IMU独立预测位姿，不依赖激光雷达数据
*/
void Predict();
/**
* @brief 对measures_中的激光点云进行运动畸变去除
* @details 核心逻辑：
* 1. 基于imu_states_中IMU预测的逐时刻位姿，计算激光扫描过程中机器人的运动轨迹；
* 2. 对激光点云中每个点，根据其采集时间戳，用对应时刻的IMU位姿修正坐标；
* 3. 去畸变后的点云存入scan_undistort_，用于后续增量NDT配准；
* 关键作用：消除机器人运动导致的点云拉伸/模糊，提升NDT配准精度
*/
void Undistort();
/**
* @brief 执行增量NDT配准，并将结果作为观测更新ESKF
* @details 松耦合核心观测环节：
* 1. 调用增量NDT里程计（inc_ndt_lo_），对去畸变后的点云（scan_undistort_）做配准，得到激光里程计位姿pose_of_lo_；
* 2. 将激光里程计位姿作为观测值，代入ESKF的观测方程，更新ESKF的状态（修正IMU预测的位姿误差）；
* 3. 若为第一帧激光（flg_first_scan_），初始化增量NDT的参考地图；
* 松耦合特征：激光仅提供位姿观测值，而非原始点云残差约束
*/
void Align();
private:
/// 核心模块（松耦合LIO的依赖组件）
std::shared_ptr<MessageSync> sync_ = nullptr; ///< 激光-IMU消息同步器（完成时间戳对齐，输出MeasureGroup）
StaticIMUInit imu_init_; ///< IMU静止初始化模块（估计初始零偏、重力、姿态）
std::shared_ptr<sad::IncrementalNDTLO> inc_ndt_lo_ = nullptr; ///< 增量NDT里程计（激光配准核心，输出激光位姿）
/// 点云数据（去畸变相关）
FullCloudPtr scan_undistort_{new FullPointCloudType()}; ///< 运动畸变去除后的激光点云（用于NDT配准）
SE3 pose_of_lo_; ///< 增量NDT配准得到的激光里程计位姿（作为ESKF观测值）
Options options_; ///< 运行配置选项（控制PCD保存、UI可视化）
// 状态标志位（控制算法流程）
bool imu_need_init_ = true; ///< IMU是否需要初始化（true：未初始化，false：已初始化）
bool flg_first_scan_ = true; ///< 是否为第一帧激光点云（用于初始化NDT参考地图）
int frame_num_ = 0; ///< 激光帧计数（用于PCD文件命名、日志输出）
// EKF相关数据（松耦合核心状态）
MeasureGroup measures_; ///< 从MessageSync接收的同步后激光-IMU数据组
std::vector<NavStated> imu_states_; ///< ESKF预测期间的IMU状态序列（包含位姿、速度、零偏等，用于点云去畸变）
ESKFD eskf_; ///< 误差状态卡尔曼滤波器（ESKF），松耦合融合的核心载体
SE3 TIL_; ///< 激光雷达到IMU的外参变换矩阵（旋转+平移），需从YAML加载或标定
std::shared_ptr<ui::PangolinWindow> ui_ = nullptr; ///< Pangolin可视化窗口（实时显示位姿、点云、IMU状态）
};该类是松耦合 LIO 的核心载体，核心逻辑为 "IMU ESKF 预测 + 增量 NDT 位姿观测更新"，体现松耦合 "结果级融合" 的特征；核心流程依赖MessageSync的时间同步、StaticIMUInit的初始化、IncrementalNDTLO的配准、ESKFD的融合四大模块；ProcessMeasurements是流程入口，串联 IMU 预测、点云去畸变、NDT 配准、ESKF 更新全环节；
对ProcessMeasurements函数的注释拆解了松耦合 LIO 的完整流程：IMU 初始化→预测→去畸变→配准→观测更新。
初始化完毕后，先使用IMU数据进行预测，再用预测数据对点云去畸变，最后对去畸变的点云做配准。
**首先第一步，使用IMU数据进行预测。**直接将IMU数据传递给滤波器，然后记录滤波器的名义状态变量（机器人当前真实状态的近似值，包含位置、姿态、速度、IMU零偏）：
此处滤波器的作用，
- 接收 IMU 数据做 "非线性预测"，输出名义状态（位姿 / 速度等）；
- 接收激光位姿做 "线性更新"，修正名义状态的误差；
最终输出无漂移、连续的机器人状态，解决纯 IMU 漂移、纯激光跳变的问题 。
cpp
/**
* @brief 松耦合LIO的IMU预测核心函数
* @details 核心功能：
* 1. 清空历史IMU状态序列，避免旧数据干扰当前帧的预测；
* 2. 从ESKF中获取初始名义状态，作为IMU预测的起点；
* 3. 遍历同步后的IMU数据序列，逐帧调用ESKF的Predict接口完成状态预测；
* 4. 记录每一步预测后的名义状态到imu_states_，为后续点云去畸变提供逐时刻的IMU位姿；
* 工程背景：该函数是ESKF预测步的具体实现，IMU高频数据（200-500Hz）的连续预测保证了状态的时间连续性，
* 预测得到的imu_states_是点云去畸变的核心输入（每个激光点对应一个IMU预测状态）
*/
void LooselyLIO::Predict() {
// 清空历史IMU状态序列：避免上一帧的IMU状态残留，保证当前帧预测的独立性
imu_states_.clear();
// 初始化IMU状态序列：将ESKF当前的名义状态（预测起点）加入序列，作为预测的第0帧状态
// 注：ESKF的初始名义状态来自上一帧的最终修正结果，或IMU初始化后的初始状态
imu_states_.emplace_back(eskf_.GetNominalState());
/// 对同步后的IMU数据逐帧执行ESKF预测，生成连续的名义状态序列
// 遍历对象：measures_.imu_ ------ 与当前激光帧同步的所有IMU数据（激光扫描时段内的IMU读数）
for (auto &imu : measures_.imu_) {
// 调用ESKF的Predict接口：用单帧IMU数据（角速度/加速度）完成非线性积分，更新ESKF的名义状态
// 内部逻辑：补偿IMU零偏/重力→积分计算位姿/速度变化→更新协方差矩阵（表示状态可信度）
eskf_.Predict(*imu);
// 记录当前预测后的名义状态到imu_states_：
// imu_states_最终存储"激光帧对应时段内的所有IMU名义状态"，序列长度 = IMU帧数 + 1（初始状态）
// 后续Undistort()函数会根据激光点的时间戳，从该序列中插值得到对应时刻的IMU位姿，完成点云去畸变
imu_states_.emplace_back(eskf_.GetNominalState());
}
}第二步，去畸变。首先了解去谁的畸变，为什么会产生畸变。 去畸变是去当前帧激光点云的畸变。激光雷达扫描一帧点云需要时间（通常 10Hz 雷达≈100ms / 帧），而机器人在这 100ms 内会运动（平移 / 旋转），导致同一帧内不同时刻采集的点云 "不在同一个坐标系下"，出现拉伸 / 模糊 / 错位，这就是运动畸变。
去畸变的具体原理：**使用IMU预测位姿进行运动补偿 。**由于松耦合LIO系统里有IMU位姿，因此可以用IMU预测的位姿对点云进行运动补偿。所谓运动补偿，指的是补偿由车辆运动带来的扫描数据畸变。如果扫描过程中雷达本身静止，那么扫描到的物体真实距离与测量距离一致。然而，如果雷达本身随车辆运动，就应当在扫描的过程中考虑车辆的运动情况。也就是上文提到的运动畸变。车速越快，或者车辆转动角度越大，运动畸变就越明显。
运动补偿的原理 :它需要知道在扫描过程中的车辆位姿。假设雷达的单次扫描时间为。在这次扫描过程中，雷达返回的每个点应该带有它的时间信息.记单个扫描点的位置(雷达坐标系下)为
。通过某种手段，可以任意地查询到从0到
时刻的雷达位姿(实践中可以通过对IMU位姿进行插值得到)。不妨记t时刻对应的IMU位姿为
结束时刻为
。为了实现运动补偿，计算这个点在扫描结束时刻的位姿，对它的坐标进行转换。(根据时间来进行参数预测)
设IMU与雷达之间的外参为TL,那么运动补偿的转换公式应为
如果希望直接得到IMU坐标系下的点云，则可以不乘左侧的那个矩阵。实现中，可以通过不同手段获取雷达起止时刻的位姿。在松耦合LIO系统里，一个很自然的做法是在EKF预测阶段得到上个扫描到下个扫描之间的相对位姿。
去运动畸变代码展示：
cpp
/**
* @brief 松耦合LIO的点云运动畸变去除核心函数
* @details 核心功能：
* 1. 基于IMU预测的名义状态序列（imu_states_），将带运动畸变的原始激光点云逐点修正；
* 2. 统一将所有激光点转换到"当前激光帧最后时刻的IMU坐标系"下，消除机器人运动导致的点云拉伸/错位；
* 3. 支持保存去畸变前后的点云PCD文件，用于调试和畸变效果分析；
* 去畸变原理：通过时间插值获取每个激光点采集时刻的IMU位姿，利用位姿变换将点从"采集时刻的激光坐标系"转换到"帧末时刻的激光坐标系"；
* 输入：measures_.lidar_（同步后的原始带畸变点云），imu_states_（Predict()生成的IMU名义状态序列）；
* 输出：scan_undistort_（去畸变后的无畸变点云）
*/
void LooselyLIO::Undistort() {
// 1. 获取输入：同步后的原始带畸变激光点云（去畸变的处理对象）
auto cloud = measures_.lidar_;
// 2. 获取当前激光帧最后时刻的IMU名义状态（作为去畸变的目标坐标系基准）
auto imu_state = eskf_.GetNominalState(); // 最后时刻的IMU名义状态（位置p_、旋转R_）
SE3 T_end = SE3(imu_state.R_, imu_state.p_); // 帧末时刻IMU在世界坐标系下的位姿（目标位姿）
// 3. 若开启保存配置，保存去畸变前的原始点云（用于对比畸变效果）
if (options_.save_motion_undistortion_pcd_) {
sad::SaveCloudToFile("./data/ch7/before_undist.pcd", *cloud);
}
/// 4. 核心逻辑：并行遍历所有激光点，逐点消除运动畸变
// std::execution::par_unseq：启用多线程并行处理点云，提升去畸变效率（激光点数量通常上万，并行可大幅提速）
std::for_each(std::execution::par_unseq, cloud->points.begin(), cloud->points.end(), [&](auto &pt) {
// 初始化Ti为帧末位姿T_end（插值失败时的默认值）
SE3 Ti = T_end;
NavStated match; // 插值得到的该点采集时刻的IMU状态
// 4.1 时间插值：根据激光点的采集时间，从imu_states_中匹配对应时刻的IMU位姿
// 关键参数解释：
// - measures_.lidar_begin_time_：当前激光帧的起始时间（秒级）
// - pt.time：该激光点的采集时间与帧起始时间的差值（毫秒级）→ 转换为秒需×1e-3
// - imu_states_：Predict()生成的IMU名义状态序列（包含逐时刻的位姿、时间戳）
// - 第一个lambda：提取IMU状态的时间戳（用于插值匹配）
// - 第二个lambda：提取IMU状态的位姿（SE3格式，用于插值计算）
// - 输出：Ti（该点采集时刻的IMU位姿）、match（该点采集时刻的IMU完整状态）
math::PoseInterp<NavStated>(
measures_.lidar_begin_time_ + pt.time * 1e-3, // 该激光点的绝对采集时间（秒）
imu_states_, // IMU名义状态序列（插值数据源）
[](const NavStated &s) { return s.timestamp_; }, // 状态→时间戳的映射函数
[](const NavStated &s) { return s.GetSE3(); }, // 状态→位姿（SE3）的映射函数
Ti, // 输出：插值得到的该点时刻IMU位姿
match); // 输出：插值得到的该点时刻IMU完整状态
// 4.2 坐标转换：将激光点从"采集时刻的激光坐标系"转换到"帧末时刻的激光坐标系"（核心去畸变操作）
// pi：当前激光点在采集时刻激光坐标系下的原始坐标（x/y/z）
Vec3d pi = ToVec3d(pt);
// 坐标转换公式解析（从内到外）：
// 1. TIL_ * pi：激光点→采集时刻IMU坐标系（激光到IMU的外参变换）
// 2. Ti * (TIL_ * pi)：采集时刻IMU坐标系→世界坐标系
// 3. T_end.inverse() * ...：世界坐标系→帧末时刻IMU坐标系
// 4. TIL_.inverse() * ...：帧末时刻IMU坐标系→帧末时刻激光坐标系
// 最终p_compensate：该点在帧末激光坐标系下的无畸变坐标
Vec3d p_compensate = TIL_.inverse() * T_end.inverse() * Ti * TIL_ * pi;
// 4.3 更新激光点坐标：替换为去畸变后的坐标
pt.x = p_compensate(0);
pt.y = p_compensate(1);
pt.z = p_compensate(2);
});
// 5. 赋值去畸变后的点云：作为后续NDT配准的输入
scan_undistort_ = cloud;
// 6. 若开启保存配置，保存去畸变后的点云（用于对比畸变修正效果）
if (options_.save_motion_undistortion_pcd_) {
sad::SaveCloudToFile("./data/ch7/after_undist.pcd", *cloud);
}
}下图右侧部分的点云接近扫描结束点，所以修正量更小；左侧部分的点云接近扫描开始时间点，所以修正量较大。当然，这种去畸变的方法的前提是滤波器本身有效。如果滤波器失效或位姿发散，则去畸变算法也就随之发散了。
7.5.5松耦合LIO系统配准部分
只需将上文去畸变的点云数据传递给NDT里程计。
cpp
/**
* @brief 松耦合LIO的观测更新核心函数（增量NDT配准 + ESKF状态修正）
* @details 核心功能：
* 1. 坐标系对齐：将去畸变后的激光点云从激光坐标系转换到IMU坐标系（与ESKF的状态坐标系统一）；
* 2. 点云降采样：通过体素滤波减少点云数量，平衡NDT配准的精度与计算效率；
* 3. 首帧处理：初始化增量NDT里程计的参考地图，完成里程计初始位姿设定；
* 4. 非首帧处理：以ESKF预测的IMU位姿为初始值执行增量NDT配准，将配准得到的激光里程计位姿作为观测值更新ESKF；
* 5. 可视化：若启用UI，实时更新点云与导航状态的可视化界面；
* 松耦合核心特征：激光仅输出位姿结果作为ESKF的观测值（结果级融合），而非将点云残差直接作为观测方程，区别于紧耦合LIO；
* 输入：scan_undistort_（去畸变后的激光点云）、eskf_（ESKF当前名义状态）；
* 输出：更新后的ESKF名义状态（融合激光观测后的位姿）、pose_of_lo_（激光里程计输出的位姿）
*/
void LooselyLIO::Align() {
// 1. 创建临时点云指针，存储坐标系转换后的点云
FullCloudPtr scan_undistort_trans(new FullPointCloudType);
// 1.1 激光点云→IMU坐标系转换（松耦合坐标系对齐关键步骤）
// TIL_：激光雷达到IMU的外参变换矩阵（旋转+平移），matrix()转为4×4齐次矩阵适配PCL接口；
// 转换原因：ESKF的状态（位姿/速度等）以IMU为中心定义，需将激光点云转换到IMU坐标系才能与ESKF状态匹配
pcl::transformPointCloud(*scan_undistort_, *scan_undistort_trans, TIL_.matrix());
// 1.2 更新去畸变点云为IMU坐标系版本，作为后续NDT配准的输入
scan_undistort_ = scan_undistort_trans;
// 2. 点云类型转换：将FullPointCloudType（含时间戳/反射率等扩展信息）转为NDT要求的PointCloudType（仅x/y/z）
// 适配inc_ndt_lo_（增量NDT里程计）的输入数据格式
auto current_scan = ConvertToCloud<FullPointType>(scan_undistort_);
// 3. 体素网格降采样（工程性能优化核心步骤）
pcl::VoxelGrid<PointType> voxel;
// 设置体素大小为0.5m×0.5m×0.5m：
// - 过大：点云特征丢失，配准精度下降；
// - 过小：点云数量过多，配准耗时增加；
// 0.5m是户外场景的经验值，平衡精度（厘米级）与效率（上万点→数千点）
voxel.setLeafSize(0.5, 0.5, 0.5);
voxel.setInputCloud(current_scan); // 设置待滤波的原始点云
CloudPtr current_scan_filter(new PointCloudType);
voxel.filter(*current_scan_filter); // 执行降采样，输出滤波后的稀疏点云
/// 4. 首帧激光数据特殊处理：初始化增量NDT里程计
if (flg_first_scan_) {
SE3 pose; // 首帧位姿默认设为单位矩阵（世界坐标系原点）
// 将首帧降采样点云加入增量NDT，作为初始参考地图，完成里程计初始化
inc_ndt_lo_->AddCloud(current_scan_filter, pose);
flg_first_scan_ = false; // 标记首帧处理完成，后续帧执行正常配准流程
return; // 首帧无ESKF预测位姿，无需执行观测更新，直接返回
}
/// 5. 非首帧激光数据处理：增量NDT配准 + ESKF观测更新（松耦合核心融合逻辑）
// 5.1 从ESKF获取当前预测的IMU名义位姿（作为NDT配准的初始值）
// 作用：提供初始位姿可大幅提升NDT配准的收敛速度和稳定性，避免配准陷入局部最优
SE3 pose_predict = eskf_.GetNominalSE3();
// 5.2 执行增量NDT配准：以ESKF预测位姿为初始值，将当前帧配准到参考地图
// 参数true：启用"增量配准模式"，基于历史参考地图做局部配准，而非全局配准
// 输出：pose_predict被更新为激光里程计输出的IMU位姿（观测值）
inc_ndt_lo_->AddCloud(current_scan_filter, pose_predict, true);
// 5.3 记录激光里程计输出的位姿（松耦合的观测值）
pose_of_lo_ = pose_predict;
// 5.4 ESKF观测更新：将激光位姿作为观测值修正IMU预测的名义状态
// 参数说明：
// - pose_of_lo_：激光观测到的IMU位姿（观测值）；
// - 1e-2：位置观测噪声协方差（单位：m²），值越小表示激光位置观测可信度越高；
// - 1e-2：姿态观测噪声协方差（单位：rad²），值越小表示激光姿态观测可信度越高；
// 内部逻辑：计算观测值与ESKF预测值的误差→通过卡尔曼增益修正名义状态→重置误差状态→更新协方差矩阵
eskf_.ObserveSE3(pose_of_lo_, 1e-2, 1e-2);
// 6. 可视化更新：若启用UI，实时展示当前帧点云与ESKF名义状态
if (options_.with_ui_) {
// 更新点云可视化：将当前帧点云与ESKF名义位姿关联显示（转换为激光坐标系供UI渲染）
ui_->UpdateScan(current_scan, eskf_.GetNominalSE3());
// 更新导航状态可视化：展示ESKF的完整名义状态（位置、姿态、速度、IMU零偏等）
ui_->UpdateNavState(eskf_.GetNominalState());
}
// 7. 激光帧计数：统计已处理的帧数（用于日志输出、数据保存、调试分析）
frame_num_++;
}该函数是松耦合 LIO "激光观测更新" 的核心，核心链路为：**坐标系对齐→点云降采样→NDT 配准→ESKF 融合。**体素降采样是必做的工程优化，是平衡 NDT 配准精度与效率的关键；
ps：感觉这章需要结合很多前面的知识，争取1.10之前进行第二次的学习，加油！