添加闭环检测

.vscode/settings.json

@@ -69,6 +69,7 @@
         "valarray": "cpp",
         "*.ipp": "cpp",
         "string": "cpp",
-        "random": "cpp"
+        "random": "cpp",
+        "iterator": "cpp"
     }
 }

lidar_localization/CMakeLists.txt

@@ -55,6 +55,7 @@ add_executable(data_pretreat_node src/apps/data_pretreat_node.cpp ${ALL_SRCS})
 add_dependencies(data_pretreat_node ${catkin_EXPORTED_TARGETS})
 target_link_libraries(data_pretreat_node ${catkin_LIBRARIES} ${ALL_TARGET_LIBRARIES})
 
+
 add_executable(front_end_node src/apps/front_end_node.cpp ${ALL_SRCS})
 add_dependencies(front_end_node ${catkin_EXPORTED_TARGETS})
 target_link_libraries(front_end_node ${catkin_LIBRARIES} ${ALL_TARGET_LIBRARIES})
@@ -63,6 +64,10 @@ add_executable(back_end_node src/apps/back_end_node.cpp ${ALL_SRCS})
 add_dependencies(back_end_node ${catkin_EXPORTED_TARGETS} optimizeMap_gencpp)
 target_link_libraries(back_end_node ${catkin_LIBRARIES} ${ALL_TARGET_LIBRARIES})
 
+add_executable(loop_closing_node src/apps/loop_closing_node.cpp ${ALL_SRCS})
+add_dependencies(loop_closing_node ${catkin_EXPORTED_TARGETS})
+target_link_libraries(loop_closing_node ${catkin_LIBRARIES} ${ALL_TARGET_LIBRARIES})
+
 add_executable(viewer_node src/apps/viewer_node.cpp ${ALL_SRCS})
 add_dependencies(viewer_node ${catkin_EXPORTED_TARGETS} saveMap_gencpp)
 target_link_libraries(viewer_node ${catkin_LIBRARIES} ${ALL_TARGET_LIBRARIES})

lidar_localization/Log/front_end_node.ERROR

@@ -0,0 +1 @@
+front_end_node.rq.rq.log.ERROR.20200303-182534.3549

lidar_localization/config/back_end/config.yaml → lidar_localization/config/mapping/back_end.yaml

@@ -7,13 +7,13 @@ key_frame_distance: 2.0 # 关键帧距离
 graph_optimizer_type: g2o # 图优化库，目前支持g2o
 
 use_gnss: true
-use_loop_close: false
+use_loop_close: true
 
 optimize_step_with_key_frame: 10000 # 没有其他信息时，每隔 step 发送一次优化的位姿
 optimize_step_with_gnss: 950 # 每累计 step 个 gnss 观测时，优化一次
 optimize_step_with_loop: 100  # 每累计 step 个闭环约束时优化一次
 
 g2o_param:
     odom_edge_noise: [0.5, 0.5, 0.5, 0.001, 0.001, 0.001] # 噪声：x y z yaw roll pitch
-    close_loop_noise: [2.0, 2.0, 2.0, 0.01, 0.01, 0.01] # 噪声：x y z yaw roll pitch
+    close_loop_noise: [0.3, 0.3, 0.3, 0.001, 0.001, 0.001] # 噪声：x y z yaw roll pitch
     gnss_noise: [2.0, 2.0, 2.0] # 噪声：x y z

lidar_localization/config/front_end/config.yaml → lidar_localization/config/mapping/front_end.yaml

@@ -1,13 +1,17 @@
 # 匹配
 registration_method: NDT   # 选择点云匹配方法，目前支持：NDT 
 
+
+# 当前帧
+# no_filter指不对点云滤波，在匹配中，理论上点云越稠密，精度越高，但是速度也越慢
+# 所以提供这种不滤波的模式做为对比，以方便使用者去体会精度和效率随稠密度的变化关系
+frame_filter: voxel_filter # 选择当前帧点云滤波方法，目前支持：voxel_filter、no_filter
+
 # 局部地图
 key_frame_distance: 2.0 # 关键帧距离
 local_frame_num: 20
-local_map_filter: voxel_filter # 选择滑窗地图点云滤波方法，目前支持：voxel_filter
+local_map_filter: voxel_filter # 选择滑窗地图点云滤波方法，目前支持：voxel_filter、no_filter
 
-# 当前帧
-frame_filter: voxel_filter # 选择当前帧点云滤波方法，目前支持：voxel_filter
 
 # 各配置选项对应参数
 ## 匹配相关参数

lidar_localization/config/mapping/loop_closing.yaml

@@ -0,0 +1,28 @@
+data_path: ./   # 数据存放路径
+
+registration_method: NDT   # 选择点云匹配方法，目前支持：NDT 
+# 匹配时为了精度更高，应该选用scan-to-map的方式
+# map是以历史帧为中心，往前后时刻各选取extend_frame_num个关键帧，放在一起拼接成的
+extend_frame_num: 5 
+loop_step: 5 # 防止检测过于频繁，每隔loop_step个关键帧检测一次闭环
+detect_area: 10.0 # 检测区域，只有两帧距离小于这个值，才做闭环匹配
+diff_num: 100 # 过于小的闭环没有意义，所以只有两帧之间的关键帧个数超出这个值再做检测
+fitness_score_limit: 0.2 # 匹配误差小于这个值才认为是有效的
+
+# 之所以要提供no_filter（即不滤波）模式，是因为闭环检测对计算时间要求没那么高，而点云越稠密，精度就越高，所以滤波与否都有道理
+map_filter: voxel_filter # 选择滑窗地图点云滤波方法，目前支持：voxel_filter、no_filter
+scan_filter: voxel_filter # 选择当前帧点云滤波方法，目前支持：voxel_filter、no_filter
+
+# 各配置选项对应参数
+## 匹配相关参数
+NDT:
+    res : 1.0
+    step_size : 0.1
+    trans_eps : 0.01
+    max_iter : 30
+## 滤波相关参数
+voxel_filter:
+    map:
+        leaf_size: [0.3, 0.3, 0.3]
+    scan:
+        leaf_size: [0.3, 0.3, 0.3]

lidar_localization/include/lidar_localization/mapping/back_end/back_end.hpp

@@ -14,6 +14,7 @@
 #include "lidar_localization/sensor_data/cloud_data.hpp"
 #include "lidar_localization/sensor_data/pose_data.hpp"
 #include "lidar_localization/sensor_data/key_frame.hpp"
+#include "lidar_localization/sensor_data/loop_pose.hpp"
 
 #include "lidar_localization/models/graph_optimizer/g2o/g2o_graph_optimizer.hpp"
 
@@ -23,51 +24,57 @@ class BackEnd {
     BackEnd();
 
     bool Update(const CloudData& cloud_data, const PoseData& laser_odom, const PoseData& gnss_pose);
-
+    bool InsertLoopPose(const LoopPose& loop_pose);
     bool ForceOptimize();
+
     void GetOptimizedKeyFrames(std::deque<KeyFrame>& key_frames_deque);
     bool HasNewKeyFrame();
     bool HasNewOptimized();
     void GetLatestKeyFrame(KeyFrame& key_frame);
-
+    void GetLatestKeyGNSS(KeyFrame& key_frame);
+
   private:
     bool InitWithConfig();
     bool InitParam(const YAML::Node& config_node);
     bool InitGraphOptimizer(const YAML::Node& config_node);
     bool InitDataPath(const YAML::Node& config_node);
 
     void ResetParam();
-    bool SaveTrajectory(const PoseData& laser_odom, const PoseData& gnss_pose);
+    bool SavePose(std::ofstream& ofs, const Eigen::Matrix4f& pose);
     bool AddNodeAndEdge(const PoseData& gnss_data);
-    bool MaybeNewKeyFrame(const CloudData& cloud_data, const PoseData& laser_odom);
+    bool MaybeNewKeyFrame(const CloudData& cloud_data, const PoseData& laser_odom, const PoseData& gnss_pose);
     bool MaybeOptimized();
+    bool SaveOptimizedPose();
 
   private:
     std::string key_frames_path_ = "";
     std::string trajectory_path_ = "";
 
     std::ofstream ground_truth_ofs_;
     std::ofstream laser_odom_ofs_;
+    std::ofstream optimized_pose_ofs_;
 
     float key_frame_distance_ = 2.0;
 
     bool has_new_key_frame_ = false;
     bool has_new_optimized_ = false;
 
     KeyFrame current_key_frame_;
+    KeyFrame current_key_gnss_;
     std::deque<KeyFrame> key_frames_deque_;
+    std::deque<Eigen::Matrix4f> optimized_pose_;
 
     // 优化器
     std::shared_ptr<InterfaceGraphOptimizer> graph_optimizer_ptr_;
-    
+
     class GraphOptimizerConfig {
       public:
         GraphOptimizerConfig() {
           odom_edge_noise.resize(6);
           close_loop_noise.resize(6);
           gnss_noise.resize(3);
         }
-      
+
       public:
         bool use_gnss = true;
         bool use_loop_close = false;

lidar_localization/include/lidar_localization/mapping/back_end/back_end_flow.hpp

@@ -10,6 +10,7 @@
 
 #include "lidar_localization/subscriber/cloud_subscriber.hpp"
 #include "lidar_localization/subscriber/odometry_subscriber.hpp"
+#include "lidar_localization/subscriber/loop_pose_subscriber.hpp"
 
 #include "lidar_localization/publisher/odometry_publisher.hpp"
 #include "lidar_localization/publisher/key_frame_publisher.hpp"
@@ -28,25 +29,28 @@ class BackEndFlow {
 
   private:
     bool ReadData();
+    bool MaybeInsertLoopPose();
     bool HasData();
     bool ValidData();
     bool UpdateBackEnd();
-    bool SaveTrajectory();
     bool PublishData();
 
   private:
     std::shared_ptr<CloudSubscriber> cloud_sub_ptr_;
     std::shared_ptr<OdometrySubscriber> gnss_pose_sub_ptr_;
     std::shared_ptr<OdometrySubscriber> laser_odom_sub_ptr_;
+    std::shared_ptr<LoopPoseSubscriber> loop_pose_sub_ptr_;
 
     std::shared_ptr<OdometryPublisher> transformed_odom_pub_ptr_;
     std::shared_ptr<KeyFramePublisher> key_frame_pub_ptr_;
+    std::shared_ptr<KeyFramePublisher> key_gnss_pub_ptr_;
     std::shared_ptr<KeyFramesPublisher> key_frames_pub_ptr_;
     std::shared_ptr<BackEnd> back_end_ptr_;
 
     std::deque<CloudData> cloud_data_buff_;
     std::deque<PoseData> gnss_pose_data_buff_;
     std::deque<PoseData> laser_odom_data_buff_;
+    std::deque<LoopPose> loop_pose_data_buff_;
 
     PoseData current_gnss_pose_data_;
     PoseData current_laser_odom_data_;

lidar_localization/include/lidar_localization/mapping/front_end/front_end.hpp

@@ -12,8 +12,8 @@
 #include <yaml-cpp/yaml.h>
 
 #include "lidar_localization/sensor_data/cloud_data.hpp"
-#include "lidar_localization/models/registration/ndt_registration.hpp"
-#include "lidar_localization/models/cloud_filter/voxel_filter.hpp"
+#include "lidar_localization/models/registration/registration_interface.hpp"
+#include "lidar_localization/models/cloud_filter/cloud_filter_interface.hpp"
 
 namespace lidar_localization {
 class FrontEnd {

lidar_localization/include/lidar_localization/mapping/loop_closing/loop_closing.hpp

@@ -0,0 +1,65 @@
+/*
+ * @Description: 闭环检测算法
+ * @Author: Ren Qian
+ * @Date: 2020-02-04 18:52:45
+ */
+#ifndef LIDAR_LOCALIZATION_MAPPING_LOOP_CLOSING_LOOP_CLOSING_HPP_
+#define LIDAR_LOCALIZATION_MAPPING_LOOP_CLOSING_LOOP_CLOSING_HPP_
+
+#include <deque>
+#include <Eigen/Dense>
+#include <pcl/registration/ndt.h>
+#include <yaml-cpp/yaml.h>
+
+#include "lidar_localization/sensor_data/key_frame.hpp"
+#include "lidar_localization/sensor_data/loop_pose.hpp"
+#include "lidar_localization/models/registration/registration_interface.hpp"
+#include "lidar_localization/models/cloud_filter/cloud_filter_interface.hpp"
+
+namespace lidar_localization {
+class LoopClosing {
+  public:
+    LoopClosing();
+
+    bool Update(const KeyFrame key_frame, const KeyFrame key_gnss);
+
+    bool HasNewLoopPose();
+    LoopPose& GetCurrentLoopPose();
+
+  private:
+    bool InitWithConfig();
+    bool InitParam(const YAML::Node& config_node);
+    bool InitDataPath(const YAML::Node& config_node);
+    bool InitRegistration(std::shared_ptr<RegistrationInterface>& registration_ptr, const YAML::Node& config_node);
+    bool InitFilter(std::string filter_user, std::shared_ptr<CloudFilterInterface>& filter_ptr, const YAML::Node& config_node);
+
+    bool DetectNearestKeyFrame(int& key_frame_index);
+    bool CloudRegistration(int key_frame_index);
+    bool JointMap(int key_frame_index, CloudData::CLOUD_PTR& map_cloud_ptr, Eigen::Matrix4f& map_pose);
+    bool JointScan(CloudData::CLOUD_PTR& scan_cloud_ptr, Eigen::Matrix4f& scan_pose);
+    bool Registration(CloudData::CLOUD_PTR& map_cloud_ptr, 
+                      CloudData::CLOUD_PTR& scan_cloud_ptr, 
+                      Eigen::Matrix4f& scan_pose, 
+                      Eigen::Matrix4f& result_pose);
+
+  private:
+    std::string key_frames_path_ = "";
+    int extend_frame_num_ = 3;
+    int loop_step_ = 10;
+    int diff_num_ = 100;
+    float detect_area_ = 10.0;
+    float fitness_score_limit_ = 2.0;
+
+    std::shared_ptr<CloudFilterInterface> scan_filter_ptr_;
+    std::shared_ptr<CloudFilterInterface> map_filter_ptr_;
+    std::shared_ptr<RegistrationInterface> registration_ptr_; 
+
+    std::deque<KeyFrame> all_key_frames_;
+    std::deque<KeyFrame> all_key_gnss_;
+
+    LoopPose current_loop_pose_;
+    bool has_new_loop_pose_ = false;
+};
+}
+
+#endif

lidar_localization/include/lidar_localization/mapping/loop_closing/loop_closing_flow.hpp

@@ -0,0 +1,48 @@
+/*
+ * @Description: 
+ * @Author: Ren Qian
+ * @Date: 2020-02-29 03:32:14
+ */
+#ifndef LIDAR_LOCALIZATION_MAPPING_LOOP_CLOSING_LOOP_CLOSING_FLOW_HPP_
+#define LIDAR_LOCALIZATION_MAPPING_LOOP_CLOSING_LOOP_CLOSING_FLOW_HPP_
+
+#include <deque>
+#include <ros/ros.h>
+// subscriber
+#include "lidar_localization/subscriber/key_frame_subscriber.hpp"
+// publisher
+#include "lidar_localization/publisher/loop_pose_publisher.hpp"
+// loop closing
+#include "lidar_localization/mapping/loop_closing/loop_closing.hpp"
+
+namespace lidar_localization {
+class LoopClosingFlow {
+  public:
+    LoopClosingFlow(ros::NodeHandle& nh);
+
+    bool Run();
+
+  private:
+    bool ReadData();
+    bool HasData();
+    bool ValidData();
+    bool PublishData();
+
+  private:
+    // subscriber
+    std::shared_ptr<KeyFrameSubscriber> key_frame_sub_ptr_;
+    std::shared_ptr<KeyFrameSubscriber> key_gnss_sub_ptr_;
+    // publisher
+    std::shared_ptr<LoopPosePublisher> loop_pose_pub_ptr_;
+    // loop closing
+    std::shared_ptr<LoopClosing> loop_closing_ptr_;
+
+    std::deque<KeyFrame> key_frame_buff_;
+    std::deque<KeyFrame> key_gnss_buff_;
+
+    KeyFrame current_key_frame_;
+    KeyFrame current_key_gnss_;
+};
+}
+
+#endif

lidar_localization/include/lidar_localization/models/cloud_filter/cloud_filter_interface.hpp

@@ -14,7 +14,7 @@ class CloudFilterInterface {
   public:
     virtual ~CloudFilterInterface() = default;
 
-    virtual bool Filter(const CloudData::CLOUD_PTR& input_cloud_ptr, CloudData::CLOUD_PTR& filtered_cloud_ptr) = 0;
+    virtual bool Filter(CloudData::CLOUD_PTR& input_cloud_ptr, CloudData::CLOUD_PTR& filtered_cloud_ptr) = 0;
 };
 }
 

lidar_localization/include/lidar_localization/models/cloud_filter/no_filter.hpp

@@ -0,0 +1,19 @@
+/*
+ * @Description: 不滤波
+ * @Author: Ren Qian
+ * @Date: 2020-02-09 19:37:49
+ */
+#ifndef LIDAR_LOCALIZATION_MODELS_CLOUD_FILTER_NO_FILTER_HPP_
+#define LIDAR_LOCALIZATION_MODELS_CLOUD_FILTER_NO_FILTER_HPP_
+
+#include "lidar_localization/models/cloud_filter/cloud_filter_interface.hpp"
+
+namespace lidar_localization {
+class NoFilter: public CloudFilterInterface {
+  public:
+    NoFilter();
+
+    bool Filter(CloudData::CLOUD_PTR& input_cloud_ptr, CloudData::CLOUD_PTR& filtered_cloud_ptr) override;
+};
+}
+#endif

lidar_localization/include/lidar_localization/models/cloud_filter/voxel_filter.hpp

@@ -15,7 +15,7 @@ class VoxelFilter: public CloudFilterInterface {
     VoxelFilter(const YAML::Node& node);
     VoxelFilter(float leaf_size_x, float leaf_size_y, float leaf_size_z);
 
-    bool Filter(const CloudData::CLOUD_PTR& input_cloud_ptr, CloudData::CLOUD_PTR& filtered_cloud_ptr) override;
+    bool Filter(CloudData::CLOUD_PTR& input_cloud_ptr, CloudData::CLOUD_PTR& filtered_cloud_ptr) override;
 
   private:
     bool SetFilterParam(float leaf_size_x, float leaf_size_y, float leaf_size_z);

lidar_localization/include/lidar_localization/models/registration/ndt_registration.hpp

@@ -20,6 +20,7 @@ class NDTRegistration: public RegistrationInterface {
                    const Eigen::Matrix4f& predict_pose, 
                    CloudData::CLOUD_PTR& result_cloud_ptr,
                    Eigen::Matrix4f& result_pose) override;
+    float GetFitnessScore() override;
 
   private:
     bool SetRegistrationParam(float res, float step_size, float trans_eps, int max_iter);

lidar_localization/include/lidar_localization/models/registration/registration_interface.hpp

@@ -20,6 +20,7 @@ class RegistrationInterface {
                           const Eigen::Matrix4f& predict_pose, 
                           CloudData::CLOUD_PTR& result_cloud_ptr,
                           Eigen::Matrix4f& result_pose) = 0;
+    virtual float GetFitnessScore() = 0;
 };
 }