LTA-OM（三）代码：回环检测

张

张建站

2026/8/12 21:05:43

10分钟阅读

目录代码1.读取参数2.订阅话题3.载入先验 STD 描述子3.1 保存 STD 描述子代码3.2 加载先验 STD描述子代码4.子图累计关键帧5.关键帧处理5.1生成描述子5.2回环检测5.3更新STD描述子数据库代码1.读取参数读取 config中的参数2.订阅话题订阅 LIO 发布的世界系Lidar点云数据放到buffer中订阅 LIO 发布的里程计话题里程计数据放到buffer中订阅 LIO 发布的 /odom_correction_infoLIO中的线程每次需要重建的时候发布消息回环检测停止生成新的关键帧3.载入先验 STD 描述子先知道STD 三角形描述子是什么在STD 代码中三角形描述子的定义为// Structure for Stabel Triangle Descriptor typedef struct STDesc { // the side lengths of STDesc, arranged from short to long // 描述子的三条边的长度从短到长排列 Eigen::Vector3d side_length_; // projection angle between vertices Eigen::Vector3d angle_; Eigen::Vector3d center_; // 三角形重心 unsigned int frame_id_; // 描述子所在帧的ID // three vertexs // 三个顶点 Eigen::Vector3d vertex_A_; Eigen::Vector3d vertex_B_; Eigen::Vector3d vertex_C_; Eigen::Vector3d vertex_attached_; } STDesc;但是在 LTA-OM中三角形描述子的定义为typedef struct STD { Eigen::Vector3d triangle_; Eigen::Vector3d angle_; Eigen::Vector3d center_; unsigned short frame_number_; // std::vectorunsigned short score_frame_; // std::vectorEigen::Matrix3d position_list_; BinaryDescriptor binary_A_; BinaryDescriptor binary_B_; BinaryDescriptor binary_C_; } STD;但这个实际上是BTC在三角形描述子的基础上增加了二进制描述子代码中的定义。为了不和论文冲突认为是 STD吧multi_session模式需要载入 prior map、prior key poses、prior STD database三部分数据LIO中已经加载了前两个这里需要加载第三个。和 LIO 中载入先验地图一样先看保存 STD 描述子的代码。3.1 保存 STD 描述子代码保存每个关键帧的 STD描述子save_descriptor()void save_descriptor(const std::vectorSTD current_descriptor, pcl::PointCloudpcl::PointXYZ::Ptr cloud_to_store) { for (auto descrip : current_descriptor) { pcl::PointXYZ pt_tmp; // 分割符,但是 y 表示关键帧信息 pt_tmp.x -1010.1; pt_tmp.y descrip.frame_number_; pt_tmp.z 0; //-1010.1 index denote a new descriptor cloud_to_store-points.push_back(pt_tmp); // 三角形的边长 pt_tmp.x descrip.triangle_[0]; pt_tmp.y descrip.triangle_[1]; pt_tmp.z descrip.triangle_[2]; cloud_to_store-points.push_back(pt_tmp); // 三角形的角度(实际上不是) pt_tmp.x descrip.angle_[0]; pt_tmp.y descrip.angle_[1]; pt_tmp.z descrip.angle_[2]; cloud_to_store-points.push_back(pt_tmp); // 三角形的质心 pt_tmp.x descrip.center_[0]; pt_tmp.y descrip.center_[1]; pt_tmp.z descrip.center_[2]; cloud_to_store-points.push_back(pt_tmp); // 二进制描述子对应的三维位置 pt_tmp.x descrip.binary_A_.location_[0]; pt_tmp.y descrip.binary_A_.location_[1]; pt_tmp.z descrip.binary_A_.location_[2]; cloud_to_store-points.push_back(pt_tmp); pt_tmp.x descrip.binary_B_.location_[0]; pt_tmp.y descrip.binary_B_.location_[1]; pt_tmp.z descrip.binary_B_.location_[2]; cloud_to_store-points.push_back(pt_tmp); pt_tmp.x descrip.binary_C_.location_[0]; pt_tmp.y descrip.binary_C_.location_[1]; pt_tmp.z descrip.binary_C_.location_[2]; cloud_to_store-points.push_back(pt_tmp); // 二进制描述子被占用的高度层数occupy_array_中true的数量 pt_tmp.x float(descrip.binary_A_.summary_); pt_tmp.y float(descrip.binary_B_.summary_); pt_tmp.z float(descrip.binary_C_.summary_); cloud_to_store-points.push_back(pt_tmp); // 二进制描述子中的关键点的二进制字符串该像素上方各高度层是否有点 pt_tmp.x descrip.binary_A_.occupy_array_.size(); pt_tmp.y descrip.binary_B_.occupy_array_.size(); pt_tmp.z descrip.binary_C_.occupy_array_.size(); cloud_to_store-points.push_back(pt_tmp); // 每个 STD 描述子写入点云按顺序 // 分隔符点 // triangle, angle, center // 三个 BinaryDescriptor 的位置 // summary // occupy_array_ // std::cout occupy_array size: pt_tmp.x , pt_tmp.y , pt_tmp.z std::endl; // 将 occupy_array_ 每三个 bool 打包为一个 PointXYZ 保存,不足三个补 -100 save_an_array(descrip.binary_A_, cloud_to_store); save_an_array(descrip.binary_B_, cloud_to_store); save_an_array(descrip.binary_C_, cloud_to_store); } }3.2 加载先验 STD描述子代码load_descriptor()void load_descriptor(std::unordered_mapSTD_LOC, std::vectorSTD std_map, const pcl::PointCloudpcl::PointXYZ::Ptr cloud_to_load, int index_max) { if (cloud_to_load-points.size() 0) return; STD descrip; int occupy_array_size 47; // 硬编码 // occupy_array_是由0和1组成的47位二进制字符串 descrip.binary_A_.occupy_array_.resize(occupy_array_size); descrip.binary_B_.occupy_array_.resize(occupy_array_size); descrip.binary_C_.occupy_array_.resize(occupy_array_size); size_t i 0; std_map.reserve(1000000); while(i cloud_to_load-points.size()){ if( fabs(cloud_to_load-points[i].x - (-1010.1)) 0.01){ descrip.frame_number_ cloud_to_load-points[i].y; // 当前描述子对应的关键帧ID index_max descrip.frame_number_ index_max ? descrip.frame_number_ : index_max; i; // triangle, angle, center descrip.triangle_[0] cloud_to_load-points[i].x; descrip.triangle_[1] cloud_to_load-points[i].y; descrip.triangle_[2] cloud_to_load-points[i].z; i; descrip.angle_[0] cloud_to_load-points[i].x; descrip.angle_[1] cloud_to_load-points[i].y; descrip.angle_[2] cloud_to_load-points[i].z; i; descrip.center_[0] cloud_to_load-points[i].x; descrip.center_[1] cloud_to_load-points[i].y; descrip.center_[2] cloud_to_load-points[i].z; i; // A/B/C binary descriptor 位置 descrip.binary_A_.location_[0] cloud_to_load-points[i].x; descrip.binary_A_.location_[1] cloud_to_load-points[i].y; descrip.binary_A_.location_[2] cloud_to_load-points[i].z; i; descrip.binary_B_.location_[0] cloud_to_load-points[i].x; descrip.binary_B_.location_[1] cloud_to_load-points[i].y; descrip.binary_B_.location_[2] cloud_to_load-points[i].z; i; descrip.binary_C_.location_[0] cloud_to_load-points[i].x; descrip.binary_C_.location_[1] cloud_to_load-points[i].y; descrip.binary_C_.location_[2] cloud_to_load-points[i].z; i; // summary descrip.binary_A_.summary_ cloud_to_load-points[i].x; descrip.binary_B_.summary_ cloud_to_load-points[i].y; descrip.binary_C_.summary_ cloud_to_load-points[i].z; i; // occupy_array 的大小每三个点存储一组 int bin_A_size ceil(cloud_to_load-points[i].x/3); int bin_B_size ceil(cloud_to_load-points[i].y/3); int bin_C_size ceil(cloud_to_load-points[i].z/3); i; load_array(descrip.binary_A_.occupy_array_, cloud_to_load, i); ibin_A_size; load_array(descrip.binary_B_.occupy_array_, cloud_to_load, i); ibin_B_size; load_array(descrip.binary_C_.occupy_array_, cloud_to_load, i); ibin_C_size; // 注意是整形,表示hash key, // STD_LOC可以理解为这个三角形的大概长成什么样,这样相似的三角形可以映射到同一个 STD_LOC STD_LOC position; position.x (int)(descrip.triangle_[0] 0.5); position.y (int)(descrip.triangle_[1] 0.5); position.z (int)(descrip.triangle_[2] 0.5); position.a (int)(descrip.angle_[0]); position.b (int)(descrip.angle_[1]); position.c (int)(descrip.angle_[2]); auto iter std_map.find(position); if (iter ! std_map.end()) { std_map[position].push_back(descrip); } else { std::vectorSTD descriptor_list; descriptor_list.reserve(100); descriptor_list.push_back(descrip); std_map[position] descriptor_list; // std::cout std_map.size() ; } } } }保存和加载历史平面点云代码相对简单很多略去。4.子图累计关键帧通过子图点云的时间戳找到对应的里程计数据对子图点云进行体素降采样累计多个子图的点前后两个子图的平移和旋转差需要满足条件代码中有很多硬编码后续改为参数读取10个子图构成关键帧5.关键帧处理发布关键帧idLIO中会订阅这个话题从第二个关键帧开始IKD-Tree 重建线程开始执行5.1生成描述子对输入的关键帧点云进行体素化然后基于体素信息对点云进行平面检测init_voxel_map()从体素地图中提取所有已检测到的平面并将这些平面信息包括中心点和法线存储到plane_cloud中get_plane()将多个平面通过相似性合并成一个更大的平面get_project_plane()参考平面生成merge_plane()生成二进制描述子binary_extractor()生成三角形描述子generate_std()在连续关键帧中找到空间上足够接近的角点对associate_consecutive_frames()将连续关键帧角点匹配对打包成点云并发布 ROS 消息用于可视化或调试角点匹配的关键帧关系回环优化会订阅这个话题对当前关键帧角点与上上帧角点做匹配并发布但是回环优化处理这部分的代码注释掉了发布关键帧相关的位姿话题最后一个子图的位姿作为关键的位姿关键帧id组成这个关键帧的头尾子图时间戳LIO和回环优化订阅这个话题发布当前关键帧点云话题回环优化订阅发布当前关键帧的角点在 RViz 中显示每个二进制描述子对应的角点位置并用数字表示该点的高度占用数 summary_5.2回环检测first_session当前关键帧的描述子与历史关键帧描述子进行匹配检测回环second_session当前关键帧的描述子与先验地图的描述子匹配检测回环具体而言当前帧的STD描述子和数据库中的进行匹配挑出匹配次数较多的历史帧作为回环候选帧遍历所有候选帧逐个进行几何验证RANSAC ICP找出最优的作为回环帧计算 STD描述子的平均欧式距离和二进制描述子的相似度发布当前帧和回环帧的位姿信息以及对应的ID回环优化订阅发布当前帧和回环帧的角点回环优化订阅5.3更新STD描述子数据库将当前关键帧提取到的所有 STD 描述子加入STD数据库中