LIO-SAM_based_relocalization在KITTI数据集上的轨迹评估与源码解析（一）—————— 重定位模块的架构与实现

1. LIO-SAM_based_relocalization系统概述

LIO-SAM_based_relocalization是基于LIO-SAM框架改进的机器人重定位系统，由哈尔滨工业大学GaoChao团队开发。这个系统在原有LIO-SAM的基础上增加了全局重定位功能，使其能够在已有地图中进行精准定位。与原始LIO-SAM相比，这个改进版本特别优化了重定位模块的架构，使其更适合在KITTI数据集这样的标准评测环境中进行轨迹评估。

我在实际测试中发现，该系统的一个显著特点是它对先验地图的依赖性。系统会预先构建环境地图，然后在重定位阶段利用这些地图信息进行位姿估计。这种设计使得它在已知环境中的定位精度非常高，特别是在KITTI数据集这样的结构化道路场景中表现尤为出色。

系统主要由四个核心模块组成：imageProjection（数据预处理）、imuPreintegration（IMU预积分）、featureExtraction（特征提取）和mapOptimization（地图优化）。其中mapOptimization模块是系统的核心，承担了大部分的计算任务，也是重定位功能的主要实现部分。

2. 重定位模块的架构设计

2.1 整体架构

重定位模块的核心是mapOptimization类，这个类包含了系统的大部分关键功能。从代码结构来看，它主要包含以下几个重要组成部分：

• 因子图优化框架：使用GTSAM库实现的因子图优化，整合了IMU、激光雷达和GPS等多种传感器数据
• 局部地图管理：维护当前帧周围的局部点云地图，用于scan-to-map匹配
• 位姿估计与优化：实现当前帧位姿的初始估计和迭代优化
• 关键帧管理：决定何时创建新的关键帧，并管理历史关键帧数据

我在分析代码时注意到，这个重定位模块相比原始LIO-SAM做了不少简化，特别是移除了回环检测功能。这种设计选择使得系统更加专注于重定位任务，减少了不必要的计算开销。

2.2 关键数据结构

mapOptimization类中定义了几个重要的数据结构：

struct PointXYZIRPYT { PCL_ADD_POINT4D PCL_ADD_INTENSITY; float roll; float pitch; float yaw; double time; EIGEN_MAKE_ALIGNED_OPERATOR_NEW } EIGEN_ALIGN16;

这个结构体定义了带有6D位姿信息的点云类型，是系统存储关键帧位姿的基础。在实际运行中，系统会维护两个关键的点云集合：

• cloudKeyPoses3D：存储关键帧的3D位置
• cloudKeyPoses6D：存储关键帧的6D位姿（位置+姿态）

3. 核心功能实现解析

3.1 扫描匹配优化

系统的核心功能之一是scan-to-map的优化，这部分主要在scan2MapOptimization()函数中实现。我通过实验发现，这个过程可以分为几个关键步骤：

1. 角点优化：当前帧角点与局部地图角点匹配
2. 平面点优化：当前帧平面点与局部地图平面点匹配
3. 联合优化：将匹配结果结合到因子图中进行全局优化

具体实现上，角点优化使用了基于特征值分析的方法来判断点是否构成有效的边缘线。我在测试中发现，当特征值满足matD1.at<float>(0,0)>3*matD1.at<float>(0,1)条件时，系统认为这些点构成了有效的边缘特征。

3.2 因子图优化

系统使用GTSAM库进行因子图优化，这是整个重定位系统的核心。关键的数据结构包括：

gtsam::NonlinearFactorGraph gtSAMgraph; gtsam::Values initialEstimate; gtsam::ISAM2 *isam;

在实际运行中，系统会不断向因子图中添加以下几种约束：

1. 里程计因子：相邻关键帧之间的相对位姿约束
2. GPS因子：当GPS数据可用时提供的全局位置约束
3. 回环因子：在检测到回环时添加的约束

我在代码调试过程中发现，GPS因子的添加条件比较严格，只有当位姿协方差较大时才会使用GPS数据进行校正，这种设计有效避免了GPS噪声对系统的影响。

4. KITTI数据集上的轨迹评估

4.1 轨迹输出实现

为了在KITTI数据集上进行评估，我修改了源码以支持TUM格式的轨迹输出。这个修改主要在publishOdometry()函数中实现。关键代码如下：

std::ofstream pose1("/path/to/trajectory.txt", std::ios::app); Eigen::Matrix<double,4,4> myaloam_pose; myaloam_pose.topLeftCorner(3,3) = rotation_matrix; myaloam_pose(0,3) = laserOdometryROS.pose.pose.position.x; // ...其余坐标赋值... pose1 << myaloam_pose(0,0) << " " << myaloam_pose(0,1) << " ... " << std::endl;

这个修改使得系统能够输出标准的轨迹文件，方便后续使用evo等工具进行评估。在实际使用中，我还添加了时间戳记录功能，确保轨迹数据能够与ground truth正确对齐。

4.2 评估注意事项

在KITTI数据集上评估时，有几个关键点需要注意：

1. 坐标系对齐：KITTI数据集的坐标系与系统默认坐标系不同，需要进行适当的转换
2. 时间同步：确保输出的轨迹时间戳与数据集的时间戳对齐
3. 评估指标：常用的指标包括绝对位姿误差(APE)和相对位姿误差(RPE)

我在实际测试中发现，系统的性能很大程度上依赖于参数配置。特别是以下几个参数对重定位精度影响较大：

• surroundingKeyframeSearchRadius：局部地图搜索半径
• historyKeyframeSearchRadius：回环检测搜索半径
• mappingProcessInterval：地图更新频率

5. 关键函数深度解析

5.1 mapOptimization类构造函数

构造函数主要完成以下初始化工作：

1. 设置ISAM2优化器的参数
2. 初始化各种发布器和订阅器
3. 配置点云降采样滤波器参数
4. 分配内存空间

一个关键的初始化代码如下：

downSizeFilterCorner.setLeafSize(mappingCornerLeafSize, mappingCornerLeafSize, mappingCornerLeafSize); downSizeFilterSurf.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize);

这些降采样参数直接影响系统的性能和精度，需要根据实际场景进行调整。

5.2 激光雷达数据处理流程

激光雷达数据的处理主要在laserCloudInfoHandler函数中完成，基本流程如下：

1. 获取当前帧时间戳
2. 提取特征点云（角点和平面点）
3. 执行位姿初始化
4. 对当前帧点云进行降采样
5. 执行scan-to-map优化
6. 提取周围关键帧构建局部地图
7. 保存关键帧并更新因子图
8. 校正所有关键帧位姿
9. 发布结果

这个流程中的每个步骤都对最终的重定位精度有重要影响。我在调试过程中发现，updateInitialGuess()函数中的位姿初始化策略对系统稳定性特别关键。

6. 系统优化与实践建议

6.1 性能优化技巧

根据我的实践经验，以下几点可以显著提升系统性能：

1. 调整降采样参数：平衡精度和计算效率
2. 优化关键帧策略：调整关键帧创建条件，避免过多冗余关键帧
3. 合理设置搜索半径：根据环境特点调整局部地图和回环检测的搜索范围
4. 利用IMU数据：合理配置IMU权重，提高初始位姿估计质量

6.2 常见问题解决

在系统实现和测试过程中，我遇到过几个典型问题及解决方案：

1. 位姿发散问题：通常是由于IMU参数不匹配或初始位姿估计不准确导致，可以通过调整IMU噪声参数解决
2. 地图漂移问题：适当增加GPS因子权重或减小里程计噪声参数
3. 计算资源不足：降低点云分辨率或减少局部地图大小

一个特别需要注意的问题是时间同步。不同传感器数据的时间对齐对系统性能影响很大，在实际部署时需要确保所有传感器的时间戳正确同步。