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

1. LIO-SAM_based_relocalization系统概述LIO-SAM_based_relocalization是基于LIO-SAM框架改进的机器人重定位系统由哈尔滨工业大学GaoChao团队开发。这个系统在原有LIO-SAM的基础上增加了全局重定位功能使其能够在已有地图中进行精准定位。与原始LIO-SAM相比这个改进版本特别优化了重定位模块的架构使其更适合在KITTI数据集这样的标准评测环境中进行轨迹评估。我在实际测试中发现该系统的一个显著特点是它对先验地图的依赖性。系统会预先构建环境地图然后在重定位阶段利用这些地图信息进行位姿估计。这种设计使得它在已知环境中的定位精度非常高特别是在KITTI数据集这样的结构化道路场景中表现尤为出色。系统主要由四个核心模块组成imageProjection数据预处理、imuPreintegrationIMU预积分、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()函数中实现。我通过实验发现这个过程可以分为几个关键步骤角点优化当前帧角点与局部地图角点匹配平面点优化当前帧平面点与局部地图平面点匹配联合优化将匹配结果结合到因子图中进行全局优化具体实现上角点优化使用了基于特征值分析的方法来判断点是否构成有效的边缘线。我在测试中发现当特征值满足matD1.atfloat(0,0)3*matD1.atfloat(0,1)条件时系统认为这些点构成了有效的边缘特征。3.2 因子图优化系统使用GTSAM库进行因子图优化这是整个重定位系统的核心。关键的数据结构包括gtsam::NonlinearFactorGraph gtSAMgraph; gtsam::Values initialEstimate; gtsam::ISAM2 *isam;在实际运行中系统会不断向因子图中添加以下几种约束里程计因子相邻关键帧之间的相对位姿约束GPS因子当GPS数据可用时提供的全局位置约束回环因子在检测到回环时添加的约束我在代码调试过程中发现GPS因子的添加条件比较严格只有当位姿协方差较大时才会使用GPS数据进行校正这种设计有效避免了GPS噪声对系统的影响。4. KITTI数据集上的轨迹评估4.1 轨迹输出实现为了在KITTI数据集上进行评估我修改了源码以支持TUM格式的轨迹输出。这个修改主要在publishOdometry()函数中实现。关键代码如下std::ofstream pose1(/path/to/trajectory.txt, std::ios::app); Eigen::Matrixdouble,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数据集上评估时有几个关键点需要注意坐标系对齐KITTI数据集的坐标系与系统默认坐标系不同需要进行适当的转换时间同步确保输出的轨迹时间戳与数据集的时间戳对齐评估指标常用的指标包括绝对位姿误差(APE)和相对位姿误差(RPE)我在实际测试中发现系统的性能很大程度上依赖于参数配置。特别是以下几个参数对重定位精度影响较大surroundingKeyframeSearchRadius局部地图搜索半径historyKeyframeSearchRadius回环检测搜索半径mappingProcessInterval地图更新频率5. 关键函数深度解析5.1 mapOptimization类构造函数构造函数主要完成以下初始化工作设置ISAM2优化器的参数初始化各种发布器和订阅器配置点云降采样滤波器参数分配内存空间一个关键的初始化代码如下downSizeFilterCorner.setLeafSize(mappingCornerLeafSize, mappingCornerLeafSize, mappingCornerLeafSize); downSizeFilterSurf.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize);这些降采样参数直接影响系统的性能和精度需要根据实际场景进行调整。5.2 激光雷达数据处理流程激光雷达数据的处理主要在laserCloudInfoHandler函数中完成基本流程如下获取当前帧时间戳提取特征点云角点和平面点执行位姿初始化对当前帧点云进行降采样执行scan-to-map优化提取周围关键帧构建局部地图保存关键帧并更新因子图校正所有关键帧位姿发布结果这个流程中的每个步骤都对最终的重定位精度有重要影响。我在调试过程中发现updateInitialGuess()函数中的位姿初始化策略对系统稳定性特别关键。6. 系统优化与实践建议6.1 性能优化技巧根据我的实践经验以下几点可以显著提升系统性能调整降采样参数平衡精度和计算效率优化关键帧策略调整关键帧创建条件避免过多冗余关键帧合理设置搜索半径根据环境特点调整局部地图和回环检测的搜索范围利用IMU数据合理配置IMU权重提高初始位姿估计质量6.2 常见问题解决在系统实现和测试过程中我遇到过几个典型问题及解决方案位姿发散问题通常是由于IMU参数不匹配或初始位姿估计不准确导致可以通过调整IMU噪声参数解决地图漂移问题适当增加GPS因子权重或减小里程计噪声参数计算资源不足降低点云分辨率或减少局部地图大小一个特别需要注意的问题是时间同步。不同传感器数据的时间对齐对系统性能影响很大在实际部署时需要确保所有传感器的时间戳正确同步。