代码launch将多个node加载到同一个进程中内存共享零拷贝。类似于ROS2中的component。1.全局参数设置multisession_mode是否启用多会话模式SaveDir日志与地图保存路径2.Nodelet Manager 启动创建一个名为sample_nodelet_manager的宿主进程所有 nodelet 插件LIO、回环检测会在这个进程中运行3.FAST-LIO2 Nodelet将fastlio_plugin加载到 sample_nodelet_manager 进程中执行然后加载 FAST-LIO2 的配置参数4.回环检测 Nodeletloop_detection_plugin也被加载到 Nodelet Manager 内启动前延迟 1 秒避免和 FAST-LIO2 同时启动导致 Eigen 内存对齐问题5.回环优化 Node作者在README提到过回环优化节点无法作为 nodelet 正常触发因此回环优化作为 node 启动。主函数实例化 Nodelet 对象时被调用 fastlio_plugin 的构造函数Nodelet Manager 调用 load 插件后调用 onInit()函数执行线程mainLIOFunction(等效理解为node的主函数)。回环检测同样的逻辑。1.初始化扩展IKFoM滤波器和FAST-LIO2 一致2.点云回调函数Livox激光雷达点云处理的时候会对点云中的 tag 字段进行筛选如果使用的是MID-360需要取消注释后半段代码# preprocess.cpp avia_handler() if((msg-points[i].line N_SCANS) ((msg-points[i].tag 0x30) 0x10) //|| (msg-points[i].tag 0x30) 0x00 //mid3603.处理回环检测节点发布消息的回调函数submap_id_cbk获取回环检测节点发布的 关键帧ID知道回环检测处理到哪个关键帧了关键帧是由多帧累计的。然后获取 map_incremental() 里逐步 push_back 累计的点作为对应的点云。填充unmap_submap_info 普通模式中unmap_submap_info保存的是回环检测发布的关键帧ID(ID 0)及对应的点云multi_session模式还会保存的是先验关键帧ID(ID 0)及对应的点云和优化前后位姿submap_pose_cbk获取该关键帧对应的位姿信息这个位姿本质上LIO发布的里程计位姿4.处理回环优化节点发布的优化后位姿的回调函数path_cor_cbk回调获取优化后的位姿。注意这里使用#ifndef进行定义而不是#ifdef很容易看错!!!5.IKD-Tree 重建线程Note:看完回环检测和优化代码再看这个逻辑代码很庞大放到最后介绍。6.载入先验地图multi_session模式中可以直接使用历史地图作为 LTA长期关联的约束对应论文4.1节 LIO内容。逻辑上讲理解先验地图的加载流程前应先了解它是如何保存的这样在阅读 load_prior_map_and_info() 时会更加清晰。保存先验地图的代码在loop_optimization_node.cpp// 函数保存先验地图用于多次会话模式Multisession Mode // 功能将当前会话的关键帧点云和优化位姿保存到磁盘 PCD 文件 void savePrior() // Multisession mode function, for storing first sessions result { std::cout [SavePrior]: save_one_time std::endl; opt_debug_file [SavePrior]: save_one_time std::endl; fullCorrected_p-clear(); fullCorrected_p-points.reserve(200000000); tmp_p-clear(); cloud_ds_p-clear(); // 创建体素滤波器用于下采样 pcl::VoxelGridPointType sor; float leafsize 0.5; sor.setLeafSize(leafsize, leafsize, leafsize); // 遍历所有关键帧 for (int i 0; i keyframes_.size(); i) { auto akeyframe keyframes_[i]; // 如果关键帧优化位姿未设置跳过 if (!akeyframe.pose_opt_set) continue; int cloud_size akeyframe.KeyCloud-size(); if (cloud_size0) continue; try { *fullCorrected_p *(akeyframe.KeyCloud); }catch(std::bad_alloc) { std::cerr std::bad_alloc std::endl; continue; } // 计算位姿优化后点云的新坐标(世界系) if (akeyframe.pose_opt_set) { // 将 KeyPose 和 KeyPoseOpt 转为 GTSAM Pose3 并计算位姿增量 gtsam::Pose3 T1 Pose6DToGTSPose(akeyframe.KeyPose).inverse(); gtsam::Pose3 T2 Pose6DToGTSPose(akeyframe.KeyPoseOpt).inverse(); gtsam::Pose3 _2T1_ T2.between(T1); // _2T1_ T2 * T1.inverse() sor.setInputCloud(akeyframe.KeyCloud); sor.filter(*cloud_ds_p); /* T1 表示T_wl(orign), T2 表示T_wl(opt), Pw(orign) T1 * Pl, Pw(opt) T2 * Pl, 上述左面的式子 左乘 _2T1_ 得到: T2 * T1.inverse() * Pw(orign) T2 * T1.inverse() * T1 * Pl Pw(opt) _2T1_ * Pw(orign) */ pcl::transformPointCloud(*(cloud_ds_p), *tmp_p, GTSPoseToEigenM4f(_2T1_)); // Note: 确认 akeyframe.KeyCloud 点云的坐标系已经从lidar系转到世界系否则上述推导不成立 } // 插入分隔符点用于 load_prior_map_and_info 解析子地图 PointType a_pt; PointType a_pt; a_pt.x -1010.1; // 分隔符标志 a_pt.y i; // 子地图索引 (来自哪个关键帧) a_pt.z tmp_p-size(); // 子地图点云数量 fullCorrected_p-push_back(a_pt); try { if (!tmp_p-empty()) *fullCorrected_p *(tmp_p); // Note:此时存储了优化前和优化后两个点云,使用 a_pt 分割 }catch(std::bad_alloc) { std::cerr std::bad_alloc std::endl ; fullCorrected_p-points.back().z 0; } // 保存优化后的关键帧位姿roll,pitch,yaw a_pt.x akeyframe.KeyPoseOpt.roll; a_pt.y akeyframe.KeyPoseOpt.pitch; a_pt.z akeyframe.KeyPoseOpt.yaw; fullCorrected_p-push_back(a_pt); // 保存优化后的关键帧平移向量 (x,y,z) a_pt.x akeyframe.KeyPoseOpt.x; a_pt.y akeyframe.KeyPoseOpt.y; a_pt.z akeyframe.KeyPoseOpt.z; opt_debug_file KF pose corrected: a_pt.x a_pt.y a_pt.z endl; fullCorrected_p-push_back(a_pt); // 保存原始关键帧位姿旋转 (roll,pitch,yaw) a_pt.x akeyframe.KeyPose.roll; a_pt.y akeyframe.KeyPose.pitch; a_pt.z akeyframe.KeyPose.yaw; fullCorrected_p-push_back(a_pt); // 保存原始关键帧平移向量 (x,y,z) a_pt.x akeyframe.KeyPose.x; a_pt.y akeyframe.KeyPose.y; a_pt.z akeyframe.KeyPose.z; opt_debug_file KF pose original: a_pt.x a_pt.y a_pt.z endl; fullCorrected_p-push_back(a_pt); std::cout [SavePrior]: prepare key frame: i std::endl; opt_debug_file [SavePrior]: prepare key frame: i std::endl; // Note此时存储了优化前和优化后两个点云;优化后的位姿,优化前的位姿 } // 保存因子图中的长跨度约束 gtsam::NonlinearFactorGraph graph_cur isam_-getFactorsUnsafe(); for (int i 0; i graph_cur.size(); i) { if(factor_del.find(i) ! factor_del.end()) continue; // 跳过已经删除的因子 PointType a_pt; auto afactor_keys graph_cur[i]-keys(); // 如果因子连接两个关键帧且跨度大于 2 if (afactor_keys.size() 2) { if (afactor_keys[1] - afactor_keys[0] 2) { a_pt.x -2020.2; // 特殊标志 a_pt.y afactor_keys[0]; // 起始关键帧索引 a_pt.z afactor_keys[1]; // 结束关键帧索引 } } fullCorrected_p-push_back(a_pt); // Note此时存储了优化前和优化后两个点云;优化后的位姿,优化前的位姿;因子图信息 } if (!fullCorrected_p-empty()) { std::string pcd_name save_directory map_prior/clouds_corrected.pcd; opt_debug_file [SavePrior]: try saving prior map. std::endl; pcl::io::savePCDFileBinary(pcd_name, *fullCorrected_p); opt_debug_file [SavePrior]: prior map saved as clouds_corrected.pcd std::endl; } }举个具体例子假设有第 5 个 keyframe原始位姿在 (100, 20, 0)优化后位姿在 (98, 21, 0)降采样并变换后的子图有 3000 个点那它写进 fullCorrected_p 的结构大概是原始 KeyCloud 的所有点(-1010.1, 5, 3000) // 子图标记第5个子图后面跟3000个点[3000 个优化后子图点](roll_opt, pitch_opt, yaw_opt)(98, 21, 0) // 优化后平移(roll_raw, pitch_raw, yaw_raw)(100, 20, 0) // 原始平移如果图里还有一个回环因子连接 5 - 42后面还会多一个特殊点(-2020.2, 5, 42)根据上述代码知道保存的 pcd 点云存储了三个信息优化前和优化后两个点云优化后的位姿优化前的位姿因子图信息同时使用特殊点作为分割标志以便后续加载解析。// 函数:1.加载先验点云地图 priormap(后续发布rviz可视化) // 2.填充 unmap_submap_info保存的是先验关键帧ID(ID 0)及对应的点云和优化前后位姿 // 3.每一帧的 位置 构建先验ikd-tree pos_kdtree_prior void load_prior_map_and_info(PointCloudXYZI::Ptr priormap) { std::cout [LoadPrior]: loading prior map. std::endl; double tload omp_get_wtime(); PointCloudXYZI::Ptr datacloud(new PointCloudXYZI()); std::string pcd_name save_directory map_prior/clouds_corrected.pcd; pcl::io::loadPCDFile(pcd_name, *datacloud); PointCloudXYZI::Ptr tmpmap(new PointCloudXYZI()); for(int j 0; j datacloud-points.size(); j) { auto a_pt datacloud-points[j]; // 检测到地图分隔标志x -1010.1 if(fabs(a_pt.x - (-1010.1)) 0.01) // separation flag { SubmapInfo tmp_submapinfo; // 对应 先验关键帧的ID,为了防止索引重复这里用负数 int tmp_id -(a_pt.y1); int cloud_size a_pt.z; if (cloud_size ! 0) { tmpmap-clear(); // 这里clear 清空了原点云,tmpmap只保存优化后的点云 for (int k 0; k cloud_size; k) { j; tmpmap-push_back(datacloud-points[j]); } } // 下一点存储的是 优化后关键帧旋转roll, pitch, yaw j; a_pt datacloud-points[j]; tmp_submapinfo.corr_pose_rotM ExtraLib::eulToRotM(a_pt.x, a_pt.y, a_pt.z) ; // 下一点存储的是 优化后关键帧平移x, y, z j; a_pt datacloud-points[j]; tmp_submapinfo.corr_pose_tran(0) a_pt.x; tmp_submapinfo.corr_pose_tran(1) a_pt.y; tmp_submapinfo.corr_pose_tran(2) a_pt.z; // 构建用于 KDTree 查询的关键帧位姿点 pcl::PointXYZI a_posi; a_posi.x a_pt.x; a_posi.y a_pt.y; a_posi.z a_pt.z; a_posi.intensity tmp_id; key_poses_prior-push_back(a_posi); tmp_submapinfo.corPoseSet true; // 标记优化位姿已设置 // 解析原始位姿旋转roll, pitch, yaw j; a_pt datacloud-points[j]; tmp_submapinfo.lidar_pose_rotM ExtraLib::eulToRotM(a_pt.x, a_pt.y, a_pt.z); // 解析原始位姿平移x, y, z j; a_pt datacloud-points[j]; tmp_submapinfo.lidar_pose_tran(0) a_pt.x; tmp_submapinfo.lidar_pose_tran(1) a_pt.y; tmp_submapinfo.lidar_pose_tran(2) a_pt.z; tmp_submapinfo.oriPoseSet true; // 标记原始位姿已设置 if (cloud_size ! 0) { *priormap *tmpmap; // 存储的是优化后的点云 tmp_submapinfo.cloud_ontree tmpmap-points; unmap_submap_info[tmp_id] tmp_submapinfo; // unmap_submap_info 保存的是先验关键帧ID及对应的点云和优化前后位姿 } PointCloudXYZI::Ptr empty_cloud (new PointCloudXYZI()); tmpmap empty_cloud; continue; } tmpmap-push_back(a_pt); } std::cout [LoadPrior]: loading prior map done, using 1000*(omp_get_wtime() - tload) ms std::endl; std::cout [LoadPrior]: prior submap number key_poses_prior-size() std::endl; std::cout [LoadPrior]: total prior map size priormap-size() std::endl; pcl::KdTreeFLANNpcl::PointXYZI::Ptr pos_kdtree_tmp(new pcl::KdTreeFLANNpcl::PointXYZI()); pos_kdtree_tmp-setInputCloud(key_poses_prior); pos_kdtree_prior pos_kdtree_tmp-makeShared(); }7. 同步一组lidar和IMU数据sync_packages() 和FAST-LIO2 一致8.对IMU数据进行预处理通过前向传播和反向传播实现点云畸变处理。当订阅到回环检测的话题 /submap_ids 检查到有新的子图关键帧IKD-Tree 重建线程ikdtree_rebuild开始执行当前IMU数据处理进入阻塞状态直到 IKD-Tree 重建完成。9.动态调整地图区域防止地图过大而内存溢出lasermap_fov_segment()10.初始化 ikd-tree变点云坐标从 lidar 系转到世界坐标系构建ikd-tree11.迭代卡尔曼滤波更新12.发布里程计回环检测订阅可能是 FAST-LIO2 代码版本的原因吧最新的代码是以 lidar采样结束时刻 作为里程计的时间戳。13.根据最新估计位姿增量添加点云到mapmap_incremental()14.每帧点云转换到世界系发布回环检测订阅。
LTA-OM（二）代码：LIO
发布时间：2026/7/30 11:37:41
代码launch将多个node加载到同一个进程中内存共享零拷贝。类似于ROS2中的component。1.全局参数设置multisession_mode是否启用多会话模式SaveDir日志与地图保存路径2.Nodelet Manager 启动创建一个名为sample_nodelet_manager的宿主进程所有 nodelet 插件LIO、回环检测会在这个进程中运行3.FAST-LIO2 Nodelet将fastlio_plugin加载到 sample_nodelet_manager 进程中执行然后加载 FAST-LIO2 的配置参数4.回环检测 Nodeletloop_detection_plugin也被加载到 Nodelet Manager 内启动前延迟 1 秒避免和 FAST-LIO2 同时启动导致 Eigen 内存对齐问题5.回环优化 Node作者在README提到过回环优化节点无法作为 nodelet 正常触发因此回环优化作为 node 启动。主函数实例化 Nodelet 对象时被调用 fastlio_plugin 的构造函数Nodelet Manager 调用 load 插件后调用 onInit()函数执行线程mainLIOFunction(等效理解为node的主函数)。回环检测同样的逻辑。1.初始化扩展IKFoM滤波器和FAST-LIO2 一致2.点云回调函数Livox激光雷达点云处理的时候会对点云中的 tag 字段进行筛选如果使用的是MID-360需要取消注释后半段代码# preprocess.cpp avia_handler() if((msg-points[i].line N_SCANS) ((msg-points[i].tag 0x30) 0x10) //|| (msg-points[i].tag 0x30) 0x00 //mid3603.处理回环检测节点发布消息的回调函数submap_id_cbk获取回环检测节点发布的 关键帧ID知道回环检测处理到哪个关键帧了关键帧是由多帧累计的。然后获取 map_incremental() 里逐步 push_back 累计的点作为对应的点云。填充unmap_submap_info 普通模式中unmap_submap_info保存的是回环检测发布的关键帧ID(ID 0)及对应的点云multi_session模式还会保存的是先验关键帧ID(ID 0)及对应的点云和优化前后位姿submap_pose_cbk获取该关键帧对应的位姿信息这个位姿本质上LIO发布的里程计位姿4.处理回环优化节点发布的优化后位姿的回调函数path_cor_cbk回调获取优化后的位姿。注意这里使用#ifndef进行定义而不是#ifdef很容易看错!!!5.IKD-Tree 重建线程Note:看完回环检测和优化代码再看这个逻辑代码很庞大放到最后介绍。6.载入先验地图multi_session模式中可以直接使用历史地图作为 LTA长期关联的约束对应论文4.1节 LIO内容。逻辑上讲理解先验地图的加载流程前应先了解它是如何保存的这样在阅读 load_prior_map_and_info() 时会更加清晰。保存先验地图的代码在loop_optimization_node.cpp// 函数保存先验地图用于多次会话模式Multisession Mode // 功能将当前会话的关键帧点云和优化位姿保存到磁盘 PCD 文件 void savePrior() // Multisession mode function, for storing first sessions result { std::cout [SavePrior]: save_one_time std::endl; opt_debug_file [SavePrior]: save_one_time std::endl; fullCorrected_p-clear(); fullCorrected_p-points.reserve(200000000); tmp_p-clear(); cloud_ds_p-clear(); // 创建体素滤波器用于下采样 pcl::VoxelGridPointType sor; float leafsize 0.5; sor.setLeafSize(leafsize, leafsize, leafsize); // 遍历所有关键帧 for (int i 0; i keyframes_.size(); i) { auto akeyframe keyframes_[i]; // 如果关键帧优化位姿未设置跳过 if (!akeyframe.pose_opt_set) continue; int cloud_size akeyframe.KeyCloud-size(); if (cloud_size0) continue; try { *fullCorrected_p *(akeyframe.KeyCloud); }catch(std::bad_alloc) { std::cerr std::bad_alloc std::endl; continue; } // 计算位姿优化后点云的新坐标(世界系) if (akeyframe.pose_opt_set) { // 将 KeyPose 和 KeyPoseOpt 转为 GTSAM Pose3 并计算位姿增量 gtsam::Pose3 T1 Pose6DToGTSPose(akeyframe.KeyPose).inverse(); gtsam::Pose3 T2 Pose6DToGTSPose(akeyframe.KeyPoseOpt).inverse(); gtsam::Pose3 _2T1_ T2.between(T1); // _2T1_ T2 * T1.inverse() sor.setInputCloud(akeyframe.KeyCloud); sor.filter(*cloud_ds_p); /* T1 表示T_wl(orign), T2 表示T_wl(opt), Pw(orign) T1 * Pl, Pw(opt) T2 * Pl, 上述左面的式子 左乘 _2T1_ 得到: T2 * T1.inverse() * Pw(orign) T2 * T1.inverse() * T1 * Pl Pw(opt) _2T1_ * Pw(orign) */ pcl::transformPointCloud(*(cloud_ds_p), *tmp_p, GTSPoseToEigenM4f(_2T1_)); // Note: 确认 akeyframe.KeyCloud 点云的坐标系已经从lidar系转到世界系否则上述推导不成立 } // 插入分隔符点用于 load_prior_map_and_info 解析子地图 PointType a_pt; PointType a_pt; a_pt.x -1010.1; // 分隔符标志 a_pt.y i; // 子地图索引 (来自哪个关键帧) a_pt.z tmp_p-size(); // 子地图点云数量 fullCorrected_p-push_back(a_pt); try { if (!tmp_p-empty()) *fullCorrected_p *(tmp_p); // Note:此时存储了优化前和优化后两个点云,使用 a_pt 分割 }catch(std::bad_alloc) { std::cerr std::bad_alloc std::endl ; fullCorrected_p-points.back().z 0; } // 保存优化后的关键帧位姿roll,pitch,yaw a_pt.x akeyframe.KeyPoseOpt.roll; a_pt.y akeyframe.KeyPoseOpt.pitch; a_pt.z akeyframe.KeyPoseOpt.yaw; fullCorrected_p-push_back(a_pt); // 保存优化后的关键帧平移向量 (x,y,z) a_pt.x akeyframe.KeyPoseOpt.x; a_pt.y akeyframe.KeyPoseOpt.y; a_pt.z akeyframe.KeyPoseOpt.z; opt_debug_file KF pose corrected: a_pt.x a_pt.y a_pt.z endl; fullCorrected_p-push_back(a_pt); // 保存原始关键帧位姿旋转 (roll,pitch,yaw) a_pt.x akeyframe.KeyPose.roll; a_pt.y akeyframe.KeyPose.pitch; a_pt.z akeyframe.KeyPose.yaw; fullCorrected_p-push_back(a_pt); // 保存原始关键帧平移向量 (x,y,z) a_pt.x akeyframe.KeyPose.x; a_pt.y akeyframe.KeyPose.y; a_pt.z akeyframe.KeyPose.z; opt_debug_file KF pose original: a_pt.x a_pt.y a_pt.z endl; fullCorrected_p-push_back(a_pt); std::cout [SavePrior]: prepare key frame: i std::endl; opt_debug_file [SavePrior]: prepare key frame: i std::endl; // Note此时存储了优化前和优化后两个点云;优化后的位姿,优化前的位姿 } // 保存因子图中的长跨度约束 gtsam::NonlinearFactorGraph graph_cur isam_-getFactorsUnsafe(); for (int i 0; i graph_cur.size(); i) { if(factor_del.find(i) ! factor_del.end()) continue; // 跳过已经删除的因子 PointType a_pt; auto afactor_keys graph_cur[i]-keys(); // 如果因子连接两个关键帧且跨度大于 2 if (afactor_keys.size() 2) { if (afactor_keys[1] - afactor_keys[0] 2) { a_pt.x -2020.2; // 特殊标志 a_pt.y afactor_keys[0]; // 起始关键帧索引 a_pt.z afactor_keys[1]; // 结束关键帧索引 } } fullCorrected_p-push_back(a_pt); // Note此时存储了优化前和优化后两个点云;优化后的位姿,优化前的位姿;因子图信息 } if (!fullCorrected_p-empty()) { std::string pcd_name save_directory map_prior/clouds_corrected.pcd; opt_debug_file [SavePrior]: try saving prior map. std::endl; pcl::io::savePCDFileBinary(pcd_name, *fullCorrected_p); opt_debug_file [SavePrior]: prior map saved as clouds_corrected.pcd std::endl; } }举个具体例子假设有第 5 个 keyframe原始位姿在 (100, 20, 0)优化后位姿在 (98, 21, 0)降采样并变换后的子图有 3000 个点那它写进 fullCorrected_p 的结构大概是原始 KeyCloud 的所有点(-1010.1, 5, 3000) // 子图标记第5个子图后面跟3000个点[3000 个优化后子图点](roll_opt, pitch_opt, yaw_opt)(98, 21, 0) // 优化后平移(roll_raw, pitch_raw, yaw_raw)(100, 20, 0) // 原始平移如果图里还有一个回环因子连接 5 - 42后面还会多一个特殊点(-2020.2, 5, 42)根据上述代码知道保存的 pcd 点云存储了三个信息优化前和优化后两个点云优化后的位姿优化前的位姿因子图信息同时使用特殊点作为分割标志以便后续加载解析。// 函数:1.加载先验点云地图 priormap(后续发布rviz可视化) // 2.填充 unmap_submap_info保存的是先验关键帧ID(ID 0)及对应的点云和优化前后位姿 // 3.每一帧的 位置 构建先验ikd-tree pos_kdtree_prior void load_prior_map_and_info(PointCloudXYZI::Ptr priormap) { std::cout [LoadPrior]: loading prior map. std::endl; double tload omp_get_wtime(); PointCloudXYZI::Ptr datacloud(new PointCloudXYZI()); std::string pcd_name save_directory map_prior/clouds_corrected.pcd; pcl::io::loadPCDFile(pcd_name, *datacloud); PointCloudXYZI::Ptr tmpmap(new PointCloudXYZI()); for(int j 0; j datacloud-points.size(); j) { auto a_pt datacloud-points[j]; // 检测到地图分隔标志x -1010.1 if(fabs(a_pt.x - (-1010.1)) 0.01) // separation flag { SubmapInfo tmp_submapinfo; // 对应 先验关键帧的ID,为了防止索引重复这里用负数 int tmp_id -(a_pt.y1); int cloud_size a_pt.z; if (cloud_size ! 0) { tmpmap-clear(); // 这里clear 清空了原点云,tmpmap只保存优化后的点云 for (int k 0; k cloud_size; k) { j; tmpmap-push_back(datacloud-points[j]); } } // 下一点存储的是 优化后关键帧旋转roll, pitch, yaw j; a_pt datacloud-points[j]; tmp_submapinfo.corr_pose_rotM ExtraLib::eulToRotM(a_pt.x, a_pt.y, a_pt.z) ; // 下一点存储的是 优化后关键帧平移x, y, z j; a_pt datacloud-points[j]; tmp_submapinfo.corr_pose_tran(0) a_pt.x; tmp_submapinfo.corr_pose_tran(1) a_pt.y; tmp_submapinfo.corr_pose_tran(2) a_pt.z; // 构建用于 KDTree 查询的关键帧位姿点 pcl::PointXYZI a_posi; a_posi.x a_pt.x; a_posi.y a_pt.y; a_posi.z a_pt.z; a_posi.intensity tmp_id; key_poses_prior-push_back(a_posi); tmp_submapinfo.corPoseSet true; // 标记优化位姿已设置 // 解析原始位姿旋转roll, pitch, yaw j; a_pt datacloud-points[j]; tmp_submapinfo.lidar_pose_rotM ExtraLib::eulToRotM(a_pt.x, a_pt.y, a_pt.z); // 解析原始位姿平移x, y, z j; a_pt datacloud-points[j]; tmp_submapinfo.lidar_pose_tran(0) a_pt.x; tmp_submapinfo.lidar_pose_tran(1) a_pt.y; tmp_submapinfo.lidar_pose_tran(2) a_pt.z; tmp_submapinfo.oriPoseSet true; // 标记原始位姿已设置 if (cloud_size ! 0) { *priormap *tmpmap; // 存储的是优化后的点云 tmp_submapinfo.cloud_ontree tmpmap-points; unmap_submap_info[tmp_id] tmp_submapinfo; // unmap_submap_info 保存的是先验关键帧ID及对应的点云和优化前后位姿 } PointCloudXYZI::Ptr empty_cloud (new PointCloudXYZI()); tmpmap empty_cloud; continue; } tmpmap-push_back(a_pt); } std::cout [LoadPrior]: loading prior map done, using 1000*(omp_get_wtime() - tload) ms std::endl; std::cout [LoadPrior]: prior submap number key_poses_prior-size() std::endl; std::cout [LoadPrior]: total prior map size priormap-size() std::endl; pcl::KdTreeFLANNpcl::PointXYZI::Ptr pos_kdtree_tmp(new pcl::KdTreeFLANNpcl::PointXYZI()); pos_kdtree_tmp-setInputCloud(key_poses_prior); pos_kdtree_prior pos_kdtree_tmp-makeShared(); }7. 同步一组lidar和IMU数据sync_packages() 和FAST-LIO2 一致8.对IMU数据进行预处理通过前向传播和反向传播实现点云畸变处理。当订阅到回环检测的话题 /submap_ids 检查到有新的子图关键帧IKD-Tree 重建线程ikdtree_rebuild开始执行当前IMU数据处理进入阻塞状态直到 IKD-Tree 重建完成。9.动态调整地图区域防止地图过大而内存溢出lasermap_fov_segment()10.初始化 ikd-tree变点云坐标从 lidar 系转到世界坐标系构建ikd-tree11.迭代卡尔曼滤波更新12.发布里程计回环检测订阅可能是 FAST-LIO2 代码版本的原因吧最新的代码是以 lidar采样结束时刻 作为里程计的时间戳。13.根据最新估计位姿增量添加点云到mapmap_incremental()14.每帧点云转换到世界系发布回环检测订阅。
