多传感器融合框架搭建

1. src/include
   • apps：节点文件
     • front_end_node.cpp:前端节点
     • data_pretreat_node.cpp:节点的数据预处理
     • back_end_node.cpp:后端节点
     • loop_closing_node.cpp:回环检测节点
   • data_pretreat：数据预处理节点相关代码
   • back_end_node：与后端节点相关的代码
   • loop_closing_node：回环检测节点相关代码
   • models：框架主模块
   • publisher:发布者
   • subscriber：订阅者
   • tools:框架具有小功能，如保存轨迹txt文件
2. slam_data 数据保存目录
   • key_frames:关键帧点云
   • map.pcd:全局点云
   • trajectory：真实轨迹和估计轨迹
   • scan context：回环检测数据
     • index.bin：scan context索引
     • scan_contexts.proto：scan context数据
     • ring_keys.proto：ring key数据
     • key_frames.proto，关键帧数据
3. cmake ros第三方库
4. config 配置文件
5. Log 日志输出
6. srv 服务

其他的略

打开lidar_localization/config/scan_context输入以下命令生成文件夹pb文件

protoc --cpp_out=./ key_frames.proto protoc --cpp_out=./ ring_keys.proto protoc --cpp_out=./ scan_contexts.proto mv key_frames.pb.cc key_frames.pb.cpp mv ring_keys.pb.cc ring_keys.pb.cpp mv scan_contexts.pb.cc scan_contexts.pb.cpp 

修改生成的三个.pb.cpp文件如下，以下ring_keys.pb.cpp为例。

// Generated by the protocol buffer compiler.  DO NOT EDIT! // source: ring_keys.proto  #define INTERNAL_SUPPRESS_PROTOBUF_FIELD_DEPRECATION #include "ring_keys.pb.h" 替换为  #include "lidar_localization/models/scan_context_manager/ring_keys.pb.h"  #include <algorithm> 

之后，使用上述步骤生成.pb.h文件替换lidar_localization/include/lidar_localization/models/scan_context_manager 同名文件。 将.pb.cpp文件替换lidar_localization/src/models/scan_context_manager中的同名文件。

这样编译就没问题了。

1. 订阅原始gnss数据，imu数据，点云数据
2. 得到gnss里程计(真值)并对点云进行畸变补偿

front_end.yaml 中 whether_use_back_end 设置为NO

1. 订阅gnss里程计和变形补偿后的点云
2. 通过前端算法配准点云获得lidar里程计，与gnss里程计一起发布，可以做evo评测

front_end.yaml 中 whether_use_back_end 设置为YES

1. 订阅畸变补偿后的点云
2. 通过前端算法配准获得lidar里程计，发布lidar里程计

服务：保存前端地图

rosservice call /save_front_end_map  

1. 订阅畸变补偿后的点云，lidar里程计，gnss里程计、回环检测节点的回环因子

2. 关键帧是否距离上一帧位移足够远，gnss关键帧，关键帧点云(这里的关键帧只包括pose、id索引、时间戳、点云分开存储)

   选择关键帧的方法大致分为三种

   1. 距离上一个关键帧的帧数是否足够，缺点明显。当运动缓慢时，会产生大量冗余关键帧，运动速度快，失去重要关键帧
   2. 上述问题可以通过远离上一个关键帧的位移来解决。然而，当来回扫描同一物体时，也会产生大量冗余关键帧
   3. 上一个关键帧的共视点上述问题。

   展望:目前只使用第二种方法，后期可以改进

3. 将新的关键帧顶点添加到图形优化器以及上一个关键帧的边缘gnss作为观测的先验一元边的位置

4. 根据图优化器中的关键帧顶点数，gnss判断是否需要优化因子数量和回环因子数量。如果需要优化，则在优化后保存更新位置数据

服务：后端轨迹数据保存

rosservice call /save_back_end_pose 

1. 订阅后端发布的关键帧，gnss关键帧，关键帧点云

2. 将当前关键帧转换为scan context和ring key，加入回环系统

3. 从上一个关键帧之间的关键帧数量足够多开始回环检测

4. 先通过ring key获得回环关键帧的候选人，然后计算候选人sector key获得准确的匹配方案

5. 匹配候选人和当前关键帧scan context，利用余弦距离获得最高相似度的最佳候选人： d ( I q , I c ) = 1 N s ∑ j = 1 N s ( 1 ? c j q ? c j c ∥ c j q ∥ ∥ c j c ∥ ) (5) d\left(I^{q}, I^{c}\right)=\frac{1}{N_{s}} \sum_{j=1}^{N_{s}}\left(1-\frac{c_{j}^{q} \cdot c_{j}^{c}}{\left\|c_{j}^{q}\right\|\left\|c_{j}^{c}\right\|}\right)\tag{5} d(Iq,Ic)=Ns​1​j=1∑Ns​​(1−∥∥​cjq​∥∥​∥∥​cjc​∥∥​cjq​⋅cjc​​)(5)

   参考我之前文章：

   https://blog.csdn.net/weixin_44035919/article/details/124926041

6. 验证1：回环因子的两帧的id得到gnss位置误差小于阈值

7. 点云配准优化回环因子的位姿：当前帧和最佳候选者的局部地图配准

8. 验证2：返回点云配准的可靠得分大于阈值，最终全部通过即可发布回环因子

服务：回环数据保存

rosservice call /save_scan_context

这个比较简单直观，略。

分段误差：

• 最大误差：3.597209 %
• 平均误差：1.864868%
• 最小误差： 0.883433%
• RMSE：1.993375%

整体误差：

• 最大误差：0.549552m
• 平均误差：0.095091m
• RMSE：0.132866m

平均绝对轨迹误差只有0.132866m，相比于前端有了很大的改进。

只说一下目前想到的待改进之处吧