前端写好了

CMakeLists.txt

@@ -13,6 +13,9 @@ endif()
 
 set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_LIST_DIR}/cmake")
 
+add_definitions("-Wall -g")   # 启动调试模式
+SET(CMAKE_BUILD_TYPE Debug)
+
 # pcl 1.7 causes a segfault when it is built with debug mode
 set(CMAKE_BUILD_TYPE "RELEASE")
 
@@ -103,13 +106,22 @@ include_directories(
 )
 
 # nodelets
+add_executable(laser2cloud src/laser2cloud.cpp)
+# add_library(laser_odom src/laser_odom.cpp)
+target_link_libraries(laser2cloud
+  ${catkin_LIBRARIES}
+  ${PCL_LIBRARIES}
+)
+
 add_executable(laser_odom src/laser_odom.cpp)
 # add_library(laser_odom src/laser_odom.cpp)
 target_link_libraries(laser_odom
   ${catkin_LIBRARIES}
   ${PCL_LIBRARIES}
+  glog
 )
 
+
 add_executable(hdl_backend
   src/hdl_backend.cpp
   src/hdl_graph_slam/map_cloud_generator.cpp
@@ -189,6 +201,7 @@ target_link_libraries(hdl_graph_slam_nodelet
   ${G2O_SOLVER_CHOLMOD}   # be aware of that cholmod is released under GPL
   ${G2O_TYPES_SLAM3D}
   ${G2O_TYPES_SLAM3D_ADDONS}
+  glog
 )
 add_dependencies(hdl_graph_slam_nodelet ${PROJECT_NAME}_gencpp)
 

apps/hdl_graph_slam_nodelet.cpp

@@ -679,6 +679,7 @@ private:
     keyframes_snapshot.swap(snapshot);
     keyframes_snapshot_mutex.unlock();
     graph_updated = true;
+    
 
     if(odom2map_pub.getNumSubscribers()) {
       geometry_msgs::TransformStamped ts = matrix2transform(keyframe->stamp, trans.matrix().cast<float>(), map_frame_id, odom_frame_id);

apps/scan_matching_odometry_nodelet.cpp

@@ -28,6 +28,9 @@
 #include <hdl_graph_slam/registrations.hpp>
 #include "hdl_graph_slam/ScanMatchingStatus.h"
 
+#include "sensor_msgs/LaserScan.h"
+
+
 namespace hdl_graph_slam {
 
 class ScanMatchingOdometryNodelet : public nodelet::Nodelet {
@@ -53,6 +56,7 @@ public:
     }
 
     points_sub = nh.subscribe("/filtered_points", 256, &ScanMatchingOdometryNodelet::cloud_callback, this);  // 接收过滤后的点云数据
+    // points_sub = nh.subscribe("/scan", 256, &ScanMatchingOdometryNodelet::cloud_callback, this);  // 接收过滤后的点云数据
     read_until_pub = nh.advertise<std_msgs::Header>("/scan_matching_odometry/read_until", 32);               //
     odom_pub = nh.advertise<nav_msgs::Odometry>(published_odom_topic, 32);                                   // 发布机器人的里程计信息
     trans_pub = nh.advertise<geometry_msgs::TransformStamped>("/scan_matching_odometry/transform", 32);      //
@@ -146,6 +150,7 @@ private:
    * @brief callback for point clouds
    * @param cloud_msg  point cloud msg
    */
+  
   void cloud_callback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg) {  // cloud_msg：滤波后的点云数据
     if(!ros::ok()) {
       return;
@@ -312,7 +317,7 @@ private:
       aligned->header.frame_id = odom_frame_id;
       aligned_points_pub.publish(*aligned);
     }
-    std::cout << "The matrix odom is: \n" << odom << std::endl;
+    // std::cout << "The matrix odom is: \n" << odom << std::endl;
     return odom;  // 返回里程计
   }
 

include/laser_odom copy.hpp

@@ -0,0 +1,89 @@
+#include <memory>
+#include <iostream>
+
+#include <ros/ros.h>
+#include <ros/time.h>
+#include <ros/duration.h>
+#include <pcl_ros/point_cloud.h>
+#include <tf_conversions/tf_eigen.h>
+#include <tf/transform_listener.h>
+#include <tf/transform_broadcaster.h>
+
+#include <std_msgs/Time.h>
+#include <nav_msgs/Odometry.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+
+#include <nodelet/nodelet.h>
+#include <pluginlib/class_list_macros.h>
+
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/filters/passthrough.h>
+#include <pcl/filters/approximate_voxel_grid.h>
+
+#include <hdl_graph_slam/ros_utils.hpp>
+#include <hdl_graph_slam/registrations.hpp>
+#include <hdl_graph_slam/ScanMatchingStatus.h>
+
+#include <laser_geometry/laser_geometry.h>
+#include <fast_gicp/gicp/fast_gicp.hpp>
+#include <glog/logging.h>
+#include <new>
+#undef new
+
+// namespace laser_odom_ns {
+typedef pcl::PointXYZI PointT;
+
+class Laser_Odom {
+private:
+  laser_geometry::LaserProjection projector;
+  sensor_msgs::PointCloud2 cloud2;             // 点云数据类型转换的中间变量
+  pcl::PointCloud<PointT>::Ptr cloud;          // 点云数据
+  pcl::PointCloud<PointT>::ConstPtr keyframe;  // 关键帧
+  ros::Time prev_time;
+  Eigen::Matrix4f prev_trans;     // 地图起点到当前   帧  的放射变换
+  Eigen::Matrix4f keyframe_pose;  // 地图起点到当前 关键帧 的仿射变换
+  ros::Time keyframe_stamp;       // 关键帧的时间戳
+  pcl::Registration<PointT, PointT>::Ptr registration;
+  // fast_gicp::FastGICP<PointT, PointT>::Ptr registration;
+  
+  pcl::Filter<PointT>::Ptr downsample_filter;
+
+  double keyframe_delta_trans = 0.5;
+  double keyframe_delta_angle = 0.5;
+  double keyframe_delta_time = 10.0;
+  // std::string odom_frame_id = "odom";
+  tf::TransformBroadcaster keyframe_broadcaster;
+  tf::TransformBroadcaster odom_broadcaster;
+
+  ros::NodeHandle nh;
+  ros::Publisher odom_pub;      // 里程计发布
+  ros::Publisher path_pub;
+  // ros::Publisher read_until_pub;  //
+  ros::Publisher source_points_pub;
+  ros::Publisher map_points_pub;
+
+  static Laser_Odom* laser_odom;
+  Eigen::Matrix4f odom;
+
+  double downsample_resolution = 0.1;  // 下采样分辨率
+
+  ros::NodeHandle private_nh;
+
+  pcl::PointCloud<PointT>::Ptr trans_cloud; 
+
+public:
+  Laser_Odom();
+  ~Laser_Odom();
+  void init();
+  static void cloud_callback(const sensor_msgs::LaserScan::ConstPtr& laser_scan_msg);
+  Eigen::Matrix4f matching(const ros::Time& stamp, const pcl::PointCloud<PointT>::ConstPtr& cloud);
+  pcl::PointCloud<PointT>::ConstPtr downsample(const pcl::PointCloud<PointT>::ConstPtr& cloud);
+  geometry_msgs::TransformStamped matrix2transform(const ros::Time& stamp, const Eigen::Matrix4f& pose, const std::string& frame_id, const std::string& child_frame_id);
+  void publish_odometry(const ros::Time& stamp, const std::string& base_frame_id, const Eigen::Matrix4f& pose);
+  void publish_keyfrrame(const ros::Time& stamp, const std::string& base_frame_id, const Eigen::Matrix4f& pose);  // 发布里程计数据
+
+
+};
+// };  // namespace laser_odom_ns

include/laser_odom.hpp

@@ -1,84 +1,107 @@
-#include <memory>
-#include <iostream>
-
-#include <ros/ros.h>
-#include <ros/time.h>
-#include <ros/duration.h>
-#include <pcl_ros/point_cloud.h>
-#include <tf_conversions/tf_eigen.h>
-#include <tf/transform_listener.h>
-#include <tf/transform_broadcaster.h>
-
-#include <std_msgs/Time.h>
-#include <nav_msgs/Odometry.h>
-#include <sensor_msgs/PointCloud2.h>
-#include <geometry_msgs/TransformStamped.h>
-#include <geometry_msgs/PoseWithCovarianceStamped.h>
-
-#include <nodelet/nodelet.h>
-#include <pluginlib/class_list_macros.h>
-
-#include <pcl/filters/voxel_grid.h>
-#include <pcl/filters/passthrough.h>
-#include <pcl/filters/approximate_voxel_grid.h>
-
-#include <hdl_graph_slam/ros_utils.hpp>
-#include <hdl_graph_slam/registrations.hpp>
-#include <hdl_graph_slam/ScanMatchingStatus.h>
-
-#include <laser_geometry/laser_geometry.h>
-#include <fast_gicp/gicp/fast_gicp.hpp>
-
-#include <new>
-#undef new
-
-// namespace laser_odom_ns {
-typedef pcl::PointXYZI PointT;
-
-class Laser_Odom {
-private:
-  laser_geometry::LaserProjection projector;
-  sensor_msgs::PointCloud2 cloud2;             // 点云数据类型转换的中间变量
-  pcl::PointCloud<PointT>::Ptr cloud;          // 点云数据
-  pcl::PointCloud<PointT>::ConstPtr keyframe;  // 关键帧
-  ros::Time prev_time;
-  Eigen::Matrix4f prev_trans;     // 地图起点到当前   帧  的放射变换
-  Eigen::Matrix4f keyframe_pose;  // 地图起点到当前 关键帧 的仿射变换
-  ros::Time keyframe_stamp;       // 关键帧的时间戳
-  // pcl::Registration<PointT, PointT>::Ptr registration;
-  fast_gicp::FastGICP<PointT, PointT>::Ptr registration;
-  pcl::Filter<PointT>::Ptr downsample_filter;
-
-  double keyframe_delta_trans = 0.25;
-  double keyframe_delta_angle = 0.15;
-  double keyframe_delta_time = 1.0;
-  // std::string odom_frame_id = "odom";
-  tf::TransformBroadcaster keyframe_broadcaster;
-  tf::TransformBroadcaster odom_broadcaster;
-
-  ros::NodeHandle nh;
-  ros::Publisher odom_pub;      // 里程计发布
-  ros::Publisher path_pub;
-  // ros::Publisher read_until_pub;  //
-  ros::Publisher aligned_points_pub;
-  ros::Publisher map_points_pub;
-
-  static Laser_Odom* laser_odom;
-  Eigen::Matrix4f odom;
-
-  double downsample_resolution = 0.1;  // 下采样分辨率
-
-  ros::NodeHandle private_nh;
-
-public:
-  Laser_Odom();
-  ~Laser_Odom();
-  void init();
-  static void cloud_callback(const sensor_msgs::LaserScan::ConstPtr& laser_scan_msg);
-  Eigen::Matrix4f matching(const ros::Time& stamp, const pcl::PointCloud<PointT>::ConstPtr& cloud);
-  pcl::PointCloud<PointT>::ConstPtr downsample(const pcl::PointCloud<PointT>::ConstPtr& cloud);
-  geometry_msgs::TransformStamped matrix2transform(const ros::Time& stamp, const Eigen::Matrix4f& pose, const std::string& frame_id, const std::string& child_frame_id);
-  void publish_odometry(const ros::Time& stamp, const std::string& base_frame_id, const Eigen::Matrix4f& pose);
-  void publish_keyfrrame(const ros::Time& stamp, const std::string& base_frame_id, const Eigen::Matrix4f& pose);  // 发布里程计数据
-};
-// };  // namespace laser_odom_ns
+    #include <memory>
+    #include <iostream>
+
+    #include <ros/ros.h>
+    #include <ros/time.h>
+    #include <ros/duration.h>
+    #include <pcl_ros/point_cloud.h>
+    #include <tf_conversions/tf_eigen.h>
+    #include <tf/transform_listener.h>
+    #include <tf/transform_broadcaster.h>
+
+    #include <std_msgs/Time.h>
+    #include <nav_msgs/Odometry.h>
+    #include <sensor_msgs/PointCloud2.h>
+    #include <geometry_msgs/TransformStamped.h>
+    #include <geometry_msgs/PoseWithCovarianceStamped.h>
+
+    #include <nodelet/nodelet.h>
+    #include <pluginlib/class_list_macros.h>
+
+    #include <pcl/filters/voxel_grid.h>
+    #include <pcl/filters/passthrough.h>
+    #include <pcl/filters/approximate_voxel_grid.h>
+
+    #include <hdl_graph_slam/ros_utils.hpp>
+    #include <hdl_graph_slam/registrations.hpp>
+    #include <hdl_graph_slam/ScanMatchingStatus.h>
+
+    #include <laser_geometry/laser_geometry.h>
+    #include <fast_gicp/gicp/fast_gicp.hpp>
+
+    #include <queue>
+    #include <thread>
+    #include <mutex>
+    #include <glog/logging.h>
+
+
+    #include <new>
+    #undef new
+    
+    
+    typedef pcl::PointXYZI PointT;
+
+    struct MatchPCL {
+      ros::Time time;
+      pcl::PointCloud<PointT> cloud;
+    };
+
+    // namespace laser_odom_ns {
+
+    class Laser_Odom {
+    private:
+      laser_geometry::LaserProjection projector;
+      sensor_msgs::PointCloud2 cloud2;             // 点云数据类型转换的中间变量
+      pcl::PointCloud<PointT>::Ptr cloud;          // 点云数据
+      pcl::PointCloud<PointT>::Ptr trans_cloud;          // 点云数据
+
+      pcl::PointCloud<PointT>::ConstPtr keyframe;  // 关键帧
+      ros::Time prev_time;
+      Eigen::Matrix4f prev_trans;     // 地图起点到当前   帧  的放射变换
+      Eigen::Matrix4f keyframe_pose;  // 地图起点到当前 关键帧 的仿射变换
+      ros::Time keyframe_stamp;       // 关键帧的时间戳
+      // pcl::Registration<PointT, PointT>::Ptr registration;
+      fast_gicp::FastGICP<PointT, PointT>::Ptr registration;
+      pcl::Filter<PointT>::Ptr downsample_filter;
+
+      double keyframe_delta_trans = 0.25;
+      double keyframe_delta_angle = 0.15;
+      double keyframe_delta_time = 1.0;
+      // std::string odom_frame_id = "odom";
+      tf::TransformBroadcaster keyframe_broadcaster;
+      tf::TransformBroadcaster odom_broadcaster;
+
+      ros::NodeHandle nh;
+      ros::Publisher odom_pub;      // 里程计发布
+      ros::Publisher path_pub;
+      // ros::Publisher read_until_pub;  //
+      ros::Publisher aligned_points_pub;
+      ros::Publisher map_points_pub;
+      ros::Publisher keyframe_points_pub;
+
+      static Laser_Odom* laser_odom;
+      Eigen::Matrix4f odom;
+
+      double downsample_resolution = 0.1;  // 下采样分辨率
+
+      ros::NodeHandle private_nh;
+
+    private:
+      void procThread();
+
+      std::mutex mutex_;
+
+      std::queue<MatchPCL> clouds_;
+
+    public:
+      Laser_Odom();
+      ~Laser_Odom();
+      void init();
+      void cloud_callback(const sensor_msgs::LaserScan::ConstPtr& laser_scan_msg);
+      Eigen::Matrix4f matching(const ros::Time& stamp, const pcl::PointCloud<PointT>::ConstPtr& cloud);
+      pcl::PointCloud<PointT>::ConstPtr downsample(const pcl::PointCloud<PointT>::ConstPtr& cloud);
+      geometry_msgs::TransformStamped matrix2transform(const ros::Time& stamp, const Eigen::Matrix4f& pose, const std::string& frame_id, const std::string& child_frame_id);
+      void publish_odometry(const ros::Time& stamp, const std::string& base_frame_id, const Eigen::Matrix4f& pose);
+      void publish_keyfrrame(const ros::Time& stamp, const std::string& base_frame_id, const Eigen::Matrix4f& pose);  // 发布里程计数据
+    };
+    // };  // namespace laser_odom_ns

launch/hdl_graph_slam_400.launch

@@ -49,11 +49,15 @@
   <!-- 使用仿真时间 -->
   <param name="use_sim_time" value="true" />
   <!-- 回放rosbag文件 -->
-  <node pkg="rosbag" type="play" name="player" output="screen" args="--clock /home/mj/hdl_slam/hdl_400.bag"/>
+  <!-- /home/mj/hdl_slam/hdl_400.bag -->
+  
+  <node pkg="rosbag" type="play" name="player" output="screen" args="--clock --start 20 /home/mj/hdl_slam/hdl_400.bag"/>
+  <node pkg="hdl_graph_slam" type="laser2cloud" name="laser2cloud" output="log" />
 
-<!-- 线程池   用另外一个线程实现后端 -->
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find hdl_graph_slam)/rviz/hdl_graph_slam.rviz"/>  
 
 
+<!-- 线程池   用另外一个线程实现后端 -->
 
   <!-- arguments -->
   <arg name="nodelet_manager" default="velodyne_nodelet_manager" />

launch/hdl_graph_slam_400_copy.launch

@@ -0,0 +1,217 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- 
+    
+    前面4个节点的代码文件在hdl_graph_slam/apps/...   是在ROS中注册的插件
+
+    prefiltering_nodelet:               ！！！！ 点云滤波  ！！！！
+        包（pkg）: nodelet
+        类型（type）: nodelet
+        名称（name）: prefiltering_nodelet
+        作用：加载并执行hdl_graph_slam包中的PrefilteringNodelet，用于对激光雷达点云数据进行预处理，包括距离滤波、下采样和离群点移除。
+
+    scan_matching_odometry_nodelet:     ！！！！帧间匹配，也就是激光里程计（前端里程计） ！！！！
+        包（pkg）: nodelet
+        类型（type）: nodelet
+        名称（name）: scan_matching_odometry_nodelet
+        作用：加载并执行hdl_graph_slam包中的ScanMatchingOdometryNodelet，用于通过扫描匹配进行里程计估计。
+
+    floor_detection_nodelet (条件启动，取决于 enable_floor_detection 参数):           ！！！！地平面检测 ！！！！
+        包（pkg）: nodelet
+        类型（type）: nodelet
+        名称（name）: floor_detection_nodelet
+        作用：加载并执行hdl_graph_slam包中的FloorDetectionNodelet，用于检测地面平面。
+
+    hdl_graph_slam_nodelet:             ！！！！后端概率图构建  ！！！！
+        包（pkg）: nodelet
+        类型（type）: nodelet
+        名称（name）: hdl_graph_slam_nodelet
+        作用：加载并执行hdl_graph_slam包中的HdlGraphSlamNodelet，用于执行基于图的SLAM（Simultaneous Localization and Mapping）。
+
+    lidar2base_publisher:
+        包（pkg）: tf
+        类型（type）: static_transform_publisher
+        作用：发布从base_link到velodyne（激光雷达）的静态坐标变换。这里的参数0 0 0 0 0 0表示没有平移和旋转，即激光雷达与base_link坐标系重合。
+
+    velodyne_nodelet_manager:
+        包（pkg）: nodelet
+        类型（type）: nodelet
+        名称（name）: velodyne_nodelet_manager
+        作用：作为一个节点管理器，用于管理其他节点（nodelet）。
+
+    map2odom_publisher:
+        包（pkg）: hdl_graph_slam
+        类型（type）: map2odom_publisher.py
+        名称（name）: map2odom_publisher
+        作用：这是一个Python脚本，用于发布从地图到里程计的变换。
+  -->
+
+  <!-- 使用仿真时间 -->
+  <param name="use_sim_time" value="true" />
+  <!-- 回放rosbag文件 -->
+  <!-- /home/mj/hdl_slam/hdl_400.bag -->
+  
+  <node pkg="rosbag" type="play" name="player" output="screen" args="--clock /home/mj/hdl_slam/hdl_400.bag"/>
+  <node pkg="hdl_graph_slam" type="laser2cloud" name="laser2cloud" output="log" />
+
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find hdl_graph_slam)/rviz/hdl_graph_slam.rviz"/>  
+
+
+<!-- 线程池   用另外一个线程实现后端 -->
+
+
+
+  <!-- arguments -->
+  <arg name="nodelet_manager" default="velodyne_nodelet_manager" />
+  <arg name="enable_floor_detection" default="true" />
+  <arg name="enable_gps" default="false" />
+  <arg name="enable_imu_acc" default="false" />
+  <arg name="enable_imu_ori" default="false" />
+
+  <arg name="points_topic" default="/velodyne_points" />
+  <arg name="map_frame_id" default="map" />
+  <arg name="lidar_odom_frame_id" default="odom" />
+
+  <!-- optional arguments -->
+  <arg name="enable_robot_odometry_init_guess" default="false" />
+  <arg name="robot_odom_frame_id" default="robot_odom" />
+
+  <!-- transformation between lidar and base_link -->
+  <node pkg="tf" type="static_transform_publisher" name="lidar2base_publisher" args="0 0 0 0 0 0 base_link velodyne 10" />
+
+  <!-- in case you use velodyne_driver, comment out the following line -->
+  <node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager)" args="manager" output="screen"/>
+
+  <!-- prefiltering_nodelet -->
+  <node pkg="nodelet" type="nodelet" name="prefiltering_nodelet" args="load hdl_graph_slam/PrefilteringNodelet $(arg nodelet_manager)">
+    <remap from="/velodyne_points" to="$(arg points_topic)" />
+    <!-- in case base_link_frame is blank, mapping will be performed in the lidar frame -->
+    <param name="base_link_frame" value="base_link" />
+    <!-- distance filter -->
+    <param name="use_distance_filter" value="true" />
+    <param name="distance_near_thresh" value="0.1" />
+    <param name="distance_far_thresh" value="100.0" />
+    <!-- NONE, VOXELGRID, or APPROX_VOXELGRID -->
+    <param name="downsample_method" value="VOXELGRID" />
+    <param name="downsample_resolution" value="0.1" />
+    <!-- NONE, RADIUS, or STATISTICAL -->
+    <param name="outlier_removal_method" value="RADIUS" />
+    <param name="statistical_mean_k" value="30" />
+    <param name="statistical_stddev" value="1.2" />
+    <param name="radius_radius" value="0.5" />
+    <param name="radius_min_neighbors" value="2" />
+  </node>
+
+  <!-- scan_matching_odometry_nodelet -->
+  <node pkg="nodelet" type="nodelet" name="scan_matching_odometry_nodelet" args="load hdl_graph_slam/ScanMatchingOdometryNodelet $(arg nodelet_manager)" output="screen">
+    <param name="points_topic" value="$(arg points_topic)" />
+    <param name="odom_frame_id" value="$(arg lidar_odom_frame_id)" />
+    <param name="robot_odom_frame_id" value="$(arg robot_odom_frame_id)" />
+    <param name="keyframe_delta_trans" value="1.0" />
+    <param name="keyframe_delta_angle" value="1.0" />
+    <param name="keyframe_delta_time" value="10000.0" />
+    <param name="transform_thresholding" value="false" />
+    <param name="enable_robot_odometry_init_guess" value="$(arg enable_robot_odometry_init_guess)" />
+    <param name="max_acceptable_trans" value="1.0" />
+    <param name="max_acceptable_angle" value="1.0" />
+    <param name="downsample_method" value="NONE" />
+    <param name="downsample_resolution" value="0.1" />
+    <!-- ICP, GICP, NDT, GICP_OMP, NDT_OMP, FAST_GICP(recommended), or FAST_VGICP -->
+    <param name="registration_method" value="FAST_GICP" />
+    <param name="reg_num_threads" value="0" />
+    <param name="reg_transformation_epsilon" value="0.1"/>
+    <param name="reg_maximum_iterations" value="64"/>
+    <param name="reg_max_correspondence_distance" value="2.0"/>
+    <param name="reg_max_optimizer_iterations" value="20"/>
+    <param name="reg_use_reciprocal_correspondences" value="false"/>
+    <param name="reg_correspondence_randomness" value="20"/>
+    <param name="reg_resolution" value="1.0" />
+    <param name="reg_nn_search_method" value="DIRECT7" />
+  </node>
+
+  <!-- floor_detection_nodelet -->
+  <node pkg="nodelet" type="nodelet" name="floor_detection_nodelet" args="load hdl_graph_slam/FloorDetectionNodelet $(arg nodelet_manager)" if="$(arg enable_floor_detection)">
+    <param name="points_topic" value="$(arg points_topic)" />
+    <param name="tilt_deg" value="0.0" />
+    <param name="sensor_height" value="2.0" />
+    <param name="height_clip_range" value="1.0" />
+    <param name="floor_pts_thresh" value="512" />
+    <param name="use_normal_filtering" value="true" />
+    <param name="normal_filter_thresh" value="20.0" />
+  </node>
+
+  <!-- hdl_graph_slam_nodelet -->
+  <node pkg="nodelet" type="nodelet" name="hdl_graph_slam_nodelet" args="load hdl_graph_slam/HdlGraphSlamNodelet $(arg nodelet_manager)" output="screen">
+    <param name="points_topic" value="$(arg points_topic)" />
+    <!-- frame settings -->
+    <param name="map_frame_id" value="$(arg map_frame_id)" />
+    <param name="odom_frame_id" value="$(arg lidar_odom_frame_id)" />
+    <!-- optimization params -->
+    <!-- typical solvers: gn_var, gn_fix6_3, gn_var_cholmod, lm_var, lm_fix6_3, lm_var_cholmod, ... -->
+    <param name="g2o_solver_type" value="lm_var_cholmod" />
+    <param name="g2o_solver_num_iterations" value="512" />
+    <!-- constraint switches -->
+    <param name="enable_gps" value="$(arg enable_gps)" />
+    <param name="enable_imu_acceleration" value="$(arg enable_imu_acc)" />
+    <param name="enable_imu_orientation" value="$(arg enable_imu_ori)" />
+    <!-- keyframe registration params -->
+    <param name="max_keyframes_per_update" value="10" />
+    <param name="keyframe_delta_trans" value="2.0" />
+    <param name="keyframe_delta_angle" value="2.0" />
+    <!-- fix first node for optimization stability -->
+    <param name="fix_first_node" value="true"/>
+    <param name="fix_first_node_stddev" value="10 10 10 1 1 1"/>
+    <param name="fix_first_node_adaptive" value="true"/>
+    <!-- loop closure params -->
+    <param name="distance_thresh" value="15.0" />
+    <param name="accum_distance_thresh" value="25.0" />
+    <param name="min_edge_interval" value="15.0" />
+    <param name="fitness_score_thresh" value="2.5" />
+    <!-- scan matching params -->
+    <param name="registration_method" value="FAST_GICP" />
+    <param name="reg_num_threads" value="0" />
+    <param name="reg_transformation_epsilon" value="0.1"/>
+    <param name="reg_maximum_iterations" value="64"/>
+    <param name="reg_max_correspondence_distance" value="2.0"/>
+    <param name="reg_max_optimizer_iterations" value="20"/>
+    <param name="reg_use_reciprocal_correspondences" value="false"/>
+    <param name="reg_correspondence_randomness" value="20"/>
+    <param name="reg_resolution" value="1.0" />
+    <param name="reg_nn_search_method" value="DIRECT7" />
+    <!-- edge params -->
+    <!-- GPS -->
+    <param name="gps_edge_robust_kernel" value="NONE" />
+    <param name="gps_edge_robust_kernel_size" value="1.0" />
+    <param name="gps_edge_stddev_xy" value="20.0" />
+    <param name="gps_edge_stddev_z" value="5.0" />
+    <!-- IMU orientation -->
+    <param name="imu_orientation_edge_robust_kernel" value="NONE" />
+    <param name="imu_orientation_edge_stddev" value="1.0" />
+    <!-- IMU acceleration (gravity vector) -->
+    <param name="imu_acceleration_edge_robust_kernel" value="NONE" />
+    <param name="imu_acceleration_edge_stddev" value="1.0" />
+    <!-- ground plane -->
+    <param name="floor_edge_robust_kernel" value="NONE" />
+    <param name="floor_edge_stddev" value="10.0" />
+    <!-- scan matching -->
+    <!-- robust kernels: NONE, Cauchy, DCS, Fair, GemanMcClure, Huber, PseudoHuber, Saturated, Tukey, Welsch -->
+    <param name="odometry_edge_robust_kernel" value="NONE" />
+    <param name="odometry_edge_robust_kernel_size" value="1.0" />
+    <param name="loop_closure_edge_robust_kernel" value="Huber" />
+    <param name="loop_closure_edge_robust_kernel_size" value="1.0" />
+    <param name="use_const_inf_matrix" value="false" />
+    <param name="const_stddev_x" value="0.5" />
+    <param name="const_stddev_q" value="0.1" />
+    <param name="var_gain_a" value="20.0" />
+    <param name="min_stddev_x" value="0.1" />
+    <param name="max_stddev_x" value="5.0" />
+    <param name="min_stddev_q" value="0.05" />
+    <param name="max_stddev_q" value="0.2" />
+    <!-- update params -->
+    <param name="graph_update_interval" value="3.0" />
+    <param name="map_cloud_update_interval" value="10.0" />
+    <param name="map_cloud_resolution" value="0.05" />
+  </node>
+
+  <node pkg="hdl_graph_slam" type="map2odom_publisher.py" name="map2odom_publisher"  output="screen"/>
+</launch>

launch/hdl_graph_slam_501.launch

@@ -1,5 +1,18 @@
 <?xml version="1.0"?>
 <launch>
+
+  <!-- 使用仿真时间 -->
+  <param name="use_sim_time" value="true" />
+  <!-- 回放rosbag文件 -->
+  <!-- /home/mj/hdl_slam/hdl_400.bag -->
+  
+  <node pkg="rosbag" type="play" name="player" output="screen" args="--clock --start 20 /home/mj/hdl_slam/hdl_501.bag"/>
+  <node pkg="hdl_graph_slam" type="laser2cloud" name="laser2cloud" output="log" />
+
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find hdl_graph_slam)/rviz/hdl_graph_slam.rviz"/>  
+
+
+
   <!-- arguments -->
   <arg name="nodelet_manager" default="velodyne_nodelet_manager" />
   <arg name="enable_floor_detection" default="true" />

launch/start.launch

@@ -1,12 +1,14 @@
 <launch>
-    <node pkg="hdl_graph_slam" type="laser_odom" name="laser_odom" output="log" />
+    <node pkg="hdl_graph_slam" type="laser_odom" name="laser_odom" output="screen" />
     <node pkg="hdl_graph_slam" type="hdl_backend" name="hdl_backend" output="screen" />
-    
+    <!-- <node pkg="hdl_graph_slam" type="laser2cloud" name="laser2cloud" output="screen" /> -->
+
     <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="log" 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"/>
 
     <node pkg="hdl_graph_slam" type="map2odom_publisher.py" name="map2odom_publisher"  output="screen"/>

src/hdl_backend.cpp

@@ -44,7 +44,7 @@ void hdl_backend::init() {
   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类型的消息。
+  cloud_sub.reset(new message_filters::Subscriber<sensor_msgs::PointCloud2>(nh, "/keyframe_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成员函数
 
@@ -74,7 +74,6 @@ void hdl_backend::init() {
  * @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);  // 创建了一个锁，用于保护对主线程的访问，确保线程安全
 
@@ -91,7 +90,6 @@ void hdl_backend::optimization_timer_callback(const ros::WallTimerEvent& event)
   }
   if(!keyframe_updated) {
     // 检查地板、GPS、IMU等队列，分别调用相应的 flush 函数（例如 flush_floor_queue、flush_gps_queue、flush_imu_queue），如果这些数据没有更新，则函数直接返回，不进行后续操作。
-    LOG(INFO) << "关键帧没有更新,退出optimization_timer_callback函数";
     return;
   }
   
@@ -106,7 +104,6 @@ void hdl_backend::optimization_timer_callback(const ros::WallTimerEvent& event)
     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) << "完成闭环检测";
 
   std::copy(new_keyframes.begin(), new_keyframes.end(), std::back_inserter(keyframes));  // 将新关键帧 new_keyframes 合并到已有的关键帧列表 keyframes 中
   new_keyframes.clear();                                                                 // 清空 new_keyframes
@@ -122,7 +119,6 @@ void hdl_backend::optimization_timer_callback(const ros::WallTimerEvent& event)
   // optimize the pose graph
   int num_iterations = 512;              // launch文件中都是设置成512
   graph_slam->optimize(num_iterations);  // 使用 g2o 优化器对位姿图进行优化，优化的迭代次数由参数 "g2o_solver_num_iterations" 控制
-  LOG(INFO) << "完成g2o的图优化";
 
   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); });
@@ -131,7 +127,6 @@ void hdl_backend::optimization_timer_callback(const ros::WallTimerEvent& event)
   keyframes_snapshot.swap(snapshot);
   // keyframes_snapshot_mutex.unlock();
   graph_updated = true;
-  LOG(INFO) << "准备发布TF变换";
 
   // publish tf     发布位姿变换
   const auto& keyframe = keyframes.back();  // 获取最新的关键帧估计
@@ -144,13 +139,11 @@ void hdl_backend::optimization_timer_callback(const ros::WallTimerEvent& event)
   geometry_msgs::TransformStamped ts = hdl_graph_slam::matrix2transform(keyframe->stamp, trans.matrix().cast<float>(), map_frame_id, odom_frame_id);
   // ts.header.stamp = ros::Time::now();   // ?
   // odom2map_pub.publish(ts);  // 发布odom2map_pub话题中/
-  LOG(INFO) << "odom2map TF变换发布完成";
   // }
 
   if(markers_pub.getNumSubscribers()) {
     auto markers = create_marker_array(ros::Time::now());
     markers_pub.publish(markers);
-    LOG(INFO) << "markers_pub 发布完成";
   }
 
 
@@ -161,44 +154,10 @@ void hdl_backend::optimization_timer_callback(const ros::WallTimerEvent& event)
  * @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"; //
-
   // 发布点云数据
-  LOG(INFO) << "准备发布点云数据";
   std::vector<hdl_graph_slam::KeyFrameSnapshot::Ptr> snapshot;
   snapshot = keyframes_snapshot;
   auto cloud = map_cloud_generator->generate(snapshot, map_cloud_resolution);
-  LOG(INFO) << "获得点云数据";
   if(!cloud) {
     return;
   }
@@ -207,12 +166,7 @@ void hdl_backend::map_points_publish_timer_callback(const ros::WallTimerEvent& e
 
   sensor_msgs::PointCloud2Ptr cloud_msg(new sensor_msgs::PointCloud2());
   pcl::toROSMsg(*cloud, *cloud_msg);
-
   map_points_pub.publish(*cloud_msg);
-  LOG(INFO) << "点云数据发布完成";
-
-  LOG(INFO) << "map_points_publish_timer_callback 函数执行完毕";
-
 }
 
 /**

src/laser2cloud copy.cpp

@@ -0,0 +1,35 @@
+#include <ros/ros.h>
+#include <sensor_msgs/LaserScan.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <laser_geometry/laser_geometry.h>
+
+class LaserScanToPointCloud {
+public:
+    LaserScanToPointCloud() {
+        // 初始化订阅者和发布者
+        scan_sub_ = nh_.subscribe("/scan", 10, &LaserScanToPointCloud::scanCallback, this);
+        cloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/filtered_points", 10);
+    }
+
+private:
+    void scanCallback(const sensor_msgs::LaserScan::ConstPtr& laser_scan) {
+        sensor_msgs::PointCloud2 cloud;
+        laser_proj_.projectLaser(*laser_scan, cloud);  // 转换 LaserScan 到 PointCloud2
+
+        // 发布转换后的点云
+        cloud_pub_.publish(cloud);
+    }
+
+    ros::NodeHandle nh_;
+    ros::Subscriber scan_sub_;
+    ros::Publisher cloud_pub_;
+    laser_geometry::LaserProjection laser_proj_;  // 激光投影对象
+};
+
+int main(int argc, char** argv) {
+    ros::init(argc, argv, "laser_scan_to_point_cloud");
+    LaserScanToPointCloud converter;
+
+    ros::spin();  // 进入循环等待回调
+    return 0;
+}

src/laser2cloud.cpp

@@ -0,0 +1,74 @@
+#include <ros/ros.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/LaserScan.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+class PointCloudToLaserScan
+{
+public:
+    PointCloudToLaserScan()
+    {
+        // 创建订阅者和发布者
+        pointcloud_sub_ = nh_.subscribe("/velodyne_points", 1, &PointCloudToLaserScan::pointCloudCallback, this);
+        laser_scan_pub_ = nh_.advertise<sensor_msgs::LaserScan>("/scan", 1);
+    }
+
+private:
+    ros::NodeHandle nh_;
+    ros::Subscriber pointcloud_sub_;
+    ros::Publisher laser_scan_pub_;
+
+    void pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
+    {
+        // 转换 PointCloud2 消息到 pcl::PointCloud
+        pcl::PointCloud<pcl::PointXYZ> cloud;
+        pcl::fromROSMsg(*cloud_msg, cloud);
+
+        // 创建 LaserScan 消息
+        sensor_msgs::LaserScan laser_scan;
+        laser_scan.header = cloud_msg->header; // 保持相同的时间戳和坐标系
+        laser_scan.angle_min = -M_PI / 2; // 设置最小角度
+        laser_scan.angle_max = M_PI / 2;   // 设置最大角度
+        laser_scan.angle_increment = M_PI / 180; // 每个激光束之间的角度增量
+        laser_scan.range_min = 0.0; // 最小范围
+        laser_scan.range_max = 100.0; // 最大范围
+
+        // 计算 LaserScan 数据
+        int num_readings = static_cast<int>((laser_scan.angle_max - laser_scan.angle_min) / laser_scan.angle_increment);
+        laser_scan.ranges.resize(num_readings);
+        
+        // 遍历点云，将其转换为激光扫描范围
+        for (int i = 0; i < num_readings; ++i)
+        {
+            float angle = laser_scan.angle_min + i * laser_scan.angle_increment;
+            // 找到与当前角度最接近的点
+            float min_range = laser_scan.range_max;
+            for (const auto& point : cloud.points)
+            {
+                float point_angle = atan2(point.y, point.x);
+                if (fabs(point_angle - angle) < laser_scan.angle_increment / 2)
+                {
+                    float range = sqrt(point.x * point.x + point.y * point.y);
+                    if (range < min_range)
+                    {
+                        min_range = range;
+                    }
+                }
+            }
+            laser_scan.ranges[i] = min_range;
+        }
+
+        // 发布 LaserScan 消息
+        laser_scan_pub_.publish(laser_scan);
+    }
+};
+
+int main(int argc, char** argv)
+{
+    ros::init(argc, argv, "pointcloud_to_laserscan");
+    PointCloudToLaserScan pcl_to_laser;
+    ros::spin();
+    return 0;
+}

src/laser_odom copy 2.cpp

@@ -0,0 +1,311 @@
+    #include "laser_odom.hpp"
+    #include "hdl_graph_slam/registrations.hpp"
+
+    #include <memory>
+    #include <iostream>
+
+    #include <ros/ros.h>
+    #include <ros/time.h>
+    #include <ros/duration.h>
+    #include <pcl_ros/point_cloud.h>
+    #include <tf_conversions/tf_eigen.h>
+    #include <tf/transform_listener.h>
+    #include <tf/transform_broadcaster.h>
+
+    #include <std_msgs/Time.h>
+    #include <nav_msgs/Odometry.h>
+    #include <sensor_msgs/PointCloud2.h>
+    #include <geometry_msgs/TransformStamped.h>
+    #include <geometry_msgs/PoseWithCovarianceStamped.h>
+
+    #include <nodelet/nodelet.h>
+    #include <pluginlib/class_list_macros.h>
+
+    #include <pcl/filters/voxel_grid.h>
+    #include <pcl/filters/passthrough.h>
+    #include <pcl/filters/approximate_voxel_grid.h>
+
+    #include <hdl_graph_slam/ros_utils.hpp>
+    #include <hdl_graph_slam/registrations.hpp>
+    #include <hdl_graph_slam/ScanMatchingStatus.h>
+
+    #include <laser_geometry/laser_geometry.h>
+    #include <nav_msgs/Path.h>
+
+    #include <tf2/LinearMath/Quaternion.h>
+
+    int main(int argc, char* argv[]) {
+      // 执行 ros 节点初始化
+      ros::init(argc, argv, "laser_odom");
+
+      Laser_Odom laser_odom;
+      laser_odom.init();
+      // odom_pub = nh.advertise<nav_msgs::Odometry>("/laser_odom", 32);  // 发布机器人的里程计信息
+
+      return 0;
+    }
+
+    Laser_Odom::Laser_Odom() {
+      laser_odom = this;
+      cloud.reset(new pcl::PointCloud<PointT>());
+      registration.reset(new fast_gicp::FastGICP<PointT, PointT>());
+    }
+
+    Laser_Odom::~Laser_Odom() {}
+
+    void Laser_Odom::init() {
+      auto voxelgrid = new pcl::VoxelGrid<PointT>();
+      voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);
+      downsample_filter.reset(voxelgrid);
+
+      registration->setNumThreads(16);  // 把 registration = hdl_graph_slam::select_registration_method(nh);  核心部分弄出来
+      registration->setTransformationEpsilon(1e-15);
+      registration->setMaximumIterations(500);
+      registration->setMaxCorrespondenceDistance(2.0);
+      registration->setCorrespondenceRandomness(20);
+
+      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);  // 对齐后的点云
+      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::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_points", 10, Laser_Odom::cloud_callback);
+      ros::spin();
+    }
+
+    Laser_Odom* laser_odom = nullptr;  // 在 .cpp 文件中定义静态成员变量
+    // 这种实现方法不算单例实现，如果有多个laser_odom的实例，this指针都会被覆盖，指向最后创建的那个实例
+    // 这里就不改了吧,后端不能这么写,因为后端要多线程
+    // 多线程实现中有一个跟这个很类似的,是 懒汉式（线程不安全） 实现, 它是在给 laser_odom 赋值前先判断是不是等于 nullptr , 如果是才赋值,否则直接返回laser_odom
+    // 但多线程很有可能同时满足 laser_odom == nullptr, 然后创建多个实例
+    // 但其实我的cpp文件只跑一次main而已, 多线程问题在这里不会出现呀,
+    void Laser_Odom::cloud_callback(const sensor_msgs::LaserScan::ConstPtr& laser_scan_msg) {
+      if(!ros::ok()) {
+        return;
+      }
+      // 消息类型转换
+      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);
+
+      // 发布对齐后的点云
+
+      // 发布点云地图
+    }
+
+    /**
+     * @brief estimate the relative pose between an input cloud and a keyframe cloud
+     * @param stamp  the timestamp of the input cloud
+     * @param cloud  the input cloud
+     * @return the relative pose between the input cloud and the keyframe cloud
+     */
+    Eigen::Matrix4f Laser_Odom::matching(const ros::Time& stamp, const pcl::PointCloud<PointT>::ConstPtr& cloud) {  // 执行扫描匹配算法，估计输入点云和关键帧之间的相对位置
+
+      if(!keyframe) {                            // 判断 keyframe 是否为空指针，是的话说明还没有初始化关键真
+        prev_time = ros::Time();                 //
+        prev_trans.setIdentity();                // 设置为单位矩阵，表示：与上一次的位资没有发生变化
+        keyframe_pose.setIdentity();             // 关键帧的初始位资
+        keyframe_stamp = stamp;                  // 关键帧的时间戳
+        keyframe = downsample(cloud);            // 对点云数据进行下采样，减少点的数量，提高处理效率和精度
+        registration->setInputTarget(keyframe);  // 将 keyframe 设置成关键帧
+
+        sensor_msgs::PointCloud2Ptr cloud_msg(new sensor_msgs::PointCloud2());
+        pcl::toROSMsg(*keyframe, *cloud_msg);
+        cloud_msg->header.frame_id = "map";
+        map_points_pub.publish(*cloud_msg);
+
+        return Eigen::Matrix4f::Identity();      // 没有之前的位资信息，假设与上次位资没有发生变化，返回单位矩阵
+
+        // registration 对象负责计算输入点云（cloud）与已有关键帧（keyframe）之间的相对变换。这种变换估算允许系统理解传感器（如激光雷达）在两次扫描之间的移动
+      }
+      auto filtered = downsample(cloud);       // 下采样
+      registration->setInputSource(filtered);  // 把点云数据给到 registration
+
+      std::string msf_source;                                       // 记录初始位资估计的来源（imu 或者 odometry)
+      Eigen::Isometry3f msf_delta = Eigen::Isometry3f::Identity();  // 三维仿射变换的单位矩阵，用于存储上一帧到当前帧的位资变化，3表示三维
+      // 缩放变换和旋转变换称为线性变换（linear transform）   线性变换和平移变换统称为仿射变换（affine transfrom）
+
+      pcl::PointCloud<PointT>::Ptr aligned(new pcl::PointCloud<PointT>());  // 创建一个新的点云对象aligned，用于存储对齐后的点云数据
+      registration->align(*aligned, prev_trans * msf_delta.matrix());       // 用 registration 来对齐点云, 如果没有imu或者odom信息, msf_delta 是单位矩阵, 即将观测到的点云数据对齐到关键帧?
+
+      // publish_scan_matching_status(stamp, cloud->header.frame_id, aligned, msf_source, msf_delta);  // 发布扫描匹配的状态信息，包括时间戳、坐标帧ID、对齐后的点云、位姿来源和位姿更新
+
+      if(!registration->hasConverged()) {  // 检查扫描匹配是否收敛(是否匹配成功?)。如果没有收敛，输出信息并返回上一次的位姿
+        ROS_ERROR("scan matching has not converged!!");
+        return keyframe_pose * prev_trans;
+      }
+
+      Eigen::Matrix4f trans = registration->getFinalTransformation();  // 获得当前点云和上一帧点云 关键帧 的仿射变换
+      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);                                // 发布关键帧的变换(这个是发送到哪里?  )
+
+      double delta_trans = trans.block<3, 1>(0, 3).norm();                                                                // 计算 当前帧 与 关键帧 变换的 平移距离
+      double delta_angle = std::acos(Eigen::Quaternionf(trans.block<3, 3>(0, 0)).w());                                    // 计算 当前帧 与 关键帧 变换的 旋转角度
+      double delta_time = (stamp - keyframe_stamp).toSec();                                                               // 计算 当前帧 与 关键帧 变换的 时间差
+      if(delta_trans > 0.01 || delta_angle > 0.01) {  // 如果有一个超过阈值,更新关键帧
+        keyframe = filtered;
+        registration->setInputTarget(keyframe);
+
+        keyframe_pose = odom;
+        keyframe_stamp = stamp;
+        prev_time = stamp;
+        prev_trans.setIdentity();
+
+
+        sensor_msgs::PointCloud2Ptr cloud_msg(new sensor_msgs::PointCloud2());
+        pcl::toROSMsg(*keyframe, *cloud_msg);
+        cloud_msg->header.frame_id = "map";
+        map_points_pub.publish(*cloud_msg);
+      }
+
+      pcl::PointCloud<PointT>::Ptr source_trans(new pcl::PointCloud<PointT>());
+      pcl::transformPointCloud(*registration->getInputSource(), *source_trans, trans);
+      sensor_msgs::PointCloud2Ptr cloud2_msg(new sensor_msgs::PointCloud2());
+      pcl::toROSMsg(*source_trans, *cloud2_msg);
+      cloud2_msg->header.frame_id = "map";
+      aligned_points_pub.publish(*cloud2_msg);
+      //
+      // Eigen::Matrix4f odom;
+      std::cout << "The matrix odom is: \n" << odom << std::endl;
+      return odom;  // 返回里程计
+    }
+
+    /**
+     * @brief downsample a point cloud
+     * @param cloud  input cloud
+     * @return downsampled point cloud
+     */
+    pcl::PointCloud<PointT>::ConstPtr Laser_Odom::downsample(const pcl::PointCloud<PointT>::ConstPtr& cloud) {  // 对点云数据进行向下采样，减少点的数量以提高处理速度
+      if(!downsample_filter) {
+        return cloud;
+      }
+      pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());  // 创建一个新的点云对象，用来存储下采样后的点云数据
+      downsample_filter->setInputCloud(cloud);
+      downsample_filter->filter(*filtered);
+      return filtered;
+    }
+
+    /**
+     * @brief convert Eigen::Matrix to geometry_msgs::TransformStamped
+     * @param stamp            timestamp
+     * @param pose             Eigen::Matrix to be converted
+     * @param frame_id         tf frame_id
+     * @param child_frame_id   tf child frame_id
+     * @return converted TransformStamped
+     */
+    geometry_msgs::TransformStamped Laser_Odom::matrix2transform(const ros::Time& stamp, const Eigen::Matrix4f& pose, const std::string& frame_id, const std::string& child_frame_id) {
+      Eigen::Quaternionf quat(pose.block<3, 3>(0, 0));
+      quat.normalize();
+      geometry_msgs::Quaternion odom_quat;
+      odom_quat.w = quat.w();
+      odom_quat.x = quat.x();
+      odom_quat.y = quat.y();
+      odom_quat.z = quat.z();
+
+      geometry_msgs::TransformStamped odom_trans;
+      odom_trans.header.stamp = stamp;
+      odom_trans.header.frame_id = frame_id;
+      odom_trans.child_frame_id = child_frame_id;
+
+      odom_trans.transform.translation.x = pose(0, 3);
+      odom_trans.transform.translation.y = pose(1, 3);
+      odom_trans.transform.translation.z = pose(2, 3);
+      odom_trans.transform.rotation = odom_quat;
+
+      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_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);
+    // }

src/laser_odom copy.cpp

@@ -0,0 +1,332 @@
+#include "laser_odom.hpp"
+#include "hdl_graph_slam/registrations.hpp"
+
+#include <memory>
+#include <iostream>
+
+#include <ros/ros.h>
+#include <ros/time.h>
+#include <ros/duration.h>
+#include <pcl_ros/point_cloud.h>
+#include <tf_conversions/tf_eigen.h>
+#include <tf/transform_listener.h>
+#include <tf/transform_broadcaster.h>
+
+#include <std_msgs/Time.h>
+#include <nav_msgs/Odometry.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+
+#include <nodelet/nodelet.h>
+#include <pluginlib/class_list_macros.h>
+
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/filters/passthrough.h>
+#include <pcl/filters/approximate_voxel_grid.h>
+
+#include <hdl_graph_slam/ros_utils.hpp>
+#include <hdl_graph_slam/registrations.hpp>
+#include <hdl_graph_slam/ScanMatchingStatus.h>
+
+#include <laser_geometry/laser_geometry.h>
+#include <nav_msgs/Path.h>
+
+#include <tf2/LinearMath/Quaternion.h>
+
+int main(int argc, char* argv[]) {
+  // 执行 ros 节点初始化
+  ros::init(argc, argv, "laser_odom");
+
+  Laser_Odom laser_odom;
+  laser_odom.init();
+  // odom_pub = nh.advertise<nav_msgs::Odometry>("/laser_odom", 32);  // 发布机器人的里程计信息
+
+  return 0;
+}
+
+Laser_Odom::Laser_Odom() {
+  laser_odom = this;
+  cloud.reset(new pcl::PointCloud<PointT>());
+  trans_cloud.reset(new pcl::PointCloud<PointT>());
+  registration.reset(new fast_gicp::FastGICP<PointT, PointT>());
+}
+
+Laser_Odom::~Laser_Odom() {}
+
+void Laser_Odom::init() {
+  auto voxelgrid = new pcl::VoxelGrid<PointT>();
+  voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);
+  downsample_filter.reset(voxelgrid);
+
+  fast_gicp::FastGICP<PointT, PointT>::Ptr gicp(new fast_gicp::FastGICP<PointT, PointT>());
+  gicp->setNumThreads(0);  // 把 registration = hdl_graph_slam::select_registration_method(nh);  核心部分弄出来
+  gicp->setTransformationEpsilon(1e-5);
+  gicp->setMaximumIterations(128);
+  gicp->setMaxCorrespondenceDistance(2.0);
+  gicp->setCorrespondenceRandomness(20);
+
+  registration = gicp;
+
+
+  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);
+  source_points_pub = nh.advertise<sensor_msgs::PointCloud2>("/aligned_points", 32);  // 对齐后的点云
+  map_points_pub = nh.advertise<sensor_msgs::PointCloud2>("/frame_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的实例，this指针都会被覆盖，指向最后创建的那个实例
+// 这里就不改了吧,后端不能这么写,因为后端要多线程
+// 多线程实现中有一个跟这个很类似的,是 懒汉式（线程不安全） 实现, 它是在给 laser_odom 赋值前先判断是不是等于 nullptr , 如果是才赋值,否则直接返回laser_odom
+// 但多线程很有可能同时满足 laser_odom == nullptr, 然后创建多个实例
+// 但其实我的cpp文件只跑一次main而已, 多线程问题在这里不会出现呀,
+void Laser_Odom::cloud_callback(const sensor_msgs::LaserScan::ConstPtr& laser_scan_msg) {
+  if(!ros::ok()) {
+    return;
+  }
+  // 消息类型转换
+  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);
+
+  // 发布对齐后的点云
+  // pcl::PointCloud<PointT>::Ptr aligned(new pcl::PointCloud<PointT>());
+  // pcl::transformPointCloud(*laser_odom->cloud, *aligned, laser_odom->odom);     //将输入的点云（laser_odom->cloud）根据指定的变换矩阵（laser_odom->odom）进行变换，并将变换后的点云结果存储到输出点云（*aligned）中
+  // aligned->header.frame_id = "odom";
+  // laser_odom->aligned_points_pub.publish(*aligned);
+
+}
+
+/**
+ * @brief estimate the relative pose between an input cloud and a keyframe cloud
+ * @param stamp  the timestamp of the input cloud
+ * @param cloud  the input cloud
+ * @return the relative pose between the input cloud and the keyframe cloud
+ */
+
+bool first = false;
+Eigen::Matrix4f last_odom;
+Eigen::Matrix4f Laser_Odom::matching(const ros::Time& stamp, const pcl::PointCloud<PointT>::ConstPtr& cloud) {  // 执行扫描匹配算法，估计输入点云和关键帧之间的相对位置
+
+  if(!first) {                            // 判断 keyframe 是否为空指针，是的话说明还没有初始化关键真
+    prev_time = ros::Time();                 //
+    prev_trans.setIdentity();                // 设置为单位矩阵，表示：与上一次的位资没有发生变化
+    keyframe_pose.setIdentity();             // 关键帧的初始位资
+    keyframe_stamp = stamp;                  // 关键帧的时间戳
+    keyframe = downsample(cloud);            // 对点云数据进行下采样，减少点的数量，提高处理效率和精度
+    registration->setInputTarget(keyframe);  // 将 keyframe 设置成关键帧
+    first = true;
+    last_odom.setIdentity();
+
+    sensor_msgs::PointCloud2Ptr cloud_msg(new sensor_msgs::PointCloud2());
+    pcl::toROSMsg(*keyframe, *cloud_msg);
+    cloud_msg->header.frame_id = "odom";
+
+    map_points_pub.publish(*cloud_msg);   // 话题名:frame_points
+
+    return Eigen::Matrix4f::Identity();      // 没有之前的位资信息，假设与上次位资没有发生变化，返回单位矩阵
+
+    // registration 对象负责计算输入点云（cloud）与已有关键帧（keyframe）之间的相对变换。这种变换估算允许系统理解传感器（如激光雷达）在两次扫描之间的移动
+  }
+  auto filtered = downsample(cloud);       // 下采样
+  registration->setInputSource(filtered);  // 把点云数据给到 registration
+
+  std::string msf_source;                                       // 记录初始位资估计的来源（imu 或者 odometry)
+  Eigen::Isometry3f msf_delta = Eigen::Isometry3f::Identity();  // 三维仿射变换的单位矩阵，用于存储上一帧到当前帧的位资变化，3表示三维
+  // 缩放变换和旋转变换称为线性变换（linear transform）   线性变换和平移变换统称为仿射变换（affine transfrom）
+
+  pcl::PointCloud<PointT>::Ptr aligned(new pcl::PointCloud<PointT>());  // 创建一个新的点云对象aligned，用于存储对齐后的点云数据
+  // registration->align(*aligned, prev_trans * msf_delta.matrix());       // 用 registration 来对齐点云, 如果没有imu或者odom信息, msf_delta 是单位矩阵, 即将观测到的点云数据对齐到关键帧?
+  registration->align(*aligned, Eigen::Isometry3f::Identity().matrix());       // 用 registration 来对齐点云, 如果没有imu或者odom信息, msf_delta 是单位矩阵, 即将观测到的点云数据对齐到关键帧?
+
+  LOG(INFO) << "===== getFitnessScore = " << registration->getFitnessScore() ;
+  // publish_scan_matching_status(stamp, cloud->header.frame_id, aligned, msf_source, msf_delta);  // 发布扫描匹配的状态信息，包括时间戳、坐标帧ID、对齐后的点云、位姿来源和位姿更新
+
+  if(!registration->hasConverged()) {  // 检查扫描匹配是否收敛(是否匹配成功?)。如果没有收敛，输出信息并返回上一次的位姿
+    ROS_ERROR("scan matching has not converged!!");
+    return keyframe_pose * prev_trans;
+  }
+
+  Eigen::Matrix4f trans = registration->getFinalTransformation();  // 获得当前点云和上一帧点云 关键帧 的仿射变换
+  odom = keyframe_pose * trans;                                    // 算出来 odom
+
+  std::cout << std::fixed << std::setprecision(5);
+  std::cout  << "keyframe_pose\n" << keyframe_pose  << std::endl;
+  LOG(INFO) << "keyframe_pose = " << atan2(keyframe_pose(1, 0), keyframe_pose(0, 0)) * (180.0 / M_PI);
+
+  std::cout << "trans\n" << trans << std::endl;
+  LOG(INFO) << "trans = " << atan2(trans(1, 0), trans(0, 0)) * (180.0 / M_PI);
+
+  std::cout << "odom\n" << odom << std::endl;
+  LOG(INFO) << "odom = " << atan2(odom(1, 0), odom(0, 0)) * (180.0 / M_PI);
+
+
+  prev_time = stamp;   // 当前帧的时间戳
+  // prev_trans = trans;  // 当前帧的仿射变换
+
+  auto keyframe_trans = matrix2transform(stamp, keyframe_pose, "odom", "keyframe");  // 将变换矩阵转换为tf::Transform对象，用于发布关键帧的变换
+  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());                                    // 计算 当前帧 与 关键帧 变换的 旋转角度
+  double delta_time = (stamp - keyframe_stamp).toSec();                                                               // 计算 当前帧 与 关键帧 变换的 时间差
+  // if(delta_trans > 5 || delta_angle > 3.14) {  // 如果有一个超过阈值,更新关键帧
+  //   keyframe = filtered;
+  //   registration->setInputTarget(keyframe);
+
+  //   keyframe_pose = odom;
+  //   keyframe_stamp = stamp;
+  //   prev_time = stamp;
+  //   prev_trans.setIdentity();
+  // }
+  pcl::PointCloud<PointT>::Ptr filter_trans(new pcl::PointCloud<PointT>());
+  pcl::transformPointCloud(*filtered, *filter_trans, odom); // transformPointCloud(A,B,c): A * c = B
+
+  pcl::PointCloud<PointT>::Ptr aligned_trans(new pcl::PointCloud<PointT>());
+  pcl::transformPointCloud(*registration->getInputSource(), *aligned_trans,  last_odom);
+  *trans_cloud+=*aligned_trans;
+
+  // 发布
+  sensor_msgs::PointCloud2Ptr cloud2_msg(new sensor_msgs::PointCloud2());
+  pcl::toROSMsg(*trans_cloud, *cloud2_msg);
+  cloud2_msg->header.frame_id = "map";
+  source_points_pub.publish(*cloud2_msg);   // 话题名:aligned_points
+
+  // keyframe_pose = odom;
+  // 发布对齐后的点云
+  sensor_msgs::PointCloud2Ptr cloud_msg(new sensor_msgs::PointCloud2());
+  pcl::toROSMsg(*registration->getInputTarget(), *cloud_msg);
+  // pcl::toROSMsg(*aligned, *cloud_msg);
+  cloud_msg->header.frame_id = "map";
+
+  map_points_pub.publish(*cloud_msg);   // 话题名:frame_points
+
+  // Eigen::Matrix4f odom;
+  registration->setInputTarget(filtered);
+  last_odom = last_odom * trans;
+  std::cout << "The matrix last_odom is: \n" << last_odom << std::endl;
+  return last_odom;  // 返回里程计
+}
+
+/**
+ * @brief downsample a point cloud
+ * @param cloud  input cloud
+ * @return downsampled point cloud
+ */
+pcl::PointCloud<PointT>::ConstPtr Laser_Odom::downsample(const pcl::PointCloud<PointT>::ConstPtr& cloud) {  // 对点云数据进行向下采样，减少点的数量以提高处理速度
+  if(!downsample_filter) {
+    return cloud;
+  }
+  pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());  // 创建一个新的点云对象，用来存储下采样后的点云数据
+  downsample_filter->setInputCloud(cloud);
+  downsample_filter->filter(*filtered);
+  return filtered;
+}
+
+/**
+ * @brief convert Eigen::Matrix to geometry_msgs::TransformStamped
+ * @param stamp            timestamp
+ * @param pose             Eigen::Matrix to be converted
+ * @param frame_id         tf frame_id
+ * @param child_frame_id   tf child frame_id
+ * @return converted TransformStamped
+ */
+geometry_msgs::TransformStamped Laser_Odom::matrix2transform(const ros::Time& stamp, const Eigen::Matrix4f& pose, const std::string& frame_id, const std::string& child_frame_id) {
+  Eigen::Quaternionf quat(pose.block<3, 3>(0, 0));
+  quat.normalize();
+  geometry_msgs::Quaternion odom_quat;
+  odom_quat.w = quat.w();
+  odom_quat.x = quat.x();
+  odom_quat.y = quat.y();
+  odom_quat.z = quat.z();
+
+  geometry_msgs::TransformStamped odom_trans;
+  odom_trans.header.stamp = stamp;
+  odom_trans.header.frame_id = frame_id;
+  odom_trans.child_frame_id = child_frame_id;
+
+  odom_trans.transform.translation.x = pose(0, 3);
+  odom_trans.transform.translation.y = pose(1, 3);
+  odom_trans.transform.translation.z = pose(2, 3);
+  odom_trans.transform.rotation = odom_quat;
+
+  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度修正,代码如下:
+
+  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);
+// }

src/laser_odom.cpp

@@ -47,6 +47,7 @@ int main(int argc, char* argv[]) {
 
 Laser_Odom::Laser_Odom() {
   laser_odom = this;
+
   cloud.reset(new pcl::PointCloud<PointT>());
   registration.reset(new fast_gicp::FastGICP<PointT, PointT>());
 }
@@ -59,21 +60,47 @@ void Laser_Odom::init() {
   downsample_filter.reset(voxelgrid);
 
   registration->setNumThreads(0);  // 把 registration = hdl_graph_slam::select_registration_method(nh);  核心部分弄出来
-  registration->setTransformationEpsilon(0.1);
-  registration->setMaximumIterations(64);
-  registration->setMaxCorrespondenceDistance(2.0);
-  registration->setCorrespondenceRandomness(20);
+  registration->setTransformationEpsilon(0.001);
+  registration->setMaximumIterations(6400);
+  registration->setMaxCorrespondenceDistance(0.1);
+  registration->setCorrespondenceRandomness(5);
+  // registration->setRANSACOutlierRejectionThreshold();
+  trans_cloud.reset(new pcl::PointCloud<PointT>());
 
   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);    // 对齐后的点云
+  keyframe_points_pub = nh.advertise<sensor_msgs::PointCloud2>("/keyframe_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::Subscriber sub = nh.subscribe<sensor_msgs::LaserScan>("/scan", 1, &Laser_Odom::cloud_callback, this);
+
+  std::thread proc_thread(&Laser_Odom::procThread, this);
+  proc_thread.detach();
+  
   ros::spin();
 }
 
+void Laser_Odom::procThread()
+{
+  while(ros::ok())
+  {
+    if(clouds_.empty())
+    {
+      usleep(10);
+      continue;
+    }
+
+    MatchPCL cloud = clouds_.front();
+    clouds_.pop();
+
+    Eigen::Matrix4f pose = laser_odom->matching(cloud.time, cloud.cloud.makeShared());
+
+    publish_odometry(cloud.time, "odom", pose);
+  }
+}
+
 Laser_Odom* Laser_Odom::laser_odom = nullptr;  // 在 .cpp 文件中定义静态成员变量
 // 这种实现方法不算单例实现，如果有多个laser_odom的实例，this指针都会被覆盖，指向最后创建的那个实例
 // 这里就不改了吧,后端不能这么写,因为后端要多线程
@@ -85,22 +112,32 @@ void Laser_Odom::cloud_callback(const sensor_msgs::LaserScan::ConstPtr& laser_sc
     return;
   }
   // 消息类型转换
+
+  pcl::PointCloud<PointT> cloud;
   laser_odom->projector.projectLaser(*laser_scan_msg, laser_odom->cloud2);
-  pcl::fromROSMsg(laser_odom->cloud2, *laser_odom->cloud);
+  pcl::fromROSMsg(laser_odom->cloud2, cloud);
 
-  // 点云匹配
-  Eigen::Matrix4f pose = laser_odom->matching(laser_scan_msg->header.stamp, laser_odom->cloud);  // 点云匹配函数，返回
+  MatchPCL mpc;
+  mpc.cloud = cloud;
+  mpc.time = laser_scan_msg->header.stamp;
 
-  // 发布激光里程计数据
-  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);
-  aligned->header.frame_id = "odom";
-  laser_odom->aligned_points_pub.publish(*aligned);
+  clouds_.push(mpc);
 
-  // 发布点云地图
+  return;
+  // // 点云匹配
+  // 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);
+
+  // // 发布对齐后的点云
+  // pcl::PointCloud<PointT>::Ptr aligned(new pcl::PointCloud<PointT>());
+  // pcl::transformPointCloud(*laser_odom->cloud, *aligned, laser_odom->odom);
+  // aligned->header.frame_id = "odom";
+  // laser_odom->aligned_points_pub.publish(*aligned);
+
+  // // 发布点云地图
 }
 
 /**
@@ -148,10 +185,15 @@ Eigen::Matrix4f Laser_Odom::matching(const ros::Time& stamp, const pcl::PointClo
   auto keyframe_trans = matrix2transform(stamp, keyframe_pose, "odom", "keyframe");  // 将变换矩阵转换为tf::Transform对象，用于发布关键帧的变换
   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());                                    // 计算 当前帧 与 关键帧 变换的 旋转角度
-  double delta_time = (stamp - keyframe_stamp).toSec();                                                               // 计算 当前帧 与 关键帧 变换的 时间差
-  if(delta_trans > keyframe_delta_trans || delta_angle > keyframe_delta_angle || delta_time > keyframe_delta_time) {  // 如果有一个超过阈值,更新关键帧
+  double delta_trans = trans.block<3, 1>(0, 3).norm();                              // 计算 当前帧 与 关键帧 变换的 平移距离
+  double delta_angle = std::acos(Eigen::Quaternionf(trans.block<3, 3>(0, 0)).w());  // 计算 当前帧 与 关键帧 变换的 旋转角度
+  double delta_time = (stamp - keyframe_stamp).toSec();                             // 计算 当前帧 与 关键帧 变换的 时间差
+  
+  // LOG(INFO)<<"delta_angle:"<<delta_angle<<" delta_trans:"<<delta_trans;
+
+  if(delta_trans > 1 || delta_angle > 1 || delta_time > 0.5) {   
+    
+                                           // 如果有一个超过阈值,更新关键帧
     keyframe = filtered;
     registration->setInputTarget(keyframe);
 
@@ -160,16 +202,29 @@ Eigen::Matrix4f Laser_Odom::matching(const ros::Time& stamp, const pcl::PointClo
     prev_time = stamp;
     prev_trans.setIdentity();
 
-
     sensor_msgs::PointCloud2Ptr cloud_msg(new sensor_msgs::PointCloud2());
     cloud_msg->header.frame_id = "map";
     pcl::toROSMsg(*keyframe, *cloud_msg);
 
     map_points_pub.publish(*cloud_msg);
+
+    trans_cloud->points.clear();
+  }
+  else
+  {
+    pcl::PointCloud<PointT>::Ptr aligned_trans(new pcl::PointCloud<PointT>());
+    pcl::transformPointCloud(*filtered, *aligned_trans, odom);
+    *trans_cloud = *aligned_trans;
+
+    // 发布
+    sensor_msgs::PointCloud2Ptr cloud2_msg(new sensor_msgs::PointCloud2());
+    pcl::toROSMsg(*trans_cloud, *cloud2_msg);
+    cloud2_msg->header.frame_id = "map";
+    keyframe_points_pub.publish(*cloud2_msg);  // 话题名:
   }
   //
   // Eigen::Matrix4f odom;
-  std::cout << "The matrix odom is: \n" << odom << std::endl;
+  // std::cout << "The matrix odom is: \n" << odom << std::endl;
   return odom;  // 返回里程计
 }
 
@@ -248,7 +303,6 @@ void Laser_Odom::publish_odometry(const ros::Time& stamp, const std::string& bas
   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);