代码框架写好,在调试

CMakeLists.txt

@@ -46,7 +46,7 @@ message(STATUS "PCL_DEFINITIONS:" ${PCL_DEFINITIONS})
 find_package(G2O REQUIRED)
 include_directories(SYSTEM ${G2O_INCLUDE_DIR} ${G2O_INCLUDE_DIRS})
 link_directories(${G2O_LIBRARY_DIRS})
-# link_libraries(${G2O_LIBRARIES})
+link_libraries(${G2O_LIBRARIES})
 
 find_package(OpenMP)
 if (OPENMP_FOUND)
@@ -110,6 +110,39 @@ target_link_libraries(laser_odom
   ${PCL_LIBRARIES}
 )
 
+add_executable(hdl_backend
+  src/hdl_backend.cpp
+  src/hdl_graph_slam/map_cloud_generator.cpp
+  src/hdl_graph_slam/information_matrix_calculator.cpp
+  src/hdl_graph_slam/graph_slam.cpp
+  src/hdl_graph_slam/keyframe.cpp
+  src/hdl_graph_slam/map_cloud_generator.cpp
+  # src/hdl_graph_slam/src/hdl_graph_slam/map2odom_publisher.py
+  src/hdl_graph_slam/registrations.cpp
+  src/g2o/robust_kernel_io.cpp
+  )
+# add_library(hdl_backend src/hdl_backend.cpp)
+target_link_libraries(hdl_backend
+  ${catkin_LIBRARIES}
+  ${PCL_LIBRARIES}
+  glog
+  # ${G2O_LIBRARY_DIRS}
+)
+
+target_link_libraries(hdl_backend
+  ${catkin_LIBRARIES}
+  ${PCL_LIBRARIES}
+  ${G2O_TYPES_DATA}
+  ${G2O_CORE_LIBRARY}
+  ${G2O_STUFF_LIBRARY}
+  ${G2O_SOLVER_PCG}
+  ${G2O_SOLVER_CSPARSE}   # be aware of that CSPARSE is released under LGPL
+  ${G2O_SOLVER_CHOLMOD}   # be aware of that cholmod is released under GPL
+  ${G2O_TYPES_SLAM3D}
+  ${G2O_TYPES_SLAM3D_ADDONS}
+)
+
+
 add_library(prefiltering_nodelet apps/prefiltering_nodelet.cpp)
 target_link_libraries(prefiltering_nodelet
   ${catkin_LIBRARIES}
@@ -181,3 +214,4 @@ install(TARGETS
 install(DIRECTORY include/
    DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}
 )
+

apps/hdl_graph_slam_nodelet.cpp

@@ -144,6 +144,7 @@ public:
 
     // subscribers
     // 话题通信的接受方
+    // 这个launch中的话题名是: odom
     odom_sub.reset(new message_filters::Subscriber<nav_msgs::Odometry>(mt_nh, published_odom_topic, 256));      // 创建一个新的message_filters::Subscriber对象，用于订阅nav_msgs::Odometry类型的消息
     cloud_sub.reset(new message_filters::Subscriber<sensor_msgs::PointCloud2>(mt_nh, "/filtered_points", 32));  // 创建一个新的message_filters::Subscriber对象，用于订阅sensor_msgs::PointCloud2类型的消息。
     // ApproxSyncPolicy 是一个同步策略，允许在一定时间窗口内的消息进行同步，这里设置的窗口大小为32
@@ -888,7 +889,7 @@ private:
   }
 
   /**
-   * @brief load all data from a directory
+   * @brief load all data from a directory  从目录加载所有数据
    * @param req
    * @param res
    * @return
@@ -1000,7 +1001,7 @@ private:
   }
 
   /**
-   * @brief dump all data to the current directory
+   * @brief dump all data to the current directory    将所有数据转储到当前目录
    * @param req
    * @param res
    * @return

include/hdl_backend.hpp

@@ -7,37 +7,187 @@
 #include <sensor_msgs/PointCloud2.h>
 #include <message_filters/sync_policies/approximate_time.h>
 #include <message_filters/synchronizer.h>
+// #include "hdl_graph_slam/keyframe_updater.hpp"
+#include <hdl_graph_slam/keyframe_updater.hpp>
+#include <hdl_graph_slam/keyframe.hpp>
+#include <hdl_graph_slam/SaveMap.h>
+#include <hdl_graph_slam/map_cloud_generator.hpp>
+#include <boost/format.hpp>
+#include <hdl_graph_slam/graph_slam.hpp>
+#include <hdl_graph_slam/ros_time_hash.hpp>
+#include <g2o/types/slam3d/vertex_se3.h>
+#include <hdl_graph_slam/information_matrix_calculator.hpp>
+
+
+// 直接把头文件都复制过来
+
+#include <ctime>
+#include <mutex>
+#include <atomic>
+#include <memory>
+#include <iomanip>
+#include <iostream>
+#include <unordered_map>
+#include <boost/format.hpp>
+#include <boost/thread.hpp>
+#include <boost/filesystem.hpp>
+#include <boost/algorithm/string.hpp>
+#include <Eigen/Dense>
+#include <pcl/io/pcd_io.h>
+
+#include <ros/ros.h>
+#include <geodesy/utm.h>
+#include <geodesy/wgs84.h>
+#include <pcl_ros/point_cloud.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/time_synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <tf_conversions/tf_eigen.h>
+#include <tf/transform_listener.h>
+
+#include <std_msgs/Time.h>
+#include <nav_msgs/Odometry.h>
+#include <nmea_msgs/Sentence.h>
+#include <sensor_msgs/Imu.h>
+#include <sensor_msgs/NavSatFix.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <geographic_msgs/GeoPointStamped.h>
+#include <visualization_msgs/MarkerArray.h>
+#include <hdl_graph_slam/FloorCoeffs.h>
+
+#include <hdl_graph_slam/SaveMap.h>
+#include <hdl_graph_slam/LoadGraph.h>
+#include <hdl_graph_slam/DumpGraph.h>
+
+#include <nodelet/nodelet.h>
+#include <pluginlib/class_list_macros.h>
+
+#include <hdl_graph_slam/ros_utils.hpp>
+#include <hdl_graph_slam/ros_time_hash.hpp>
+
+#include <hdl_graph_slam/graph_slam.hpp>
+#include <hdl_graph_slam/keyframe.hpp>
+#include <hdl_graph_slam/keyframe_updater.hpp>
+#include <hdl_graph_slam/loop_detector.hpp>
+#include <hdl_graph_slam/information_matrix_calculator.hpp>
+#include <hdl_graph_slam/map_cloud_generator.hpp>
+#include <hdl_graph_slam/nmea_sentence_parser.hpp>
+
+#include <g2o/types/slam3d/edge_se3.h>
+#include <g2o/types/slam3d/vertex_se3.h>
+#include <g2o/edge_se3_plane.hpp>
+#include <g2o/edge_se3_priorxy.hpp>
+#include <g2o/edge_se3_priorxyz.hpp>
+#include <g2o/edge_se3_priorvec.hpp>
+#include <g2o/edge_se3_priorquat.hpp>
+
+
 
 typedef message_filters::sync_policies::ApproximateTime<nav_msgs::Odometry, sensor_msgs::PointCloud2> ApproxSyncPolicy;
 typedef pcl::PointXYZI PointT;
 
 class hdl_backend {
 private:
-  
-
   ros::NodeHandle nh;             // ros 的节点句柄
-  ros::Publisher map_points_pub;  // 话题通常包含了所有之前扫描过的点云，随着机器人在环境中移动，新的点云数据会被不断添加到这个地图中，从而提供一个逐步完善的环境表示
 
   // std::string published_odom_topic;
-  std::string map_frame_id;
-  // std::string odom_frame_id;
-  
-  std::string map_cloud_resolution;  // 地图分辨率
-  Eigen::Matrix4f trans_odom2map;    // 表示从 里程计坐标系（odom frame）到地图坐标系（map frame）的变换矩阵       这个变换矩阵是跟机械臂变换矩阵类似的 齐次变换矩阵
 
-  int max_keyframes_per_update;  // 限制每次更新中可以处理的最大关键帧数量
 
-  std::string points_topic;  // 点云数据的ROS话题名称，SLAM系统将订阅这个话题来获取点云数据
+
+  int max_keyframes_per_update;  // 限制每次更新中可以处理的最大关键帧数量
 
   std::unique_ptr<message_filters::Subscriber<nav_msgs::Odometry>> odom_sub;
   std::unique_ptr<message_filters::Subscriber<sensor_msgs::PointCloud2>> cloud_sub;
   std::unique_ptr<message_filters::Synchronizer<ApproxSyncPolicy>> sync;
 
-  void cloud_callback(const nav_msgs::OdometryConstPtr& odom_msg, const sensor_msgs::PointCloud2::ConstPtr& cloud_msg){};
+  void cloud_callback(const nav_msgs::OdometryConstPtr& odom_msg, const sensor_msgs::PointCloud2::ConstPtr& cloud_msg);
+  void optimization_timer_callback(const ros::WallTimerEvent& event);
+  void map_points_publish_timer_callback(const ros::WallTimerEvent& event) ;
+
+
+
+
+  static hdl_backend instance;  // 静态实例, 保证唯一
+  // 静态成员变量与类的所有实例共享，而不仅仅属于某个对象实例。因为它是私有的，所以外部不能直接访问或修改这个变量
+  // 该声明并不会分配内存空间。静态变量的定义和初始化必须在类的外部进行,因为类的声明不会为静态成员分配内存，只有类外定义才会分配空间。    仅是声明,没有分配内存
+  // 静态成员变量的生命周期与类的生命周期一致，而不是与对象的生命周期一致
+  // C++ 语言允许在类外部对静态成员变量进行定义和初始化，这是因为静态成员变量的存储空间在全局内存中分配，而不是在对象实例中。
+
+
+  ros::ServiceServer save_map_service_server;  // 一个 ROS 服务服务器，用于保存地图
+
+  bool save_map_service(hdl_graph_slam::SaveMapRequest& req, hdl_graph_slam::SaveMapResponse& res);
+
+
+
+  boost::optional<Eigen::Vector3d> zero_utm;
+
+
+  ros::WallTimer optimization_timer;
+  ros::WallTimer map_publish_timer;
+ 
+
+  // int max_keyframes_per_update;             // 限制每次更新中可以处理的最大关键帧数量
+
+
+  // g2o::VertexPlane* floor_plane_node;    // 用于表示地面平面节点的指针。
+  // std::vector<hdl_graph_slam::KeyFrame::Ptr> keyframes;  // 存储当前系统中的所有关键帧。
+  std::unordered_map<ros::Time, hdl_graph_slam::KeyFrame::Ptr, RosTimeHash> keyframe_hash;  // 用于快速查找关键帧的哈希表
+
+
+
+
+
+
+
 
 public:
   hdl_backend(){};   // 构造函数
   ~hdl_backend(){};  // 析构函数
   void init();       // 初始化函数
   std::string base_frame_id;
+
+  std::unique_ptr<hdl_graph_slam::KeyframeUpdater> keyframe_updater;
+  std::mutex keyframe_queue_mutex;                           // std::mutex 是一种独占式互斥量
+  std::deque<hdl_graph_slam::KeyFrame::Ptr> keyframe_queue;  // 关键帧队列
+
+  // static hdl_backend& getInstance() {
+  //   return instance;
+  // }
+
+  std::string points_topic;  // 点云数据的ROS话题名称，SLAM系统将订阅这个话题来获取点云数据    这个不要?
+  ros::Publisher read_until_pub;
+
+  std::mutex main_thread_mutex;  // 互斥量
+
+  bool flush_keyframe_queue();
+
+  std::unique_ptr<hdl_graph_slam::LoopDetector> loop_detector;
+  std::vector<hdl_graph_slam::KeyFrame::Ptr> keyframes;  // 存储当前系统中的所有关键帧。
+  std::deque<hdl_graph_slam::KeyFrame::Ptr> new_keyframes;  // 存储新生成的关键帧指针
+  std::unique_ptr<hdl_graph_slam::GraphSLAM> graph_slam;  // 声明一个指向GraphSLAM 类型的智能指针 graph_slam
+
+  g2o::VertexSE3* anchor_node;           // 一个指向 g2o::VertexSE3 类型的指针，表示位姿图中的一个特定节点（锚点节点）。锚点节点通常是用来约束图的某个部分，使得图优化时可以保持稳定。
+  g2o::EdgeSE3* anchor_edge;             // 一个指向 g2o::EdgeSE3 类型的指针，表示连接锚点节点和其他节点的边。这条边用于描述锚点与其他位姿节点之间的相对约束关系。
+
+  std::unique_ptr<hdl_graph_slam::InformationMatrixCalculator> inf_calclator;
+  std::mutex trans_odom2map_mutex;
+  Eigen::Matrix4f trans_odom2map;    // 表示从 里程计坐标系（odom frame）到地图坐标系（map frame）的变换矩阵       这个变换矩阵是跟机械臂变换矩阵类似的 齐次变换矩阵
+  
+  std::mutex keyframes_snapshot_mutex;                                     // 互斥量
+  std::vector<hdl_graph_slam::KeyFrameSnapshot::Ptr> keyframes_snapshot;   // 用来存储关键帧的快照。这个变量很重要，发布 map_point 和 保存 pcd 地图都是用这个变量！！
+  std::atomic_bool graph_updated;                                          // 用于标识 位资图（pose graph）是否更新，确保多线程环境下的线程安全
+
+  ros::Publisher odom2map_pub;
+  ros::Publisher markers_pub;
+
+  visualization_msgs::MarkerArray create_marker_array(const ros::Time& stamp) const;
+
+  std::string map_frame_id;
+  std::string odom_frame_id;
+  ros::Publisher map_points_pub;  // 话题通常包含了所有之前扫描过的点云，随着机器人在环境中移动，新的点云数据会被不断添加到这个地图中，从而提供一个逐步完善的环境表示
+
+  double map_cloud_resolution;
+  std::unique_ptr<hdl_graph_slam::MapCloudGenerator> map_cloud_generator;  // 生成点云地图的对象指针
+
 };

include/hdl_graph_slam/keyframe_updater.hpp

@@ -20,8 +20,8 @@ public:
    * @param pnh
    */
   KeyframeUpdater(ros::NodeHandle& pnh) : is_first(true), prev_keypose(Eigen::Isometry3d::Identity()) {
-    keyframe_delta_trans = pnh.param<double>("keyframe_delta_trans", 2.0);
-    keyframe_delta_angle = pnh.param<double>("keyframe_delta_angle", 2.0);
+    keyframe_delta_trans = pnh.param<double>("keyframe_delta_trans", 0.5);
+    keyframe_delta_angle = pnh.param<double>("keyframe_delta_angle", 0.5);
 
     accum_distance = 0.0;
   }
@@ -45,7 +45,7 @@ public:
     double da = Eigen::AngleAxisd(delta.linear()).angle();
 
     // too close to the previous frame
-    if(dx < keyframe_delta_trans && da < keyframe_delta_angle) {
+    if(dx < 2 && da < 2) {
       return false;
     }
 

launch/start.launch

@@ -1,11 +1,12 @@
 <launch>
     <node pkg="hdl_graph_slam" type="laser_odom" name="laser_odom" output="log" />
+    <node pkg="hdl_graph_slam" type="hdl_backend" name="hdl_backend" output="screen" />
     
-        <node name="rviz" pkg="rviz" type="rviz" args="-d $(find hdl_graph_slam)/rviz/hdl_graph_slam.rviz"/>  
+    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find hdl_graph_slam)/rviz/hdl_graph_slam.rviz"/>  
 
     <!-- 使用仿真时间 -->
     <param name="use_sim_time" value="true" />
     <!-- 回放rosbag文件 -->
-    <node pkg="rosbag" type="play" name="player" output="screen" args="--clock /home/mj/hdl_slam/ros_yfm.bag"/>
+    <node pkg="rosbag" type="play" name="player" output="log" args="--clock /home/mj/hdl_slam/ros_yfm.bag"/>
 
 </launch>

src/hdl_backend.cpp

@@ -6,84 +6,547 @@
 #include <hdl_graph_slam/ros_utils.hpp>
 #include <functional>  // std::bind 和 std::placeholders
 
+#include <hdl_graph_slam/keyframe_updater.hpp>
+#include "hdl_graph_slam/keyframe_updater.hpp"
+#include <visualization_msgs/MarkerArray.h>
 
+#include <glog/logging.h>
+
+hdl_backend* instance = nullptr;  // 初始化静态单例(分配内存, 调用hdl_backend 的默认构造函数)
+// hdl_backend& hdl_opt = hdl_backend::getInstance();  // 因为 静态成员变量 instance 是私有的,需要 静态成员函数 getInstance() 拿到 instance, 并且起个别名(引用)
+
+typedef pcl::PointXYZI PointT;
 
 int main(int argc, char* argv[]) {
   // 执行 ros 节点初始化
   ros::init(argc, argv, "hdl_backend");
-
+  std::cout << "hdl backend get in ...===============" << std::endl;
+  instance = new hdl_backend();
+  // hdl_backend& hdl_opt = instance->getInstance();
+  instance->init();
   return 0;
 }
 
 void hdl_backend::init() {
   map_frame_id = "map";
-  map_cloud_resolution = "0.05";  // 地图点云分辨率
-  trans_odom2map.setIdentity();   // 设置为单位矩阵
+  // map_frame_id = "odom";
+  odom_frame_id = "odom";
+  map_cloud_resolution = 0.05;   // 地图点云分辨率
+  trans_odom2map.setIdentity();  // 设置为单位矩阵
   max_keyframes_per_update = 10;
+  keyframe_updater.reset(new hdl_graph_slam::KeyframeUpdater(nh));
+  points_topic = "/velodyne_points";
+  graph_slam.reset(new hdl_graph_slam::GraphSLAM("lm_var_cholmod"));  // 智能指针，用reset重新初始化，让它重新指向新分配的对象
+  // graph_slam.reset(new GraphSLAM(private_nh.param<std::string>("g2o_solver_type", "lm_var")));  // 智能指针，用reset重新初始化，让它重新指向新分配的对象
+
+  inf_calclator.reset(new hdl_graph_slam::InformationMatrixCalculator(nh));
+  loop_detector.reset(new hdl_graph_slam::LoopDetector(nh));
 
-  // odom_sub.reset(new message_filters::Subscriber<nav_msgs::Odometry>(nh, "/laser_odom", 256));                   // 创建 message_filters::Subscriber 对象，用于订阅nav_msgs::Odometry类型的消息
-  // cloud_sub.reset(new message_filters::Subscriber<sensor_msgs::PointCloud2>(nh, "/filtered_points", 32));        // 创建 message_filters::Subscriber 对象，用于订阅sensor_msgs::PointCloud2类型的消息。
-  // sync.reset(new message_filters::Synchronizer<ApproxSyncPolicy>(ApproxSyncPolicy(32), *odom_sub, *cloud_sub));  // 创建 message_filters::Synchronizer 对象，用于同步上面创建的两个订阅者（odom_sub和cloud_sub）的消息。
-  // // sync->registerCallback(boost::bind(&hdl_backend::cloud_callback, this, _1, _2));                               // 为同步器注册了一个回调函数，当同步条件满足时，会调用HdlGraphSlamNodelet类的cloud_callback成员函数
-  // sync->registerCallback(std::bind(&hdl_backend::cloud_callback, this, std::placeholders::_1, std::placeholders::_2));
+  anchor_node = nullptr;
+  anchor_edge = nullptr;
+  odom_sub.reset(new message_filters::Subscriber<nav_msgs::Odometry>(nh, "/laser_odom", 256));                   // 创建 message_filters::Subscriber 对象，用于订阅nav_msgs::Odometry类型的消息
+  cloud_sub.reset(new message_filters::Subscriber<sensor_msgs::PointCloud2>(nh, "/aligned_points", 32));         // 创建 message_filters::Subscriber 对象，用于订阅sensor_msgs::PointCloud2类型的消息。
+  sync.reset(new message_filters::Synchronizer<ApproxSyncPolicy>(ApproxSyncPolicy(32), *odom_sub, *cloud_sub));  // 创建 message_filters::Synchronizer 对象，用于同步上面创建的两个订阅者（odom_sub和cloud_sub）的消息。
+  sync->registerCallback(boost::bind(&hdl_backend::cloud_callback, this, _1, _2));                               // 为同步器注册了一个回调函数，当同步条件满足时，会调用HdlGraphSlamNodelet类的cloud_callback成员函数
 
-  // 订阅者的创建
-  odom_sub.reset(new message_filters::Subscriber<nav_msgs::Odometry>(nh, "/laser_odom", 256)); // 创建 message_filters::Subscriber 对象，用于订阅nav_msgs::Odometry类型的消息
-  cloud_sub.reset(new message_filters::Subscriber<sensor_msgs::PointCloud2>(nh, "/filtered_points", 32)); // 创建 message_filters::Subscriber 对象，用于订阅sensor_msgs::PointCloud2类型的消息
+  markers_pub = nh.advertise<visualization_msgs::MarkerArray>("/hdl_graph_slam/markers", 16);      // 发布可视化数据（具体是什么还不知道）   这种类型的消息通常用于在RViz等可视化工具中显示标记（markers）
+  odom2map_pub = nh.advertise<geometry_msgs::TransformStamped>("/hdl_graph_slam/odom2pub", 16);    // 发布从里程计到地图的变换
+  map_points_pub = nh.advertise<sensor_msgs::PointCloud2>("/hdl_graph_slam/map_points", 1, true);  // 这个话题会保留以前扫描过的点云！！ 扫过的地方的点云就是
 
-  // 同步器的创建
-  sync.reset(new message_filters::Synchronizer<ApproxSyncPolicy>(ApproxSyncPolicy(32), *odom_sub, *cloud_sub)); // 创建 message_filters::Synchronizer 对象，用于同步上面创建的两个订阅者（odom_sub和cloud_sub）的消息
+  read_until_pub = nh.advertise<std_msgs::Header>("/hdl_graph_slam/read_until", 32);  // 这种类型的消息通常用于指示读取操作应该持续到何时
 
-  // 注册回调函数
-  sync->registerCallback(boost::bind(&hdl_backend::cloud_callback, this, std::placeholders::_1, std::placeholders::_2)); // 使用 std::bind 注册回调函数
+  save_map_service_server = nh.advertiseService("/hdl_graph_slam/save_map", &hdl_backend::save_map_service, this);  // 保存地图
 
+  graph_updated = false;
 
+  double graph_update_interval = 1;       // 表示图优化更新的时间间隔，默认值为3.0秒    interval：间隔
+  double map_cloud_update_interval = 4;  // 地图点云更新的时间间隔，默认值为10.0秒
 
+  optimization_timer = nh.createWallTimer(ros::WallDuration(graph_update_interval), &hdl_backend::optimization_timer_callback, this);  // 创建一个定时器，定时器会在每个 graph_update_interval 秒后触发，然后调用 optimization_timer_callback 函数
+  map_publish_timer = nh.createWallTimer(ros::WallDuration(map_cloud_update_interval), &hdl_backend::map_points_publish_timer_callback, this);  // 创建一个定时器，定时器会在每个 map_cloud_update_interval 秒后触发，然后调用 map_points_publish_timer_callback 函数
+
+  map_cloud_resolution = 0.05;
+
+  ros::spin();
 }
 
+/**
+ * @brief 这个函数将队列中的所有数据添加到姿势图中，然后优化姿势图
+ * @param event   // 是ROS提供的定时器时间类，包含信息有：计时器出发的时间戳、上一次出发的时间戳、计时器的周期信息等
+ */
+void hdl_backend::optimization_timer_callback(const ros::WallTimerEvent& event) {
+  LOG(INFO) << "optimization_timer_callback";
+
+  std::lock_guard<std::mutex> lock(main_thread_mutex);  // 创建了一个锁，用于保护对主线程的访问，确保线程安全
+
+  // add keyframes and floor coeffs in the queues to the pose graph  将队列中的关键帧和楼层系数添加到姿势图中
+  bool keyframe_updated = flush_keyframe_queue();  // 调用 flush_keyframe_queue 函数，将关键帧队列中的数据添加到位姿图中，并返回是否有关键帧被更新
+  // LOG(INFO) << keyframe_updated;
+
+  if(!keyframe_updated) {  // 如果没有关键帧被更新，则执行大括号内的代码
+    std_msgs::Header read_until;
+    read_until.stamp = ros::Time::now() + ros::Duration(30, 0);  // 时间戳为什么要加30秒？
+    read_until.frame_id = points_topic;
+    read_until_pub.publish(read_until);
+    read_until.frame_id = "/filtered_points";
+    read_until_pub.publish(read_until);  // 不同的 frame_id 和 话题 发布两次， 是干嘛用的？
+  }
+  if(!keyframe_updated) {
+    // 检查地板、GPS、IMU等队列，分别调用相应的 flush 函数（例如 flush_floor_queue、flush_gps_queue、flush_imu_queue），如果这些数据没有更新，则函数直接返回，不进行后续操作。
+    LOG(INFO) << "!keyframe_updated";
+    return;
+  }
+  // loop detection   闭环检测
+  std::vector<hdl_graph_slam::Loop::Ptr> loops = loop_detector->detect(keyframes, new_keyframes, *graph_slam);
+
+  // 如果检测到闭环，则为闭环关系添加一个新的边到位姿图中，计算相对位姿并生成信息矩阵，确保闭环边的优化权重。
+  for(const auto& loop : loops) {  // 遍历闭环检测结果
+    Eigen::Isometry3d relpose(loop->relative_pose.cast<double>());
+    Eigen::MatrixXd information_matrix = inf_calclator->calc_information_matrix(loop->key1->cloud, loop->key2->cloud, relpose);
+    auto edge = graph_slam->add_se3_edge(loop->key1->node, loop->key2->node, relpose, information_matrix);
+    graph_slam->add_robust_kernel(edge, "Huber", 1.0);
+  }
+  LOG(INFO) << "!111111111";
+
+
+  std::copy(new_keyframes.begin(), new_keyframes.end(), std::back_inserter(keyframes));  // 将新关键帧 new_keyframes 合并到已有的关键帧列表 keyframes 中
+  new_keyframes.clear();                                                                 // 清空 new_keyframes
+
+  // move the first node anchor position to the current estimate of the first node pose   将第一节点锚点位置移动到第一节点姿态的当前估计值
+  // so the first node moves freely while trying to stay around the origin                因此，第一个节点在试图停留在原点附近的同时可以自由移动
+
+  if(anchor_node && true) {  // launch文件中，fix_first_node_adaptive 设置为 true
+    Eigen::Isometry3d anchor_target = static_cast<g2o::VertexSE3*>(anchor_edge->vertices()[1])->estimate();
+    anchor_node->setEstimate(anchor_target);
+    // 如果启用了自适应固定第一帧的功能（参数 "fix_first_node_adaptive"），则将第一个关键帧的锚点位置更新为当前估计的位置，以允许它自由移动但仍然保持在原点附近。
+  }
+  LOG(INFO) << "!2222222222";
+  // optimize the pose graph
+  int num_iterations = 512;              // launch文件中都是设置成512
+  graph_slam->optimize(num_iterations);  // 使用 g2o 优化器对位姿图进行优化，优化的迭代次数由参数 "g2o_solver_num_iterations" 控制
+
+
+  std::vector<hdl_graph_slam::KeyFrameSnapshot::Ptr> snapshot(keyframes.size());
+  std::transform(keyframes.begin(), keyframes.end(), snapshot.begin(), [=](const hdl_graph_slam::KeyFrame::Ptr& k) { return std::make_shared<hdl_graph_slam::KeyFrameSnapshot>(k); });
+
+  keyframes_snapshot_mutex.lock();
+  keyframes_snapshot.swap(snapshot);
+  keyframes_snapshot_mutex.unlock();
+  graph_updated = true;
+  LOG(INFO) << "graph_updated";
+
+  // publish tf     发布位姿变换
+  const auto& keyframe = keyframes.back();  // 获取最新的关键帧估计
+  Eigen::Isometry3d trans = keyframe->node->estimate() * keyframe->odom.inverse();
+  trans_odom2map_mutex.lock();
+  trans_odom2map = trans.matrix().cast<float>();
+  trans_odom2map_mutex.unlock();
+
+  if(odom2map_pub.getNumSubscribers()) {
+    geometry_msgs::TransformStamped ts = hdl_graph_slam::matrix2transform(keyframe->stamp, trans.matrix().cast<float>(), map_frame_id, odom_frame_id);
+    odom2map_pub.publish(ts);  // 发布odom2map_pub话题中
+    LOG(INFO) << "pub odom2map_pub";
+  }
+
+  if(markers_pub.getNumSubscribers()) {
+    auto markers = create_marker_array(ros::Time::now());
+    markers_pub.publish(markers);
+    LOG(INFO) << "pub markers_pub";
+  }
+
+}
+
+/**
+ * @brief generate map point cloud and publish it
+ * @param event
+ */
+void hdl_backend::map_points_publish_timer_callback(const ros::WallTimerEvent& event) {
+  // LOG(INFO) << "map_points_publish_timer_callback";
+
+  if(!map_points_pub.getNumSubscribers() || !graph_updated) {
+    // LOG(INFO) << "cloud 222222";
+
+    return;
+  }
+  // LOG(INFO) << "cloud 33333";
+
+  std::vector<hdl_graph_slam::KeyFrameSnapshot::Ptr> snapshot;
+
+  // keyframes_snapshot_mutex.lock();
+  snapshot = keyframes_snapshot;
+  // keyframes_snapshot_mutex.unlock();
+
+  auto cloud = map_cloud_generator->generate(snapshot, map_cloud_resolution);
+  if(!cloud) {
+    return;
+  }
+
+  cloud->header.frame_id = map_frame_id;
+  cloud->header.stamp = snapshot.back()->cloud->header.stamp;
+
+  sensor_msgs::PointCloud2Ptr cloud_msg(new sensor_msgs::PointCloud2());
+  pcl::toROSMsg(*cloud, *cloud_msg);
+
+  // cloud->header.frame_id = "odom";    // 
+  LOG(INFO) << cloud_msg->header.frame_id;
+
+  map_points_pub.publish(*cloud_msg);
+  // cloud->header.frame_id = "map"; // 
+} 
+
 /**
  * @brief 接收点云数据并放到 keyframe_queue 中
  * @param odom_msg  前端的激光里程计数据
  * @param cloud_msg 前端滤波后的点云数据
  */
-void cloud_callback(const nav_msgs::OdometryConstPtr& odom_msg, const sensor_msgs::PointCloud2::ConstPtr& cloud_msg) {
-    const ros::Time& stamp = cloud_msg->header.stamp;  // 获取点云数据的时间戳
-    Eigen::Isometry3d odom = hdl_graph_slam::odom2isometry(odom_msg);  // 将里程计消息转换为Eigen库中的Isometry3d类型，它表示一个3D刚体变换（包括旋转和平移）
+void hdl_backend::cloud_callback(const nav_msgs::OdometryConstPtr& odom_msg, const sensor_msgs::PointCloud2::ConstPtr& cloud_msg) {
+  // LOG(INFO)<< "cloud_callback";
+  const ros::Time& stamp = cloud_msg->header.stamp;                  // 获取点云数据的时间戳
+  Eigen::Isometry3d odom = hdl_graph_slam::odom2isometry(odom_msg);  // 将里程计消息转换为Eigen库中的Isometry3d类型，它表示一个3D刚体变换（包括旋转和平移）
+
+  pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());  // 创建一个点云对象的指针
+  pcl::fromROSMsg(*cloud_msg, *cloud);                                // 将ROS的点云消息 cloud_msg 转换为PCL（Point Cloud Library）的点云格式
+  if(base_frame_id.empty()) {                                         // 用类的单例实现应该就ok?
+    base_frame_id = cloud_msg->header.frame_id;                       // 将当前点云消息的坐标系 frame_id 设置为基础坐标系      base_frame_id 是整个系统中关键帧数据的参考坐标系
+  }
+
+  // 更新关键帧判断
+  if(!keyframe_updater->update(odom)) {                      // 根据当前的里程计信息 odom 判断是否需要生成新的关键帧
+    // LOG(INFO) << "!keyframe_updater->update(odom)";
+    std::lock_guard<std::mutex> lock(keyframe_queue_mutex);  // 这行代码的作用是确保线程安全，防止多个线程同时访问或修改 keyframe_queue 队列
+    // std::lock_guard<std::mutex> 是 C++ 标准库中的一个类，用来简化互斥锁（std::mutex）的使用。它的作用是在其作用域内自动对给定的互斥量加锁，并在作用域结束时自动解锁
+    // keyframe_queue_mutex 是一个互斥量（std::mutex），用于保护关键帧队列 keyframe_queue。当多个线程试图同时访问或修改 keyframe_queue 时，互斥量确保只有一个线程能够访问该资源，避免数据竞争和并发问题
+    // 工作原理：当这行代码执行时，lock_guard 会锁住 keyframe_queue_mutex，使其他线程无法访问与之关联的资源（即 keyframe_queue） 当程序执行离开该作用域时，lock_guard 会自动释放锁，无需显式调用 unlock() 函数
+    // 为什么需要锁？   在多线程环境中（比如 ROS 回调函数可能被不同线程调用），如果多个线程同时读取或写入 keyframe_queue，可能导致数据不一致或崩溃。使用互斥量确保对 keyframe_queue 的操作是原子性的，即一个线程在操作时，其他线程必须等待
+
+    if(keyframe_queue.empty()) {  // 如果关键帧队列是空的，发布一个 ROS 消息通知系统继续读取点云数据，直到指定时间点（stamp + ros::Duration(10, 0)）。这段代码确保在没有关键帧生成时，系统能够继续读取点云数据
+      // LOG(INFO) << "keyframe_queue.empty()";
+      std_msgs::Header read_until;
+      read_until.stamp = stamp + ros::Duration(10, 0);
+      read_until.frame_id = points_topic;
+      read_until_pub.publish(read_until);
+      read_until.frame_id = "/filtered_points";
+      read_until_pub.publish(read_until);
+    }
+    return;
+  }
+
+  double accum_d = keyframe_updater->get_accum_distance();  // 获取累计的运动距离，用于判断关键帧生成的条件
+  hdl_graph_slam::KeyFrame::Ptr keyframe(new hdl_graph_slam::KeyFrame(stamp, odom, accum_d, cloud));  // 创建一个新的关键帧 keyframe，其中包含当前的时间戳 stamp，里程计信息 odom，累计距离 accum_d，以及处理后的点云数据 cloud（这里的处理就是做了个消息类型的转换）
+
+  std::lock_guard<std::mutex> lock(keyframe_queue_mutex);  // 使用 std::lock_guard 加锁，确保对关键帧队列 keyframe_queue 的操作是线程安全的
+  keyframe_queue.push_back(keyframe);                      // 将新生成的关键帧 keyframe 推入 keyframe_queue 队列，供后续的处理使用
+  // LOG(INFO) << "keyframe_queue.push_back(keyframe)";
+}
+
+/**
+ * @brief save map data as pcd
+ * @param req
+ * @param res
+ * @return
+ */
+bool hdl_backend::save_map_service(hdl_graph_slam::SaveMapRequest& req, hdl_graph_slam::SaveMapResponse& res) {
+  std::vector<hdl_graph_slam::KeyFrameSnapshot::Ptr> snapshot;
 
-    pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());  // 创建一个点云对象的指针
-    pcl::fromROSMsg(*cloud_msg, *cloud);                                // 将ROS的点云消息 cloud_msg 转换为PCL（Point Cloud Library）的点云格式
-    if(base_frame_id.empty()) { // 用类的单例实现应该就ok?  
-      // 明天再试一下
+  keyframes_snapshot_mutex.lock();
+  snapshot = keyframes_snapshot;
+  keyframes_snapshot_mutex.unlock();
 
+  auto cloud = map_cloud_generator->generate(snapshot, req.resolution);
+  if(!cloud) {
+    res.success = false;
+    return true;
+  }
 
-      base_frame_id = cloud_msg->header.frame_id;  // 将当前点云消息的坐标系 frame_id 设置为基础坐标系      base_frame_id 是整个系统中关键帧数据的参考坐标系
+  if(zero_utm && req.utm) {
+    for(auto& pt : cloud->points) {
+      pt.getVector3fMap() += (*zero_utm).cast<float>();
     }
+  }
 
-    // 更新关键帧判断
-    if(!keyframe_updater->update(odom)) {                      // 根据当前的里程计信息 odom 判断是否需要生成新的关键帧
-      std::lock_guard<std::mutex> lock(keyframe_queue_mutex);  // 这行代码的作用是确保线程安全，防止多个线程同时访问或修改 keyframe_queue 队列
-      // std::lock_guard<std::mutex> 是 C++ 标准库中的一个类，用来简化互斥锁（std::mutex）的使用。它的作用是在其作用域内自动对给定的互斥量加锁，并在作用域结束时自动解锁
-      // keyframe_queue_mutex 是一个互斥量（std::mutex），用于保护关键帧队列 keyframe_queue。当多个线程试图同时访问或修改 keyframe_queue 时，互斥量确保只有一个线程能够访问该资源，避免数据竞争和并发问题
-      // 工作原理：当这行代码执行时，lock_guard 会锁住 keyframe_queue_mutex，使其他线程无法访问与之关联的资源（即 keyframe_queue） 当程序执行离开该作用域时，lock_guard 会自动释放锁，无需显式调用 unlock() 函数
-      // 为什么需要锁？   在多线程环境中（比如 ROS 回调函数可能被不同线程调用），如果多个线程同时读取或写入 keyframe_queue，可能导致数据不一致或崩溃。使用互斥量确保对 keyframe_queue 的操作是原子性的，即一个线程在操作时，其他线程必须等待
-      
-      if(keyframe_queue.empty()) {  // 如果关键帧队列是空的，发布一个 ROS 消息通知系统继续读取点云数据，直到指定时间点（stamp + ros::Duration(10, 0)）。这段代码确保在没有关键帧生成时，系统能够继续读取点云数据
-        std_msgs::Header read_until;
-        read_until.stamp = stamp + ros::Duration(10, 0);
-        read_until.frame_id = points_topic;
-        read_until_pub.publish(read_until);
-        read_until.frame_id = "/filtered_points";
-        read_until_pub.publish(read_until);
+  cloud->header.frame_id = map_frame_id;
+  cloud->header.stamp = snapshot.back()->cloud->header.stamp;
+
+  if(zero_utm) {
+    std::ofstream ofs(req.destination + ".utm");
+    ofs << boost::format("%.6f %.6f %.6f") % zero_utm->x() % zero_utm->y() % zero_utm->z() << std::endl;
+  }
+  int ret = pcl::io::savePCDFileBinary(req.destination, *cloud);
+  res.success = ret == 0;
+
+  return true;
+}
+
+/**
+ * @brief this method adds all the keyframes in #keyframe_queue to the pose graph (odometry edges)    此方法将#keyframe_queue中的所有关键帧添加到姿势图中（里程计边）
+ * @return if true, at least one keyframe was added to the pose graph   如果为真，则至少有一个关键帧被添加到姿势图中
+ * 将队列中的关键帧添加到位姿图中，并返回是否至少有一个关键帧被添加
+ *
+ * pose graph  是什么？
+ */
+bool hdl_backend::flush_keyframe_queue() {
+  std::lock_guard<std::mutex> lock(keyframe_queue_mutex);  // 多线程锁
+
+  LOG(INFO) << "flush_keyframe_queue";
+
+  if(keyframe_queue.empty()) {  // 没有关键帧就返回
+    LOG(INFO) << "flush_keyframe_queue: keyframe_queue.empty()";
+    return false;
+  }
+  LOG(INFO) << "flush_keyframe_queue: keyframe_queue.empty() ===========";
+
+  trans_odom2map_mutex.lock();
+  Eigen::Isometry3d odom2map(trans_odom2map.cast<double>());
+  trans_odom2map_mutex.unlock();
+
+  std::cout << "flush_keyframe_queue - keyframes len:" << keyframes.size() << std::endl;
+  int num_processed = 0;
+  for(int i = 0; i < std::min<int>(keyframe_queue.size(), max_keyframes_per_update); i++) {
+    num_processed = i;
+
+    const auto& keyframe = keyframe_queue[i];
+    // new_keyframes will be tested later for loop closure
+    new_keyframes.push_back(keyframe);
+
+    // add pose node
+    Eigen::Isometry3d odom = odom2map * keyframe->odom;
+    keyframe->node = graph_slam->add_se3_node(odom);
+    keyframe_hash[keyframe->stamp] = keyframe;
+
+    // fix the first node
+    if(keyframes.empty() && new_keyframes.size() == 1) {
+      if(nh.param<bool>("fix_first_node", false)) {
+        Eigen::MatrixXd inf = Eigen::MatrixXd::Identity(6, 6);
+        std::stringstream sst(nh.param<std::string>("fix_first_node_stddev", "1 1 1 1 1 1"));
+        for(int i = 0; i < 6; i++) {
+          double stddev = 1.0;
+          sst >> stddev;
+          inf(i, i) = 1.0 / stddev;
+        }
+
+        anchor_node = graph_slam->add_se3_node(Eigen::Isometry3d::Identity());
+        anchor_node->setFixed(true);
+        anchor_edge = graph_slam->add_se3_edge(anchor_node, keyframe->node, Eigen::Isometry3d::Identity(), inf);
+      }
+    }
+
+    if(i == 0 && keyframes.empty()) {
+      continue;
+    }
+
+    // add edge between consecutive keyframes
+    const auto& prev_keyframe = i == 0 ? keyframes.back() : keyframe_queue[i - 1];
+
+    Eigen::Isometry3d relative_pose = keyframe->odom.inverse() * prev_keyframe->odom;
+    Eigen::MatrixXd information = inf_calclator->calc_information_matrix(keyframe->cloud, prev_keyframe->cloud, relative_pose);
+    auto edge = graph_slam->add_se3_edge(keyframe->node, prev_keyframe->node, relative_pose, information);
+    graph_slam->add_robust_kernel(edge, "NONE", 1.0);
+  }
+
+  std_msgs::Header read_until;
+  read_until.stamp = keyframe_queue[num_processed]->stamp + ros::Duration(10, 0);
+  read_until.frame_id = points_topic;
+  read_until_pub.publish(read_until);
+  read_until.frame_id = "/filtered_points";
+  read_until_pub.publish(read_until);
+
+  keyframe_queue.erase(keyframe_queue.begin(), keyframe_queue.begin() + num_processed + 1);
+  return true;
+}
+
+/**
+ * @brief create visualization marker
+ * @param stamp
+ * @return
+ */
+visualization_msgs::MarkerArray hdl_backend::create_marker_array(const ros::Time& stamp) const {
+  visualization_msgs::MarkerArray markers;
+  markers.markers.resize(4);
+
+  // node markers
+  visualization_msgs::Marker& traj_marker = markers.markers[0];
+  traj_marker.header.frame_id = "map";
+  traj_marker.header.stamp = stamp;
+  traj_marker.ns = "nodes";
+  traj_marker.id = 0;
+  traj_marker.type = visualization_msgs::Marker::SPHERE_LIST;
+
+  traj_marker.pose.orientation.w = 1.0;
+  traj_marker.scale.x = traj_marker.scale.y = traj_marker.scale.z = 0.5;
+
+  visualization_msgs::Marker& imu_marker = markers.markers[1];
+  imu_marker.header = traj_marker.header;
+  imu_marker.ns = "imu";
+  imu_marker.id = 1;
+  imu_marker.type = visualization_msgs::Marker::SPHERE_LIST;
+
+  imu_marker.pose.orientation.w = 1.0;
+  imu_marker.scale.x = imu_marker.scale.y = imu_marker.scale.z = 0.75;
+
+  traj_marker.points.resize(keyframes.size());
+  traj_marker.colors.resize(keyframes.size());
+  for(int i = 0; i < keyframes.size(); i++) {
+    Eigen::Vector3d pos = keyframes[i]->node->estimate().translation();
+    traj_marker.points[i].x = pos.x();
+    traj_marker.points[i].y = pos.y();
+    traj_marker.points[i].z = pos.z();
+
+    double p = static_cast<double>(i) / keyframes.size();
+    traj_marker.colors[i].r = 1.0 - p;
+    traj_marker.colors[i].g = p;
+    traj_marker.colors[i].b = 0.0;
+    traj_marker.colors[i].a = 1.0;
+
+    if(keyframes[i]->acceleration) {
+      Eigen::Vector3d pos = keyframes[i]->node->estimate().translation();
+      geometry_msgs::Point point;
+      point.x = pos.x();
+      point.y = pos.y();
+      point.z = pos.z();
+
+      std_msgs::ColorRGBA color;
+      color.r = 0.0;
+      color.g = 0.0;
+      color.b = 1.0;
+      color.a = 0.1;
+
+      imu_marker.points.push_back(point);
+      imu_marker.colors.push_back(color);
+    }
+  }
+
+  // edge markers
+  visualization_msgs::Marker& edge_marker = markers.markers[2];
+  edge_marker.header.frame_id = "map";
+  edge_marker.header.stamp = stamp;
+  edge_marker.ns = "edges";
+  edge_marker.id = 2;
+  edge_marker.type = visualization_msgs::Marker::LINE_LIST;
+
+  edge_marker.pose.orientation.w = 1.0;
+  edge_marker.scale.x = 0.05;
+
+  edge_marker.points.resize(graph_slam->graph->edges().size() * 2);
+  edge_marker.colors.resize(graph_slam->graph->edges().size() * 2);
+
+  auto edge_itr = graph_slam->graph->edges().begin();
+  for(int i = 0; edge_itr != graph_slam->graph->edges().end(); edge_itr++, i++) {
+    g2o::HyperGraph::Edge* edge = *edge_itr;
+    g2o::EdgeSE3* edge_se3 = dynamic_cast<g2o::EdgeSE3*>(edge);
+    if(edge_se3) {
+      g2o::VertexSE3* v1 = dynamic_cast<g2o::VertexSE3*>(edge_se3->vertices()[0]);
+      g2o::VertexSE3* v2 = dynamic_cast<g2o::VertexSE3*>(edge_se3->vertices()[1]);
+      Eigen::Vector3d pt1 = v1->estimate().translation();
+      Eigen::Vector3d pt2 = v2->estimate().translation();
+
+      edge_marker.points[i * 2].x = pt1.x();
+      edge_marker.points[i * 2].y = pt1.y();
+      edge_marker.points[i * 2].z = pt1.z();
+      edge_marker.points[i * 2 + 1].x = pt2.x();
+      edge_marker.points[i * 2 + 1].y = pt2.y();
+      edge_marker.points[i * 2 + 1].z = pt2.z();
+
+      double p1 = static_cast<double>(v1->id()) / graph_slam->graph->vertices().size();
+      double p2 = static_cast<double>(v2->id()) / graph_slam->graph->vertices().size();
+      edge_marker.colors[i * 2].r = 1.0 - p1;
+      edge_marker.colors[i * 2].g = p1;
+      edge_marker.colors[i * 2].a = 1.0;
+      edge_marker.colors[i * 2 + 1].r = 1.0 - p2;
+      edge_marker.colors[i * 2 + 1].g = p2;
+      edge_marker.colors[i * 2 + 1].a = 1.0;
+
+      if(std::abs(v1->id() - v2->id()) > 2) {
+        edge_marker.points[i * 2].z += 0.5;
+        edge_marker.points[i * 2 + 1].z += 0.5;
       }
 
-      return;
+      continue;
+    }
+
+    g2o::EdgeSE3Plane* edge_plane = dynamic_cast<g2o::EdgeSE3Plane*>(edge);
+    if(edge_plane) {
+      g2o::VertexSE3* v1 = dynamic_cast<g2o::VertexSE3*>(edge_plane->vertices()[0]);
+      Eigen::Vector3d pt1 = v1->estimate().translation();
+      Eigen::Vector3d pt2(pt1.x(), pt1.y(), 0.0);
+
+      edge_marker.points[i * 2].x = pt1.x();
+      edge_marker.points[i * 2].y = pt1.y();
+      edge_marker.points[i * 2].z = pt1.z();
+      edge_marker.points[i * 2 + 1].x = pt2.x();
+      edge_marker.points[i * 2 + 1].y = pt2.y();
+      edge_marker.points[i * 2 + 1].z = pt2.z();
+
+      edge_marker.colors[i * 2].b = 1.0;
+      edge_marker.colors[i * 2].a = 1.0;
+      edge_marker.colors[i * 2 + 1].b = 1.0;
+      edge_marker.colors[i * 2 + 1].a = 1.0;
+
+      continue;
+    }
+
+    g2o::EdgeSE3PriorXY* edge_priori_xy = dynamic_cast<g2o::EdgeSE3PriorXY*>(edge);
+    if(edge_priori_xy) {
+      g2o::VertexSE3* v1 = dynamic_cast<g2o::VertexSE3*>(edge_priori_xy->vertices()[0]);
+      Eigen::Vector3d pt1 = v1->estimate().translation();
+      Eigen::Vector3d pt2 = Eigen::Vector3d::Zero();
+      pt2.head<2>() = edge_priori_xy->measurement();
+
+      edge_marker.points[i * 2].x = pt1.x();
+      edge_marker.points[i * 2].y = pt1.y();
+      edge_marker.points[i * 2].z = pt1.z() + 0.5;
+      edge_marker.points[i * 2 + 1].x = pt2.x();
+      edge_marker.points[i * 2 + 1].y = pt2.y();
+      edge_marker.points[i * 2 + 1].z = pt2.z() + 0.5;
+
+      edge_marker.colors[i * 2].r = 1.0;
+      edge_marker.colors[i * 2].a = 1.0;
+      edge_marker.colors[i * 2 + 1].r = 1.0;
+      edge_marker.colors[i * 2 + 1].a = 1.0;
+
+      continue;
+    }
+
+    g2o::EdgeSE3PriorXYZ* edge_priori_xyz = dynamic_cast<g2o::EdgeSE3PriorXYZ*>(edge);
+    if(edge_priori_xyz) {
+      g2o::VertexSE3* v1 = dynamic_cast<g2o::VertexSE3*>(edge_priori_xyz->vertices()[0]);
+      Eigen::Vector3d pt1 = v1->estimate().translation();
+      Eigen::Vector3d pt2 = edge_priori_xyz->measurement();
+
+      edge_marker.points[i * 2].x = pt1.x();
+      edge_marker.points[i * 2].y = pt1.y();
+      edge_marker.points[i * 2].z = pt1.z() + 0.5;
+      edge_marker.points[i * 2 + 1].x = pt2.x();
+      edge_marker.points[i * 2 + 1].y = pt2.y();
+      edge_marker.points[i * 2 + 1].z = pt2.z();
+
+      edge_marker.colors[i * 2].r = 1.0;
+      edge_marker.colors[i * 2].a = 1.0;
+      edge_marker.colors[i * 2 + 1].r = 1.0;
+      edge_marker.colors[i * 2 + 1].a = 1.0;
+
+      continue;
     }
+  }
 
-    double accum_d = keyframe_updater->get_accum_distance();  // 获取累计的运动距离，用于判断关键帧生成的条件
-    KeyFrame::Ptr keyframe(new KeyFrame(stamp, odom, accum_d, cloud));  // 创建一个新的关键帧 keyframe，其中包含当前的时间戳 stamp，里程计信息 odom，累计距离 accum_d，以及处理后的点云数据 cloud（这里的处理就是做了个消息类型的转换）
+  // sphere
+  visualization_msgs::Marker& sphere_marker = markers.markers[3];
+  sphere_marker.header.frame_id = "map";
+  sphere_marker.header.stamp = stamp;
+  sphere_marker.ns = "loop_close_radius";
+  sphere_marker.id = 3;
+  sphere_marker.type = visualization_msgs::Marker::SPHERE;
 
-    std::lock_guard<std::mutex> lock(keyframe_queue_mutex);  // 使用 std::lock_guard 加锁，确保对关键帧队列 keyframe_queue 的操作是线程安全的
-    keyframe_queue.push_back(keyframe);                      // 将新生成的关键帧 keyframe 推入 keyframe_queue 队列，供后续的处理使用
+  if(!keyframes.empty()) {
+    Eigen::Vector3d pos = keyframes.back()->node->estimate().translation();
+    sphere_marker.pose.position.x = pos.x();
+    sphere_marker.pose.position.y = pos.y();
+    sphere_marker.pose.position.z = pos.z();
+  }
+  sphere_marker.pose.orientation.w = 1.0;
+  sphere_marker.scale.x = sphere_marker.scale.y = sphere_marker.scale.z = loop_detector->get_distance_thresh() * 2.0;
 
+  sphere_marker.color.r = 1.0;
+  sphere_marker.color.a = 0.3;
 
+  return markers;
 }

src/hdl_graph_slam/map_cloud_generator.cpp

@@ -21,6 +21,8 @@ pcl::PointCloud<MapCloudGenerator::PointT>::Ptr MapCloudGenerator::generate(cons
 
   for(const auto& keyframe : keyframes) {
     Eigen::Matrix4f pose = keyframe->pose.matrix().cast<float>();
+    std::cout << "pose: ===================:" << std::endl;
+    std::cout << pose << std::endl;
     for(const auto& src_pt : keyframe->cloud->points) {
       PointT dst_pt;
       dst_pt.getVector4fMap() = pose * src_pt.getVector4fMap();
@@ -33,8 +35,7 @@ pcl::PointCloud<MapCloudGenerator::PointT>::Ptr MapCloudGenerator::generate(cons
   cloud->height = 1;
   cloud->is_dense = false;
 
-  if (resolution <=0.0)
-    return cloud; // To get unfiltered point cloud with intensity
+  if(resolution <= 0.0) return cloud;  // To get unfiltered point cloud with intensity
 
   pcl::octree::OctreePointCloud<PointT> octree(resolution);
   octree.setInputCloud(cloud);

src/laser_odom.cpp

@@ -31,6 +31,7 @@
 
 #include <laser_geometry/laser_geometry.h>
 
+#include <tf2/LinearMath/Quaternion.h>
 
 int main(int argc, char* argv[]) {
   // 执行 ros 节点初始化
@@ -55,8 +56,8 @@ void Laser_Odom::init() {
   auto voxelgrid = new pcl::VoxelGrid<PointT>();
   voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);
   downsample_filter.reset(voxelgrid);
-  
-  registration->setNumThreads(0);   // 把 registration = hdl_graph_slam::select_registration_method(nh);  核心部分弄出来
+
+  registration->setNumThreads(0);  // 把 registration = hdl_graph_slam::select_registration_method(nh);  核心部分弄出来
   registration->setTransformationEpsilon(0.1);
   registration->setMaximumIterations(64);
   registration->setMaxCorrespondenceDistance(2.0);
@@ -65,19 +66,19 @@ void Laser_Odom::init() {
   odom_pub = nh.advertise<nav_msgs::Odometry>("/laser_odom", 32);  // 发布机器人的里程计信息
   // keyframe_pub = nh.advertise<pcl::PointCloud<PointT>>("/keyframe", 32);
   // read_until_pub = nh.advertise<std_msgs::Header>("/keyframe", 32);
-  aligned_points_pub = nh.advertise<sensor_msgs::PointCloud2>("/aligned_points", 32);                      // 对齐后的点云
+  aligned_points_pub = nh.advertise<sensor_msgs::PointCloud2>("/aligned_points", 32);  // 对齐后的点云
   map_points_pub = nh.advertise<sensor_msgs::PointCloud2>("/map_points", 1, true);
 
   ros::Subscriber sub = nh.subscribe<sensor_msgs::LaserScan>("/scan", 10, Laser_Odom::cloud_callback);
   ros::spin();
 }
 
-Laser_Odom* Laser_Odom::laser_odom = nullptr;  // 在 .cpp 文件中定义静态成员变量  
+Laser_Odom* Laser_Odom::laser_odom = nullptr;  // 在 .cpp 文件中定义静态成员变量
 // 这种实现方法不算单例实现，如果有多个laser_odom的实例，this指针都会被覆盖，指向最后创建的那个实例
 // 这里就不改了吧,后端不能这么写,因为后端要多线程
 // 多线程实现中有一个跟这个很类似的,是 懒汉式（线程不安全） 实现, 它是在给 laser_odom 赋值前先判断是不是等于 nullptr , 如果是才赋值,否则直接返回laser_odom
 // 但多线程很有可能同时满足 laser_odom == nullptr, 然后创建多个实例
-// 但其实我的cpp文件只跑一次main而已, 多线程问题在这里不会出现呀, 
+// 但其实我的cpp文件只跑一次main而已, 多线程问题在这里不会出现呀,
 void Laser_Odom::cloud_callback(const sensor_msgs::LaserScan::ConstPtr& laser_scan_msg) {
   if(!ros::ok()) {
     return;
@@ -85,16 +86,16 @@ void Laser_Odom::cloud_callback(const sensor_msgs::LaserScan::ConstPtr& laser_sc
   // 消息类型转换
   laser_odom->projector.projectLaser(*laser_scan_msg, laser_odom->cloud2);
   pcl::fromROSMsg(laser_odom->cloud2, *laser_odom->cloud);
-  
+
   // 点云匹配
   Eigen::Matrix4f pose = laser_odom->matching(laser_scan_msg->header.stamp, laser_odom->cloud);  // 点云匹配函数，返回
 
   // 发布激光里程计数据
-  laser_odom->publish_odometry(laser_scan_msg->header.stamp, laser_scan_msg->header.frame_id, pose);  
+  laser_odom->publish_odometry(laser_scan_msg->header.stamp, laser_scan_msg->header.frame_id, pose);
 
-  // 发布对齐后的点晕
+  // 发布对齐后的点云
   pcl::PointCloud<PointT>::Ptr aligned(new pcl::PointCloud<PointT>());
-  pcl::transformPointCloud(*laser_odom->cloud, *aligned, laser_odom-> odom);
+  pcl::transformPointCloud(*laser_odom->cloud, *aligned, laser_odom->odom);
   aligned->header.frame_id = "odom";
   laser_odom->aligned_points_pub.publish(*aligned);
 
@@ -115,7 +116,7 @@ Eigen::Matrix4f Laser_Odom::matching(const ros::Time& stamp, const pcl::PointClo
     keyframe_pose.setIdentity();             // 关键帧的初始位资
     keyframe_stamp = stamp;                  // 关键帧的时间戳
     keyframe = downsample(cloud);            // 对点云数据进行下采样，减少点的数量，提高处理效率和精度
-    registration->setInputTarget(keyframe);  // 将 keyframe 设置成关键帧  
+    registration->setInputTarget(keyframe);  // 将 keyframe 设置成关键帧
     return Eigen::Matrix4f::Identity();      // 没有之前的位资信息，假设与上次位资没有发生变化，返回单位矩阵
 
     // registration 对象负责计算输入点云（cloud）与已有关键帧（keyframe）之间的相对变换。这种变换估算允许系统理解传感器（如激光雷达）在两次扫描之间的移动
@@ -138,14 +139,13 @@ Eigen::Matrix4f Laser_Odom::matching(const ros::Time& stamp, const pcl::PointClo
   }
 
   Eigen::Matrix4f trans = registration->getFinalTransformation();  // 获得当前点云和上一帧点云 关键帧 的仿射变换
-  odom = keyframe_pose * trans;                    // 算出来 odom
+  odom = keyframe_pose * trans;                                    // 算出来 odom
 
   prev_time = stamp;   // 当前帧的时间戳
   prev_trans = trans;  // 当前帧的仿射变换
 
   auto keyframe_trans = matrix2transform(stamp, keyframe_pose, "odom", "keyframe");  // 将变换矩阵转换为tf::Transform对象，用于发布关键帧的变换
-  keyframe_broadcaster.sendTransform(keyframe_trans);                                       // 发布关键帧的变换(这个是发送到哪里?  )
-
+  keyframe_broadcaster.sendTransform(keyframe_trans);                                // 发布关键帧的变换(这个是发送到哪里?  )
 
   double delta_trans = trans.block<3, 1>(0, 3).norm();                                                                // 计算 当前帧 与 关键帧 变换的 平移距离
   double delta_angle = std::acos(Eigen::Quaternionf(trans.block<3, 3>(0, 0)).w());                                    // 计算 当前帧 与 关键帧 变换的 旋转角度
@@ -210,68 +210,85 @@ geometry_msgs::TransformStamped Laser_Odom::matrix2transform(const ros::Time& st
   return odom_trans;
 }
 
+/**
+ * @brief publish odometry
+ * @param stamp  timestamp
+ * @param pose   odometry pose to be published
+ */
+void Laser_Odom::publish_odometry(const ros::Time& stamp, const std::string& base_frame_id, const Eigen::Matrix4f& pose) {  // 发布里程计数据
+  // publish transform stamped for IMU integration
+  // geometry_msgs::TransformStamped odom_trans = matrix2transform(stamp, pose, odom_frame_id, base_frame_id);
+  geometry_msgs::TransformStamped odom_trans = matrix2transform(stamp, pose, "odom", base_frame_id);
+  // trans_pub.publish(odom_trans);
+
+  // // broadcast the transform over tf
+  // odom_broadcaster.sendTransform(odom_trans);
+
+  // publish the transform
+  nav_msgs::Odometry odom;
+  odom.header.stamp = stamp;
+  odom.header.frame_id = "odom";
+  odom.child_frame_id = "base_link";
+
+  odom.pose.pose.position.x = pose(0, 3);
+  odom.pose.pose.position.y = pose(1, 3);
+  odom.pose.pose.position.z = pose(2, 3);
+  odom.pose.pose.orientation = odom_trans.transform.rotation;
+
+  odom.child_frame_id = base_frame_id;
+  odom.twist.twist.linear.x = 0.0;
+  odom.twist.twist.linear.y = 0.0;
+  odom.twist.twist.angular.z = 0.0;
+
+
+  // 上面算出来的odom朝向有问题,需要顺时针转90度修正,代码如下:
+  // 提取原始朝向
+  tf2::Quaternion original_orientation(odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, odom.pose.pose.orientation.z, odom.pose.pose.orientation.w);
+
+  // 定义顺时针旋转90度的四元数
+  tf2::Quaternion rotation_90(0, 0, -0.7071, 0.7071);
+
+  // 计算旋转后的朝向
+  tf2::Quaternion new_orientation = rotation_90 * original_orientation;
+  new_orientation.normalize();  // 确保结果为单位四元数
+
+  // 将旋转后的朝向赋值回原变量
+  odom.pose.pose.orientation.x = new_orientation.x();
+  odom.pose.pose.orientation.y = new_orientation.y();
+  odom.pose.pose.orientation.z = new_orientation.z();
+  odom.pose.pose.orientation.w = new_orientation.w();
+
+  odom_pub.publish(odom);
+}
 
-  /**
-   * @brief publish odometry
-   * @param stamp  timestamp
-   * @param pose   odometry pose to be published
-   */
-  void Laser_Odom::publish_odometry(const ros::Time& stamp, const std::string& base_frame_id, const Eigen::Matrix4f& pose) {  // 发布里程计数据
-    // publish transform stamped for IMU integration
-    // geometry_msgs::TransformStamped odom_trans = matrix2transform(stamp, pose, odom_frame_id, base_frame_id);
-    geometry_msgs::TransformStamped odom_trans = matrix2transform(stamp, pose, "odom", base_frame_id);
-    // trans_pub.publish(odom_trans);
-
-    // // broadcast the transform over tf
-    // odom_broadcaster.sendTransform(odom_trans);
-
-    // publish the transform
-    nav_msgs::Odometry odom;
-    odom.header.stamp = stamp;
-    odom.header.frame_id = "odom";
-
-    odom.pose.pose.position.x = pose(0, 3);
-    odom.pose.pose.position.y = pose(1, 3);
-    odom.pose.pose.position.z = pose(2, 3);
-    odom.pose.pose.orientation = odom_trans.transform.rotation;
-
-    odom.child_frame_id = base_frame_id;
-    odom.twist.twist.linear.x = 0.0;
-    odom.twist.twist.linear.y = 0.0;
-    odom.twist.twist.angular.z = 0.0;
-
-    odom_pub.publish(odom);
-  }
-
-
-  // /**
-  //  * @brief publish odometry
-  //  * @param stamp  timestamp
-  //  * @param pose   odometry pose to be published
-  //  */
-  // void Laser_Odom::publish_keyfrrame(const ros::Time& stamp, const std::string& base_frame_id, const Eigen::Matrix4f& pose) {  // 发布里程计数据
-  //   // publish transform stamped for IMU integration
-  //   // geometry_msgs::TransformStamped odom_trans = matrix2transform(stamp, pose, odom_frame_id, base_frame_id);
-  //   geometry_msgs::TransformStamped odom_trans = matrix2transform(stamp, pose, "odom", base_frame_id);
-  //   // trans_pub.publish(odom_trans);
-
-  //   // // broadcast the transform over tf
-  //   // odom_broadcaster.sendTransform(odom_trans);
-
-  //   // publish the transform
-  //   nav_msgs::Odometry odom;
-  //   odom.header.stamp = stamp;
-  //   odom.header.frame_id = "odom";
-
-  //   odom.pose.pose.position.x = pose(0, 3);
-  //   odom.pose.pose.position.y = pose(1, 3);
-  //   odom.pose.pose.position.z = pose(2, 3);
-  //   odom.pose.pose.orientation = odom_trans.transform.rotation;
-
-  //   odom.child_frame_id = base_frame_id;
-  //   odom.twist.twist.linear.x = 0.0;
-  //   odom.twist.twist.linear.y = 0.0;
-  //   odom.twist.twist.angular.z = 0.0;
-
-  //   odom_pub.publish(odom);
-  // }
+// /**
+//  * @brief publish odometry
+//  * @param stamp  timestamp
+//  * @param pose   odometry pose to be published
+//  */
+// void Laser_Odom::publish_keyfrrame(const ros::Time& stamp, const std::string& base_frame_id, const Eigen::Matrix4f& pose) {  // 发布里程计数据
+//   // publish transform stamped for IMU integration
+//   // geometry_msgs::TransformStamped odom_trans = matrix2transform(stamp, pose, odom_frame_id, base_frame_id);
+//   geometry_msgs::TransformStamped odom_trans = matrix2transform(stamp, pose, "odom", base_frame_id);
+//   // trans_pub.publish(odom_trans);
+
+//   // // broadcast the transform over tf
+//   // odom_broadcaster.sendTransform(odom_trans);
+
+//   // publish the transform
+//   nav_msgs::Odometry odom;
+//   odom.header.stamp = stamp;
+//   odom.header.frame_id = "odom";
+
+//   odom.pose.pose.position.x = pose(0, 3);
+//   odom.pose.pose.position.y = pose(1, 3);
+//   odom.pose.pose.position.z = pose(2, 3);
+//   odom.pose.pose.orientation = odom_trans.transform.rotation;
+
+//   odom.child_frame_id = base_frame_id;
+//   odom.twist.twist.linear.x = 0.0;
+//   odom.twist.twist.linear.y = 0.0;
+//   odom.twist.twist.angular.z = 0.0;
+
+//   odom_pub.publish(odom);
+// }

笔记.md

@@ -0,0 +1,110 @@
+## 参考资料
+
+1. hdl_graph_slam包中的 README.md 文件
+2. [CSDN博客：hdl_graph_slam](https://blog.csdn.net/Seinlvan/article/details/119115908)
+
+
+
+
+
+
+
+
+
+## 消息类型
+
+#### 话题：/scan
+
+Type: sensor_msgs/LaserScan
+
+发布频率: 20 hz
+
+angle_min, angle_max：雷达扫描的最小角度和最大角度
+
+angle_increment：扫描点之间的角度增量，用来确定扫描的分辨率
+
+time_increment：扫描点之间的时间增量，如果每个扫描点对应一个测量时间，则可以用来计算扫描的频率
+
+scan_time：完成一次扫描所需要的时间
+
+rangle_min, rang_max：雷达测量的最大和最小范围（距离），距离范围外的反射波视为无效
+
+**ranges：包含每个扫描点测距值的数组。数组的长度对应扫描分辨率，每个值表示从雷达到检测到的物体的直线距离**
+
+intensitiess：每个扫描点的反射强度值数组，可以用于判断物体的材质或用于后期处理，如滤波等
+
+![2024-09-25 08-35-49 的屏幕截图](/home/mj/图片/2024-09-25 08-35-49 的屏幕截图.png)
+
+
+
+
+
+
+
+### 后端实现, 节点名称: hdl_backend
+
+```
+Node [/hdl_backend]
+Publications: 
+ * /hdl_graph_slam/map_points [sensor_msgs/PointCloud2]
+ * /hdl_graph_slam/markers [visualization_msgs/MarkerArray]
+ * /hdl_graph_slam/odom2pub [geometry_msgs/TransformStamped]
+ * /hdl_graph_slam/read_until [std_msgs/Header]
+ * /rosout [rosgraph_msgs/Log]
+
+Subscriptions: 
+ * /aligned_points [sensor_msgs/PointCloud2]
+ * /clock [rosgraph_msgs/Clock]
+ * /laser_odom [nav_msgs/Odometry]
+
+Services: 
+ * /hdl_backend/get_loggers
+ * /hdl_backend/set_logger_level
+ * /hdl_graph_slam/save_map
+```
+
+
+
+何师兄给的数据中, 回放的时候是有里程计数据的,话题名称是/odom    但后端不用这个数据,是用前端匹配的/laser_odom
+
+
+
+自己实现的/laser_odom的消息:
+
+```
+---
+header: 
+  seq: 3616
+  stamp: 
+    secs: 1626936142
+    nsecs: 297182351
+  frame_id: "odom"
+child_frame_id: "laser"
+pose: 
+  pose: 
+    position: 					位置
+      x: -22.39583396911621
+      y: 3.211024045944214
+      z: 0.0
+    orientation: 				朝向,这里错了要改
+      x: 0.0
+      y: 0.0
+      z: -0.8115295767784119
+      w: 0.584311306476593
+  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+twist: 
+  twist: 
+    linear: 
+      x: 0.0
+      y: 0.0
+      z: 0.0
+    angular: 
+      x: 0.0
+      y: 0.0
+      z: 0.0
+  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+---
+```
+
+
+