后端在调

apps/hdl_graph_slam_nodelet.cpp

@@ -100,7 +100,7 @@ public:
     max_keyframes_per_update = private_nh.param<int>("max_keyframes_per_update", 10);
 
     // 一些指针的初始化
-    anchor_node = nullptr;  // 普通指针，直接给 nullptr
+    anchor_node = nullptr;  // 普通指针， 直接给 nullptr
     anchor_edge = nullptr;
     floor_plane_node = nullptr;
     graph_slam.reset(new GraphSLAM(private_nh.param<std::string>("g2o_solver_type", "lm_var")));  // 智能指针，用reset重新初始化，让它重新指向新分配的对象
@@ -663,7 +663,7 @@ private:
 
     // optimize the pose graph
     int num_iterations = private_nh.param<int>("g2o_solver_num_iterations", 1024);  // launch文件中都是设置成512
-    graph_slam->optimize(num_iterations);                                           // 使用 g2o 优化器对位姿图进行优化，优化的迭代次数由参数 "g2o_solver_num_iterations" 控制
+    graph_slam->optimize(num_iterations); // 使用 g2o 优化器对位姿图进行优化，优化的迭代次数由参数 "g2o_solver_num_iterations" 控制
 
     // publish tf     发布位姿变换
     const auto& keyframe = keyframes.back();  // 获取最新的关键帧估计

apps/prefiltering_nodelet.cpp

@@ -113,10 +113,10 @@ private:
       return;
     }
 
-    src_cloud = deskewing(src_cloud);
+    src_cloud = deskewing(src_cloud);   // 用 IMU 对点云做去畸变处理
 
     // if base_link_frame is defined, transform the input cloud to the frame
-    if(!base_link_frame.empty()) {
+    if(!base_link_frame.empty()) {  
       if(!tf_listener.canTransform(base_link_frame, src_cloud->header.frame_id, ros::Time(0))) {
         std::cerr << "failed to find transform between " << base_link_frame << " and " << src_cloud->header.frame_id << std::endl;
       }
@@ -133,8 +133,8 @@ private:
     }
 
     pcl::PointCloud<PointT>::ConstPtr filtered = distance_filter(src_cloud);
-    filtered = downsample(filtered);
-    filtered = outlier_removal(filtered);
+    filtered = downsample(filtered);  // 下采样
+    filtered = outlier_removal(filtered); // 
 
     points_pub.publish(*filtered);
   }

include/hdl_backend.hpp

@@ -81,7 +81,14 @@
 #include <g2o/edge_se3_priorvec.hpp>
 #include <g2o/edge_se3_priorquat.hpp>
 
+#include <thread> // 多线程
 
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/filters/passthrough.h>
+#include <pcl/filters/approximate_voxel_grid.h>
+
+#include <pcl/filters/radius_outlier_removal.h>
+#include <pcl/filters/statistical_outlier_removal.h>
 
 typedef message_filters::sync_policies::ApproximateTime<nav_msgs::Odometry, sensor_msgs::PointCloud2> ApproxSyncPolicy;
 typedef pcl::PointXYZI PointT;
@@ -101,7 +108,6 @@ private:
   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 optimization_timer_callback(const ros::WallTimerEvent& event);
   void map_points_publish_timer_callback(const ros::WallTimerEvent& event) ;
 
 
@@ -186,8 +192,23 @@ public:
   std::string map_frame_id;
   std::string odom_frame_id;
   ros::Publisher map_points_pub;  // 话题通常包含了所有之前扫描过的点云，随着机器人在环境中移动，新的点云数据会被不断添加到这个地图中，从而提供一个逐步完善的环境表示
+  ros::Publisher map_points_pub2;  // 话题通常包含了所有之前扫描过的点云，随着机器人在环境中移动，新的点云数据会被不断添加到这个地图中，从而提供一个逐步完善的环境表示
 
   double map_cloud_resolution;
   std::unique_ptr<hdl_graph_slam::MapCloudGenerator> map_cloud_generator;  // 生成点云地图的对象指针
+  // int loop_count = 0;
+
+  // void optimization_timer_callback(const ros::WallTimerEvent& event);
+  void optimization_timer_callback();
+  long long num_added_keyframe = 0;
+
+  pcl::PointCloud<PointT>::ConstPtr downsample(const pcl::PointCloud<PointT>::ConstPtr& cloud);
+  pcl::Filter<PointT>::Ptr downsample_filter;
+  double downsample_resolution = 0.1;
+
+  pcl::PointCloud<PointT>::ConstPtr outlier_removal(const pcl::PointCloud<PointT>::ConstPtr& cloud);
+// pcl::PointCloud<PointT>::ConstPtr hdl_backend::outlier_removal(const pcl::PointCloud<PointT>::ConstPtr& cloud) const {
+
+  pcl::Filter<PointT>::Ptr outlier_removal_filter;
 
 };

include/hdl_graph_slam/keyframe_updater.hpp

@@ -45,7 +45,8 @@ public:
     double da = Eigen::AngleAxisd(delta.linear()).angle();
 
     // too close to the previous frame
-    if(dx < 2 && da < 2) {
+    // if(dx < 2 && da < 2) {
+    if(dx < 2 && da < 2 ) {  
       return false;
     }
 

include/hdl_graph_slam/loop_detector.hpp

@@ -37,12 +37,18 @@ public:
    * @param pnh
    */
   LoopDetector(ros::NodeHandle& pnh) {
-    distance_thresh = pnh.param<double>("distance_thresh", 5.0);
-    accum_distance_thresh = pnh.param<double>("accum_distance_thresh", 8.0);
-    distance_from_last_edge_thresh = pnh.param<double>("min_edge_interval", 5.0);
+    // distance_thresh = pnh.param<double>("distance_thresh", 5.0); // launch 中给15
+    // accum_distance_thresh = pnh.param<double>("accum_distance_thresh", 8.0); // launch 中给25
+    // distance_from_last_edge_thresh = pnh.param<double>("min_edge_interval", 5.0);// launch 中给15
+
+    distance_thresh = 15;
+    accum_distance_thresh = 25;
+    distance_from_last_edge_thresh = 15;
+
 
     fitness_score_max_range = pnh.param<double>("fitness_score_max_range", std::numeric_limits<double>::max());
-    fitness_score_thresh = pnh.param<double>("fitness_score_thresh", 0.5);
+    // fitness_score_thresh = pnh.param<double>("fitness_score_thresh", 0.5);
+    fitness_score_thresh = 2.5;
 
     registration = select_registration_method(pnh);
     last_edge_accum_distance = 0.0;
@@ -56,9 +62,10 @@ public:
    */
   std::vector<Loop::Ptr> detect(const std::vector<KeyFrame::Ptr>& keyframes, const std::deque<KeyFrame::Ptr>& new_keyframes, hdl_graph_slam::GraphSLAM& graph_slam) {
     std::vector<Loop::Ptr> detected_loops;
-    for(const auto& new_keyframe : new_keyframes) {
-      auto candidates = find_candidates(keyframes, new_keyframe);
-      auto loop = matching(candidates, new_keyframe, graph_slam);
+    for(const auto& new_keyframe : new_keyframes) { // 便利所有new_keyframes
+      auto candidates = find_candidates(keyframes, new_keyframe); // 候选闭环检测, 基于某些策略（如距离、时间等）从已有关键帧中寻找潜在的闭环候选。
+      // std::cout << "candidates.size() = " << candidates.size() << std::endl;  
+      auto loop = matching(candidates, new_keyframe, graph_slam); // 对候选关键帧和新关键帧new_keyframe进行进一步匹配，通过计算相似度、位姿差异等来确定是否有闭环关系。
       if(loop) {
         detected_loops.push_back(loop);
       }
@@ -80,18 +87,20 @@ private:
    */
   std::vector<KeyFrame::Ptr> find_candidates(const std::vector<KeyFrame::Ptr>& keyframes, const KeyFrame::Ptr& new_keyframe) const {
     // too close to the last registered loop edge
-    if(new_keyframe->accum_distance - last_edge_accum_distance < distance_from_last_edge_thresh) {
-      return std::vector<KeyFrame::Ptr>();
-    }
+    // if(new_keyframe->accum_distance - last_edge_accum_distance < distance_from_last_edge_thresh) {
+    //   std::cout << "return std::vector<KeyFrame::Ptr>();" << std::endl;
+    //   return std::vector<KeyFrame::Ptr>();
+    // }
 
     std::vector<KeyFrame::Ptr> candidates;
     candidates.reserve(32);
 
     for(const auto& k : keyframes) {
       // traveled distance between keyframes is too small
-      if(new_keyframe->accum_distance - k->accum_distance < accum_distance_thresh) {
-        continue;
-      }
+      // if(new_keyframe->accum_distance - k->accum_distance < accum_distance_thresh) {
+      //   std::cout << "new_keyframe->accum_distance - k->accum_distance < accum_distance_thresh" << std::endl;
+      //   continue;
+      // }
 
       const auto& pos1 = k->node->estimate().translation();
       const auto& pos2 = new_keyframe->node->estimate().translation();
@@ -125,10 +134,10 @@ private:
     KeyFrame::Ptr best_matched;
     Eigen::Matrix4f relative_pose;
 
-    std::cout << std::endl;
-    std::cout << "--- loop detection ---" << std::endl;
-    std::cout << "num_candidates: " << candidate_keyframes.size() << std::endl;
-    std::cout << "matching" << std::flush;
+    // std::cout << std::endl;
+    // std::cout << "--- loop detection ---" << std::endl;
+    // std::cout << "num_candidates: " << candidate_keyframes.size() << std::endl;
+    // std::cout << "matching" << std::flush;
     auto t1 = ros::Time::now();
 
     pcl::PointCloud<PointT>::Ptr aligned(new pcl::PointCloud<PointT>());
@@ -141,7 +150,7 @@ private:
       Eigen::Matrix4f guess = (new_keyframe_estimate.inverse() * candidate_estimate).matrix().cast<float>();
       guess(2, 3) = 0.0;
       registration->align(*aligned, guess);
-      std::cout << "." << std::flush;
+      // std::cout << "." << std::flush;
 
       double score = registration->getFitnessScore(fitness_score_max_range);
       if(!registration->hasConverged() || score > best_score) {
@@ -154,16 +163,16 @@ private:
     }
 
     auto t2 = ros::Time::now();
-    std::cout << " done" << std::endl;
-    std::cout << "best_score: " << boost::format("%.3f") % best_score << "    time: " << boost::format("%.3f") % (t2 - t1).toSec() << "[sec]" << std::endl;
+    // std::cout << " done" << std::endl;
+    // std::cout << "best_score: " << boost::format("%.3f") % best_score << "    time: " << boost::format("%.3f") % (t2 - t1).toSec() << "[sec]" << std::endl;
 
     if(best_score > fitness_score_thresh) {
-      std::cout << "loop not found..." << std::endl;
+      // std::cout << "loop not found..." << std::endl;
       return nullptr;
     }
 
-    std::cout << "loop found!!" << std::endl;
-    std::cout << "relpose: " << relative_pose.block<3, 1>(0, 3) << " - " << Eigen::Quaternionf(relative_pose.block<3, 3>(0, 0)).coeffs().transpose() << std::endl;
+    // std::cout << "loop found!!" << std::endl;
+    // std::cout << "relpose: " << relative_pose.block<3, 1>(0, 3) << " - " << Eigen::Quaternionf(relative_pose.block<3, 3>(0, 0)).coeffs().transpose() << std::endl;
 
     last_edge_accum_distance = new_keyframe->accum_distance;
 

launch/start.launch

@@ -1,7 +1,7 @@
 <launch>
     <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 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"/>  
 

src/hdl_backend.cpp

@@ -6,7 +6,6 @@
 #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>
 
@@ -35,22 +34,31 @@ void hdl_backend::init() {
   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 hdl_graph_slam::GraphSLAM("lm_var"));  // 智能指针，用reset重新初始化，让它重新指向新分配的对象
   // graph_slam.reset(new GraphSLAM(private_nh.param<std::string>("g2o_solver_type", "lm_var")));  // 智能指针，用reset重新初始化，让它重新指向新分配的对象
 
+  auto voxelgrid = new pcl::VoxelGrid<PointT>();
+  voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);
+  downsample_filter.reset(voxelgrid);
+
   inf_calclator.reset(new hdl_graph_slam::InformationMatrixCalculator(nh));
   loop_detector.reset(new hdl_graph_slam::LoopDetector(nh));
 
   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, "/keyframe_points", 32));         // 创建 message_filters::Subscriber 对象，用于订阅sensor_msgs::PointCloud2类型的消息。
+  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, "/keyframe_points", 32));        // 创建 message_filters::Subscriber 对象，用于订阅sensor_msgs::PointCloud2类型的消息。
+  cloud_sub.reset(new message_filters::Subscriber<sensor_msgs::PointCloud2>(nh, "/filtered_points", 32));  // 创建 message_filters::Subscriber 对象，用于订阅sensor_msgs::PointCloud2类型的消息。
+  // 现在的 /filtered_points 是原始点云数据,没有做坐标转换, 滤波, 将采样等
+  // keyframe_points 在前端做了降采样
+
   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成员函数
 
-  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);  // 这个话题会保留以前扫描过的点云！！ 扫过的地方的点云就是
+  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);    // 这个话题会保留以前扫描过的点云！！ 扫过的地方的点云就是
+  map_points_pub2 = nh.advertise<sensor_msgs::PointCloud2>("/hdl_graph_slam/map_points2", 1, true);  // 这个话题会保留以前扫描过的点云！！ 扫过的地方的点云就是
 
   read_until_pub = nh.advertise<std_msgs::Header>("/hdl_graph_slam/read_until", 32);  // 这种类型的消息通常用于指示读取操作应该持续到何时
 
@@ -58,14 +66,22 @@ void hdl_backend::init() {
 
   graph_updated = false;
 
-  double graph_update_interval = 1;      // 表示图优化更新的时间间隔，默认值为3.0秒    interval：间隔
-  double map_cloud_update_interval = 4;  // 地图点云更新的时间间隔，默认值为10.0秒
+  double graph_update_interval = 3;      // 表示图优化更新的时间间隔，默认值为3.0秒    interval：间隔
+  double map_cloud_update_interval = 5;  // 地图点云更新的时间间隔，默认值为10.0秒
 
-  optimization_timer = nh.createWallTimer(ros::WallDuration(graph_update_interval), &hdl_backend::optimization_timer_callback, this);  // 创建一个定时器，定时器会在每个 graph_update_interval 秒后触发，然后调用 optimization_timer_callback 函数
+  // 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;
 
+  pcl::RadiusOutlierRemoval<PointT>::Ptr rad(new pcl::RadiusOutlierRemoval<PointT>());
+  rad->setRadiusSearch(0.8);
+  rad->setMinNeighborsInRadius(1);
+  outlier_removal_filter = rad;
+
+  std::thread proc_thread(&hdl_backend::optimization_timer_callback, this);
+  proc_thread.detach();
+
   ros::spin();
 }
 
@@ -73,80 +89,106 @@ void hdl_backend::init() {
  * @brief 这个函数将队列中的所有数据添加到姿势图中，然后优化姿势图
  * @param event   // 是ROS提供的定时器时间类，包含信息有：计时器出发的时间戳、上一次出发的时间戳、计时器的周期信息等
  */
-void hdl_backend::optimization_timer_callback(const ros::WallTimerEvent& event) {
-
-  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 函数，将关键帧队列中的数据添加到位姿图中，并返回是否有关键帧被更新
+// void hdl_backend::optimization_timer_callback(const ros::WallTimerEvent& event) {
+void hdl_backend::optimization_timer_callback() {
+  LOG(INFO) << "begin()";
+  while(ros::ok()) {
+    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 函数，将关键帧队列中的数据添加到位姿图中，并返回是否有关键帧被更新
+
+    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) << "关键帧没有更新,退出 optimization_timer_callback 函数";
+      // return;
+      usleep(30000);
+      continue;
+    }
 
-  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），如果这些数据没有更新，则函数直接返回，不进行后续操作。
-    return;
-  }
-  
+    // loop detection   闭环检测
+    std::vector<hdl_graph_slam::Loop::Ptr> loops = loop_detector->detect(keyframes, new_keyframes, *graph_slam);
 
-  // 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);
-  }
+    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);  // 添加边，位姿之间的关系构成边(edge)
+      graph_slam->add_robust_kernel(edge, "Tukey", 0.05);
+      // graph_slam->add_robust_kernel(edge, "NONE", 0.01);   // Huber  Cauchy   Tukey  None
 
-  std::copy(new_keyframes.begin(), new_keyframes.end(), std::back_inserter(keyframes));  // 将新关键帧 new_keyframes 合并到已有的关键帧列表 keyframes 中
-  new_keyframes.clear();                                                                 // 清空 new_keyframes
+      // loop_count++;
+    }
+    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                因此，第一个节点在试图停留在原点附近的同时可以自由移动
+    // 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   // 这个能不能设置为false
-    Eigen::Isometry3d anchor_target = static_cast<g2o::VertexSE3*>(anchor_edge->vertices()[1])->estimate();
-    anchor_node->setEstimate(anchor_target);
-    // 如果启用了自适应固定第一帧的功能（参数 "fix_first_node_adaptive"），则将第一个关键帧的锚点位置更新为当前估计的位置，以允许它自由移动但仍然保持在原点附近。
-  }
-  // 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;
-
-  // 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);
-  // ts.header.stamp = ros::Time::now();   // ?
-  // odom2map_pub.publish(ts);  // 发布odom2map_pub话题中/
-  // }
-
-  if(markers_pub.getNumSubscribers()) {
+    if(anchor_node && true) {  // launch文件中，fix_first_node_adaptive 设置为 true   // 这个能不能设置为false
+      Eigen::Isometry3d anchor_target = static_cast<g2o::VertexSE3*>(anchor_edge->vertices()[1])->estimate();
+      anchor_node->setEstimate(anchor_target);
+      // 如果启用了自适应固定第一帧的功能（参数 "fix_first_node_adaptive"），则将第一个关键帧的锚点位置更新为当前估计的位置，以允许它自由移动但仍然保持在原点附近。
+    }
+    // optimize the pose graph
+    int num_iterations = 10000000;  // launch文件中都是设置成512
+
+    // for(int i = 0; i < keyframes.size(); i++){
+    //   // std::cout << "before optimize, odom[" << i << "] = \n" << keyframes[i]->odom.matrix() << std::endl;
+    //   std::cout << "before optimize, odom[" << i << "] = \n" << keyframes[i]->node->estimate().matrix() << std::endl;
+    // }
+
+    // if(num_added_keyframe % 5 == 0){
+    graph_slam->optimize(num_iterations);
+    // }
+    num_added_keyframe++;
+
+    LOG(INFO) << "完成g2o的图优化==============";
+
+    // for(int i = 0; i < keyframes.size(); i++){
+    //   // std::cout << "after optimize, odom[" << i << "] = \n" << keyframes[i]->odom.matrix() << std::endl;
+    //   std::cout << "after optimize, odom[" << i << "] = \n" << keyframes[i]->node->estimate().matrix() << std::endl;
+    // }
+
+    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;
+
+    // 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);
+    // 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 发布完成";
+    // }
+    usleep(30000);
   }
 
-
+  LOG(INFO) << "end()";
 }
 
 /**
@@ -154,9 +196,36 @@ 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) << graph_updated;
+  if(!map_points_pub.getNumSubscribers() || !graph_updated) {
+    return;
+  }
+
+  // 直接发keyframe试一下?
+
+  pcl::PointCloud<PointT>::Ptr trans_cloud;
+  trans_cloud.reset(new pcl::PointCloud<PointT>());
+  for(int i = 0; i < keyframes.size(); i++) {
+    pcl::PointCloud<PointT>::Ptr cloud_i(new pcl::PointCloud<PointT>());
+    pcl::transformPointCloud(*keyframes[i]->cloud, *cloud_i, keyframes[i]->node->estimate().matrix());
+    *trans_cloud += *cloud_i;
+  }
+  trans_cloud->header.frame_id = map_frame_id;
+  trans_cloud->header.stamp = keyframes_snapshot.back()->cloud->header.stamp;
+
+  // 发布
+  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);  // 话题名:
+  map_points_pub2.publish(*cloud2_msg);
+
   // 发布点云数据
   std::vector<hdl_graph_slam::KeyFrameSnapshot::Ptr> snapshot;
   snapshot = keyframes_snapshot;
+
+  LOG(INFO) << "keyframes_snapshot.size() = " << keyframes_snapshot.size();
   auto cloud = map_cloud_generator->generate(snapshot, map_cloud_resolution);
   if(!cloud) {
     return;
@@ -166,16 +235,58 @@ 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 函数执行完毕";
+}
+
+
+/**
+ * @brief downsample a point cloud
+ * @param cloud  input cloud
+ * @return downsampled point cloud
+ */
+pcl::PointCloud<PointT>::ConstPtr hdl_backend::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;
+}
+
+pcl::PointCloud<PointT>::ConstPtr hdl_backend::outlier_removal(const pcl::PointCloud<PointT>::ConstPtr& cloud) {
+  if(!outlier_removal_filter) {
+    return cloud;
+  }
+
+  pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
+  outlier_removal_filter->setInputCloud(cloud);
+  outlier_removal_filter->filter(*filtered);
+  filtered->header = cloud->header;
+
+  return filtered;
 }
 
+
+
+
 /**
  * @brief 接收点云数据并放到 keyframe_queue 中
  * @param odom_msg  前端的激光里程计数据
  * @param cloud_msg 前端滤波后的点云数据
  */
 void hdl_backend::cloud_callback(const nav_msgs::OdometryConstPtr& odom_msg, const sensor_msgs::PointCloud2::ConstPtr& cloud_msg) {
-  // LOG(INFO)<< "cloud_callback";
+
+  // // 点云下采样 & 滤波
+  // pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
+  // downsample_filter->setInputCloud(cloud);
+  // downsample_filter->filter(*filtered);
+
+
   const ros::Time& stamp = cloud_msg->header.stamp;                  // 获取点云数据的时间戳
   Eigen::Isometry3d odom = hdl_graph_slam::odom2isometry(odom_msg);  // 将里程计消息转换为Eigen库中的Isometry3d类型，它表示一个3D刚体变换（包括旋转和平移）
 
@@ -185,14 +296,13 @@ void hdl_backend::cloud_callback(const nav_msgs::OdometryConstPtr& odom_msg, con
     base_frame_id = cloud_msg->header.frame_id;                       // 将当前点云消息的坐标系 frame_id 设置为基础坐标系      base_frame_id 是整个系统中关键帧数据的参考坐标系
   }
 
+  auto filtered = downsample(cloud);
+  filtered = outlier_removal(filtered);
+
+
   // 更新关键帧判断
   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 的操作是原子性的，即一个线程在操作时，其他线程必须等待
+    // std::lock_guard<std::mutex> lock(keyframe_queue_mutex);  // 这行代码的作用是确保线程安全，防止多个线程同时访问或修改 keyframe_queue 队列
 
     if(keyframe_queue.empty()) {  // 如果关键帧队列是空的，发布一个 ROS 消息通知系统继续读取点云数据，直到指定时间点（stamp + ros::Duration(10, 0)）。这段代码确保在没有关键帧生成时，系统能够继续读取点云数据
       // LOG(INFO) << "keyframe_queue.empty()";
@@ -203,80 +313,43 @@ void hdl_backend::cloud_callback(const nav_msgs::OdometryConstPtr& odom_msg, con
       read_until.frame_id = "/filtered_points";
       read_until_pub.publish(read_until);
     }
-    return;
+    return;  // 　如果没有关键帧更新，就　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::cout << "============ accum_d ============" << accum_d << std::endl;
+  hdl_graph_slam::KeyFrame::Ptr keyframe(new hdl_graph_slam::KeyFrame(stamp, odom, accum_d, filtered));  // 创建一个新的关键帧 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 队列，供后续的处理使用
+  // 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;
-
-  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;
-  }
-
-  if(zero_utm && req.utm) {
-    for(auto& pt : cloud->points) {
-      pt.getVector3fMap() += (*zero_utm).cast<float>();
-    }
-  }
-
-  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";
+  // LOG(INFO) << "flush_keyframe_queue";
+
+  // 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>());
+  Eigen::Isometry3d odom2map(trans_odom2map.cast<double>());  // odom 到　map　的齐次变换矩阵
   trans_odom2map_mutex.unlock();
 
-  std::cout << "flush_keyframe_queue - keyframes len:" << keyframes.size() << std::endl;
+  // 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++) {
+  // for(int i = 0; i < std::min<int>(keyframe_queue.size(), max_keyframes_per_update); i++) { // max_keyframes_per_update = 10
+  // 这里有问题吧？　keyframe_queue　是　push_back　的，是加在后面的呀，上面这个for循环不是只处理前１０个keyframe？
+  for(int i = 0; i < keyframe_queue.size(); i++) {  // max_keyframes_per_update = 10
     num_processed = i;
 
     const auto& keyframe = keyframe_queue[i];
@@ -284,11 +357,10 @@ bool hdl_backend::flush_keyframe_queue() {
     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;
+    Eigen::Isometry3d odom = odom2map * keyframe->odom;  // 一个4*4的矩阵，齐次变换矩阵
+    keyframe->node = graph_slam->add_se3_node(odom);     // 添加节点，在graph-based SLAM中，机器人的位姿是一个节点（node）
 
-    // fix the first node
+    // 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);
@@ -299,9 +371,9 @@ bool hdl_backend::flush_keyframe_queue() {
           inf(i, i) = 1.0 / stddev;
         }
 
-        anchor_node = graph_slam->add_se3_node(Eigen::Isometry3d::Identity());
+        anchor_node = graph_slam->add_se3_node(Eigen::Isometry3d::Identity());  // 添加节点，在graph-based SLAM中，机器人的位姿是一个节点（node）
         anchor_node->setFixed(true);
-        anchor_edge = graph_slam->add_se3_edge(anchor_node, keyframe->node, Eigen::Isometry3d::Identity(), inf);
+        anchor_edge = graph_slam->add_se3_edge(anchor_node, keyframe->node, Eigen::Isometry3d::Identity(), inf);  // 添加边，位姿之间的关系构成边(edge)
       }
     }
 
@@ -314,8 +386,9 @@ bool hdl_backend::flush_keyframe_queue() {
 
     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);
+    auto edge = graph_slam->add_se3_edge(keyframe->node, prev_keyframe->node, relative_pose, information);  // 添加边，位姿之间的关系构成边(edge)
     graph_slam->add_robust_kernel(edge, "NONE", 1.0);
+    // graph_slam->add_robust_kernel(edge, "Tukey", 0.1);
   }
 
   std_msgs::Header read_until;
@@ -325,7 +398,7 @@ bool hdl_backend::flush_keyframe_queue() {
   read_until.frame_id = "/filtered_points";
   read_until_pub.publish(read_until);
 
-  keyframe_queue.erase(keyframe_queue.begin(), keyframe_queue.begin() + num_processed + 1);
+  keyframe_queue.erase(keyframe_queue.begin(), keyframe_queue.begin() + num_processed + 1);  // 这里把keyframe都删了
   return true;
 }
 

src/hdl_graph_slam/graph_slam.cpp

@@ -291,7 +291,7 @@ void GraphSLAM::add_robust_kernel(g2o::HyperGraph::Edge* edge, const std::string
 
 int GraphSLAM::optimize(int num_iterations) {
   g2o::SparseOptimizer* graph = dynamic_cast<g2o::SparseOptimizer*>(this->graph.get());
-  if(graph->edges().size() < 10) {
+  if(graph->edges().size() < 5) {
     return -1;
   }
 
@@ -307,14 +307,20 @@ int GraphSLAM::optimize(int num_iterations) {
   std::cout << "chi2" << std::endl;
   double chi2 = graph->chi2();
 
-  std::cout << "optimize!!" << std::endl;
+  // std::cout << "optimize!!" << std::endl;
   auto t1 = ros::WallTime::now();
-  int iterations = graph->optimize(num_iterations);
+  int iterations = 0;
+  // while(graph->chi2() > 0.01 ){
+    iterations += graph->optimize(num_iterations);
+  //   if(iterations > 1e3)
+  //     break;
+  // }
+
 
   auto t2 = ros::WallTime::now();
   std::cout << "done" << std::endl;
   std::cout << "iterations: " << iterations << " / " << num_iterations << std::endl;
-  std::cout << "chi2: (before)" << chi2 << " -> (after)" << graph->chi2() << std::endl;
+  std::cout << "===chi2: (before)" << chi2 << " -> (after)" << graph->chi2() << std::endl;
   std::cout << "time: " << boost::format("%.3f") % (t2 - t1).toSec() << "[sec]" << std::endl;
 
   return iterations;

src/hdl_graph_slam/map_cloud_generator.cpp

@@ -18,50 +18,33 @@ pcl::PointCloud<MapCloudGenerator::PointT>::Ptr MapCloudGenerator::generate(cons
   }
 
   // 问题定位到这里, 坐标转换没算对.
-  LOG(INFO) << "关键帧数量  keyframes.size() = " << keyframes.size();
+  // LOG(INFO) << "关键帧数量  keyframes.size() = " << keyframes.size();
 
   pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());    // 创建一个新的点云对象 cloud
   cloud->reserve(keyframes.front()->cloud->size() * keyframes.size());  // 预留足够的内存空间，以容纳所有关键帧的点云数据
 
   for(const auto& keyframe : keyframes) {
     Eigen::Matrix4f pose = keyframe->pose.matrix().cast<float>();  // 每一帧关键帧相对第一帧的 pose 的齐次变换矩阵      这个齐次变换矩阵是对的
-    std::cout << "pose: ===================:" << std::endl;
-    std::cout << pose << std::endl;
-    int ii = 0;
+    // std::cout << "pose: ===================:" << std::endl;
+    // std::cout << pose << std::endl;
+    // int ii = 0;
     for(const auto& src_pt : keyframe->cloud->points) {
       PointT dst_pt;
       // std::cout << ii++ << " ";
       // dst_pt.getVector4fMap() = pose * src_pt.getVector4fMap(); // ?
-
-
-
-
-      // 打印原始点坐标
-      // std::cout << "原始点坐标 (src_pt): " << src_pt.getVector4fMap().transpose() << std::endl;
-
       // 进行变换
-
-      // dst_pt.getVector4fMap() = pose * src_pt.getVector4fMap();
-      dst_pt.getVector4fMap() = src_pt.getVector4fMap();
-
-      // 打印变换后的点坐标
-      // std::cout << "变换后的点坐标 (dst_pt): " << dst_pt.getVector4fMap().transpose() << std::endl;
-
-
-
-
-
+      dst_pt.getVector4fMap() = pose * src_pt.getVector4fMap();
+      // dst_pt.getVector4fMap() = src_pt.getVector4fMap();
       dst_pt.intensity = src_pt.intensity;  // 将源点的强度信息复制到目标点中。这通常用于保持激光点云的强度数据
       cloud->push_back(dst_pt);             // 将变换后的点 dst_pt 添加到合并的点云 cloud 中
     }
   }
-  LOG(INFO)<<"gg";
+  // LOG(INFO)<<"gg";
   cloud->width = cloud->size();
   cloud->height = 1;
   cloud->is_dense = false;
 
   if(resolution <= 0.0) return cloud;  // To get unfiltered point cloud with intensity
-  // return cloud;
 
   pcl::octree::OctreePointCloud<PointT> octree(resolution);
   octree.setInputCloud(cloud);

src/laser2cloud copy.cpp

@@ -1,35 +0,0 @@
-#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

@@ -1,74 +1,36 @@
 #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>
+#include <sensor_msgs/PointCloud2.h>
+#include <laser_geometry/laser_geometry.h>
 
-class PointCloudToLaserScan
-{
+class LaserScanToPointCloud {
 public:
-    PointCloudToLaserScan()
-    {
-        // 创建订阅者和发布者
-        pointcloud_sub_ = nh_.subscribe("/velodyne_points", 1, &PointCloudToLaserScan::pointCloudCallback, this);
-        laser_scan_pub_ = nh_.advertise<sensor_msgs::LaserScan>("/scan", 1);
+    LaserScanToPointCloud() {
+        // 初始化订阅者和发布者
+        scan_sub_ = nh_.subscribe("/scan", 10, &LaserScanToPointCloud::scanCallback, this);
+        cloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/filtered_points", 10);
     }
 
 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; // 最大范围
+    void scanCallback(const sensor_msgs::LaserScan::ConstPtr& laser_scan) {
+        sensor_msgs::PointCloud2 cloud;
+        laser_proj_.projectLaser(*laser_scan, cloud);  // 转换 LaserScan 到 PointCloud2
 
-        // 计算 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);
+        // 发布转换后的点云
+        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, "pointcloud_to_laserscan");
-    PointCloudToLaserScan pcl_to_laser;
-    ros::spin();
+int main(int argc, char** argv) {
+    std::cout << "launch laser_scan_to_point_cloud node" << std::endl;
+    ros::init(argc, argv, "laser_scan_to_point_cloud");
+    LaserScanToPointCloud converter;
+
+    ros::spin();  // 进入循环等待回调
     return 0;
 }

src/laser_odom copy 2.cpp

@@ -1,311 +0,0 @@
-    #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

@@ -1,332 +0,0 @@
-#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

@@ -59,10 +59,10 @@ void Laser_Odom::init() {
   voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);
   downsample_filter.reset(voxelgrid);
 
-  registration->setNumThreads(0);  // 把 registration = hdl_graph_slam::select_registration_method(nh);  核心部分弄出来
-  registration->setTransformationEpsilon(0.001);
+  registration->setNumThreads(5);  // 把 registration = hdl_graph_slam::select_registration_method(nh);  核心部分弄出来
+  registration->setTransformationEpsilon(1e-3);
   registration->setMaximumIterations(6400);
-  registration->setMaxCorrespondenceDistance(0.1);
+  registration->setMaxCorrespondenceDistance(0.2);
   registration->setCorrespondenceRandomness(5);
   // registration->setRANSACOutlierRejectionThreshold();
   trans_cloud.reset(new pcl::PointCloud<PointT>());
@@ -74,7 +74,8 @@ void Laser_Odom::init() {
   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", 1, &Laser_Odom::cloud_callback, this);
+  // ros::Subscriber sub = nh.subscribe<sensor_msgs::LaserScan>("/scan", 1, &Laser_Odom::cloud_callback, this);
+  ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2>("/filtered_points", 1, &Laser_Odom::cloud_callback, this);
 
   std::thread proc_thread(&Laser_Odom::procThread, this);
   proc_thread.detach();
@@ -101,6 +102,36 @@ void Laser_Odom::procThread()
   }
 }
 
+
+
+
+
+
+  void cloud_callback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg) {  // cloud_msg：滤波后的点云数据
+    if(!ros::ok()) {
+      return;
+    }
+
+    pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());  // 创建一个 pcl::PointCloud<PointT> 类型的指针
+    pcl::fromROSMsg(*cloud_msg, *cloud);                                // 将ROS消息格式的点云（*cloud_msg）转换为PCL（Point Cloud Library）格式的点云（*cloud）
+
+    Eigen::Matrix4f pose = matching(cloud_msg->header.stamp, cloud);              // 点云匹配函数，返回
+    publish_odometry(cloud_msg->header.stamp, cloud_msg->header.frame_id, pose);  // 发布里程计数据
+
+    // In offline estimation, point clouds until the published time will be supplied
+    std_msgs::HeaderPtr read_until(new std_msgs::Header());
+    read_until->frame_id = points_topic;
+    read_until->stamp = cloud_msg->header.stamp + ros::Duration(1, 0);
+    read_until_pub.publish(read_until);
+
+    read_until->frame_id = "/filtered_points";
+    read_until_pub.publish(read_until);
+  }
+
+
+
+
+
 Laser_Odom* Laser_Odom::laser_odom = nullptr;  // 在 .cpp 文件中定义静态成员变量
 // 这种实现方法不算单例实现，如果有多个laser_odom的实例，this指针都会被覆盖，指向最后创建的那个实例
 // 这里就不改了吧,后端不能这么写,因为后端要多线程
@@ -188,7 +219,7 @@ Eigen::Matrix4f Laser_Odom::matching(const ros::Time& stamp, const pcl::PointClo
   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) {   
@@ -210,8 +241,8 @@ Eigen::Matrix4f Laser_Odom::matching(const ros::Time& stamp, const pcl::PointClo
 
     trans_cloud->points.clear();
   }
-  else
-  {
+  // else
+  // {
     pcl::PointCloud<PointT>::Ptr aligned_trans(new pcl::PointCloud<PointT>());
     pcl::transformPointCloud(*filtered, *aligned_trans, odom);
     *trans_cloud = *aligned_trans;
@@ -221,7 +252,7 @@ Eigen::Matrix4f Laser_Odom::matching(const ros::Time& stamp, const pcl::PointClo
     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;