后端源码添加备注10.10

apps/hdl_graph_slam_nodelet.cpp

@@ -70,12 +70,13 @@ public:
   HdlGraphSlamNodelet() {}
   virtual ~HdlGraphSlamNodelet() {}
 
-  virtual void onInit() {
+  virtual void onInit() {  // 初始化函数
+    // 申请节点句柄
     nh = getNodeHandle();
     mt_nh = getMTNodeHandle();
     private_nh = getPrivateNodeHandle();
 
-    // init parameters
+    // init parameters   初始化参数
     published_odom_topic = private_nh.param<std::string>("published_odom_topic", "/odom");
     map_frame_id = private_nh.param<std::string>("map_frame_id", "map");
     odom_frame_id = private_nh.param<std::string>("odom_frame_id", "odom");
@@ -84,80 +85,100 @@ public:
 
     max_keyframes_per_update = private_nh.param<int>("max_keyframes_per_update", 10);
 
-    //
-    anchor_node = 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")));
+    graph_slam.reset(new GraphSLAM(private_nh.param<std::string>("g2o_solver_type", "lm_var")));  // 智能指针，用reset重新初始化，让它重新指向新分配的对象
     keyframe_updater.reset(new KeyframeUpdater(private_nh));
     loop_detector.reset(new LoopDetector(private_nh));
     map_cloud_generator.reset(new MapCloudGenerator());
     inf_calclator.reset(new InformationMatrixCalculator(private_nh));
     nmea_parser.reset(new NmeaSentenceParser());
 
-    gps_time_offset = private_nh.param<double>("gps_time_offset", 0.0);
-    gps_edge_stddev_xy = private_nh.param<double>("gps_edge_stddev_xy", 10000.0);
-    gps_edge_stddev_z = private_nh.param<double>("gps_edge_stddev_z", 10.0);
-    floor_edge_stddev = private_nh.param<double>("floor_edge_stddev", 10.0);
+    // 从参数服务器中读取参数
+    // 第一个参数是参数服务器中的变量名，第二个参数是如果参数服务器中没有这个变量的默认取值
+    gps_time_offset = private_nh.param<double>("gps_time_offset", 0.0);            // GPS时间戳与SLAM系统内部时间戳之间的偏移量，用于同步GPS数据和SLAM系统的时间
+    gps_edge_stddev_xy = private_nh.param<double>("gps_edge_stddev_xy", 10000.0);  // GPS测量在XY平面（水平方向）的标准差，用于量化GPS测量的不确定性，通常用于优化算法中权重的计算
+    gps_edge_stddev_z = private_nh.param<double>("gps_edge_stddev_z", 10.0);       // GPS测量在Z轴（垂直方向）的标准差，用于量化GPS测量的不确定性
+    floor_edge_stddev = private_nh.param<double>("floor_edge_stddev", 10.0);       // 与地面相关的测量或估计的标准差，例如在检测到地面时用于优化算法
 
-    imu_time_offset = private_nh.param<double>("imu_time_offset", 0.0);
-    enable_imu_orientation = private_nh.param<bool>("enable_imu_orientation", false);
-    enable_imu_acceleration = private_nh.param<bool>("enable_imu_acceleration", false);
-    imu_orientation_edge_stddev = private_nh.param<double>("imu_orientation_edge_stddev", 0.1);
-    imu_acceleration_edge_stddev = private_nh.param<double>("imu_acceleration_edge_stddev", 3.0);
+    imu_time_offset = private_nh.param<double>("imu_time_offset", 0.0);                            // IMU（惯性测量单元）时间戳与SLAM系统内部时间戳之间的偏移量，用于同步IMU数据和SLAM系统的时间
+    enable_imu_orientation = private_nh.param<bool>("enable_imu_orientation", false);              // 指示是否启用IMU的方位数据
+    enable_imu_acceleration = private_nh.param<bool>("enable_imu_acceleration", false);            // 指示是否启用IMU的加速度数据
+    imu_orientation_edge_stddev = private_nh.param<double>("imu_orientation_edge_stddev", 0.1);    // IMU方位测量的标准差，用于量化IMU方位测量的不确定性
+    imu_acceleration_edge_stddev = private_nh.param<double>("imu_acceleration_edge_stddev", 3.0);  // IMU加速度测量的标准差，用于量化IMU加速度测量的不确定性
 
-    points_topic = private_nh.param<std::string>("points_topic", "/velodyne_points");
+    points_topic = private_nh.param<std::string>("points_topic", "/velodyne_points");  // 点云数据的ROS话题名称，SLAM系统将订阅这个话题来获取点云数据
 
     // subscribers
-    odom_sub.reset(new message_filters::Subscriber<nav_msgs::Odometry>(mt_nh, published_odom_topic, 256));
-    cloud_sub.reset(new message_filters::Subscriber<sensor_msgs::PointCloud2>(mt_nh, "/filtered_points", 32));
-    sync.reset(new message_filters::Synchronizer<ApproxSyncPolicy>(ApproxSyncPolicy(32), *odom_sub, *cloud_sub));
-    sync->registerCallback(boost::bind(&HdlGraphSlamNodelet::cloud_callback, this, _1, _2));
-    imu_sub = nh.subscribe("/gpsimu_driver/imu_data", 1024, &HdlGraphSlamNodelet::imu_callback, this);
-    floor_sub = nh.subscribe("/floor_detection/floor_coeffs", 1024, &HdlGraphSlamNodelet::floor_coeffs_callback, this);
-
-    if(private_nh.param<bool>("enable_gps", true)) {
-      gps_sub = mt_nh.subscribe("/gps/geopoint", 1024, &HdlGraphSlamNodelet::gps_callback, this);
-      nmea_sub = mt_nh.subscribe("/gpsimu_driver/nmea_sentence", 1024, &HdlGraphSlamNodelet::nmea_callback, this);
-      navsat_sub = mt_nh.subscribe("/gps/navsat", 1024, &HdlGraphSlamNodelet::navsat_callback, this);
+    // 话题通信的接受方
+    odom_sub.reset(new message_filters::Subscriber<nav_msgs::Odometry>(mt_nh, published_odom_topic, 256));      // 创建一个新的message_filters::Subscriber对象，用于订阅nav_msgs::Odometry类型的消息
+    cloud_sub.reset(new message_filters::Subscriber<sensor_msgs::PointCloud2>(mt_nh, "/filtered_points", 32));  // 创建一个新的message_filters::Subscriber对象，用于订阅sensor_msgs::PointCloud2类型的消息。
+    // ApproxSyncPolicy是一个同步策略，允许在一定时间窗口内的消息进行同步，这里设置的窗口大小为32
+    sync.reset(new message_filters::Synchronizer<ApproxSyncPolicy>(ApproxSyncPolicy(32), *odom_sub, *cloud_sub));  // 创建一个新的message_filters::Synchronizer对象，用于同步上面创建的两个订阅者（odom_sub和cloud_sub）的消息。
+    sync->registerCallback(boost::bind(&HdlGraphSlamNodelet::cloud_callback, this, _1, _2));                       // 为同步器注册了一个回调函数，当同步条件满足时，会调用HdlGraphSlamNodelet类的cloud_callback成员函数
+    // 调用 cloud_callback 函数的条件：
+    //    消息同步：odom_sub 和 cloud_sub 两个订阅者各自收到消息后，sync 会根据 ApproximateTime 策略尝试同步它们的时间戳。
+    //    时间戳接近：两个消息的时间戳不需要完全一致，只要在一定范围内（由 ApproximateTime 策略自动决定），就会被认为是同步的。
+    //    队列不为空：odom_sub 和 cloud_sub 的消息队列必须都各自至少包含一条消息，以便同步器能进行匹配。
+    //    队列中找到匹配的消息对：当订阅者的队列中找到符合同步条件的消息对（即时间戳接近的 Odometry 和 PointCloud2 消息），就会触发 cloud_callback。
+
+    imu_sub = nh.subscribe("/gpsimu_driver/imu_data", 1024, &HdlGraphSlamNodelet::imu_callback, this);                   // 接受并处理 imu 数据（小车没有imu，所以这个回调函数可以不看）
+    floor_sub = nh.subscribe("/floor_detection/floor_coeffs", 1024, &HdlGraphSlamNodelet::floor_coeffs_callback, this);  // 接受并处理 地板检测 数据（小车没有地板检测，所以这个回调函数可以不看）
+
+    if(private_nh.param<bool>("enable_gps", true)) {                                                                // ！！！因为我们的小车没有GPS，所以这几个 callback 函数可以不看 ！！！
+      gps_sub = mt_nh.subscribe("/gps/geopoint", 1024, &HdlGraphSlamNodelet::gps_callback, this);                   // 接受并处理 GPS 数据
+      nmea_sub = mt_nh.subscribe("/gpsimu_driver/nmea_sentence", 1024, &HdlGraphSlamNodelet::nmea_callback, this);  //
+      navsat_sub = mt_nh.subscribe("/gps/navsat", 1024, &HdlGraphSlamNodelet::navsat_callback, this);               //
     }
 
     // publishers
-    markers_pub = mt_nh.advertise<visualization_msgs::MarkerArray>("/hdl_graph_slam/markers", 16);
-    odom2map_pub = mt_nh.advertise<geometry_msgs::TransformStamped>("/hdl_graph_slam/odom2pub", 16);
-    map_points_pub = mt_nh.advertise<sensor_msgs::PointCloud2>("/hdl_graph_slam/map_points", 1, true);
-    read_until_pub = mt_nh.advertise<std_msgs::Header>("/hdl_graph_slam/read_until", 32);
-
-    load_service_server = mt_nh.advertiseService("/hdl_graph_slam/load", &HdlGraphSlamNodelet::load_service, this);
-    dump_service_server = mt_nh.advertiseService("/hdl_graph_slam/dump", &HdlGraphSlamNodelet::dump_service, this);
+    // 话题通信的发布方
+    markers_pub = mt_nh.advertise<visualization_msgs::MarkerArray>("/hdl_graph_slam/markers", 16);      // 发布可视化数据（具体是什么还不知道）   这种类型的消息通常用于在RViz等可视化工具中显示标记（markers）
+    odom2map_pub = mt_nh.advertise<geometry_msgs::TransformStamped>("/hdl_graph_slam/odom2pub", 16);    // 发布从里程计到地图的变换
+    map_points_pub = mt_nh.advertise<sensor_msgs::PointCloud2>("/hdl_graph_slam/map_points", 1, true);  //
+    read_until_pub = mt_nh.advertise<std_msgs::Header>("/hdl_graph_slam/read_until", 32);               // 这种类型的消息通常用于指示读取操作应该持续到何时
+
+    // 服务通信：一个节点（客户端）发送一个服务请求给服务服务器，服务服务器处理这个请求并返回一个响应。这是一种同步的、请求/响应模式的通信方式。
+    load_service_server = mt_nh.advertiseService("/hdl_graph_slam/load", &HdlGraphSlamNodelet::load_service, this);  // 创建了一个服务服务器load_service_server，用于处理名为"/hdl_graph_slam/load"的服务请求，服务的处理函数是HdlGraphSlamNodelet类的load_service成员函数
+    dump_service_server = mt_nh.advertiseService("/hdl_graph_slam/dump", &HdlGraphSlamNodelet::dump_service, this);  // 创建了一个服务服务器dump_service_server，用于处理名为"/hdl_graph_slam/dump"的服务请求，服务的处理函数是HdlGraphSlamNodelet类的dump_service成员函数
     save_map_service_server = mt_nh.advertiseService("/hdl_graph_slam/save_map", &HdlGraphSlamNodelet::save_map_service, this);
 
-    graph_updated = false;
-    double graph_update_interval = private_nh.param<double>("graph_update_interval", 3.0);
-    double map_cloud_update_interval = private_nh.param<double>("map_cloud_update_interval", 10.0);
-    optimization_timer = mt_nh.createWallTimer(ros::WallDuration(graph_update_interval), &HdlGraphSlamNodelet::optimization_timer_callback, this);
-    map_publish_timer = mt_nh.createWallTimer(ros::WallDuration(map_cloud_update_interval), &HdlGraphSlamNodelet::map_points_publish_timer_callback, this);
+    graph_updated = false;                                                                                                                          // 用于跟踪图优化是否已经更新
+    double graph_update_interval = private_nh.param<double>("graph_update_interval", 3.0);                                                          // 表示图优化更新的时间间隔，默认值为3.0秒    interval：间隔
+    double map_cloud_update_interval = private_nh.param<double>("map_cloud_update_interval", 10.0);                                                 // 地图点云更新的时间间隔，默认值为10.0秒
+    optimization_timer = mt_nh.createWallTimer(ros::WallDuration(graph_update_interval), &HdlGraphSlamNodelet::optimization_timer_callback, this);  // 创建一个定时器，定时器会在每个 graph_update_interval 秒后触发，然后调用 optimization_timer_callback 函数
+    map_publish_timer = mt_nh.createWallTimer(ros::WallDuration(map_cloud_update_interval), &HdlGraphSlamNodelet::map_points_publish_timer_callback, this);  // 创建一个定时器，定时器会在每个 map_cloud_update_interval 秒后触发，然后调用 map_points_publish_timer_callback 函数
+
+    // 函数入口都是 callback 函数，也即三个：cloud_callback 、 optimization_timer_callback 、 map_points_publish_timer_callback
   }
 
 private:
   /**
    * @brief received point clouds are pushed to #keyframe_queue
-   * @param odom_msg
-   * @param cloud_msg
+   * @param odom_msg  // 前端的激光里程计数据
+   * @param cloud_msg // 前端滤波后的点云数据
    */
   void cloud_callback(const nav_msgs::OdometryConstPtr& odom_msg, const sensor_msgs::PointCloud2::ConstPtr& cloud_msg) {
-    const ros::Time& stamp = cloud_msg->header.stamp;
-    Eigen::Isometry3d odom = odom2isometry(odom_msg);
+    const ros::Time& stamp = cloud_msg->header.stamp;   // 获取点云数据的时间戳
+    Eigen::Isometry3d odom = odom2isometry(odom_msg);   // 将里程计消息转换为Eigen库中的Isometry3d类型，它表示一个3D刚体变换（包括旋转和平移）
 
-    pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
-    pcl::fromROSMsg(*cloud_msg, *cloud);
+    pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());  // 创建一个点云对象的指针
+    pcl::fromROSMsg(*cloud_msg, *cloud);    // 将ROS的点云消息 cloud_msg 转换为PCL（Point Cloud Library）的点云格式
     if(base_frame_id.empty()) {
-      base_frame_id = cloud_msg->header.frame_id;
-    }
-
-    if(!keyframe_updater->update(odom)) {
-      std::lock_guard<std::mutex> lock(keyframe_queue_mutex);
-      if(keyframe_queue.empty()) {
+      base_frame_id = cloud_msg->header.frame_id;   // 将当前点云消息的坐标系 frame_id 设置为基础坐标系      base_frame_id 是整个系统中关键帧数据的参考坐标系
+    } 
+
+    // 更新关键帧判断
+    if(!keyframe_updater->update(odom)) {   // 根据当前的里程计信息 odom 判断是否需要生成新的关键帧
+      std::lock_guard<std::mutex> lock(keyframe_queue_mutex); // 这行代码的作用是确保线程安全，防止多个线程同时访问或修改 keyframe_queue 队列
+      // std::lock_guard<std::mutex> 是 C++ 标准库中的一个类，用来简化互斥锁（std::mutex）的使用。它的作用是在其作用域内自动对给定的互斥量加锁，并在作用域结束时自动解锁
+      // keyframe_queue_mutex 是一个互斥量（std::mutex），用于保护关键帧队列 keyframe_queue。当多个线程试图同时访问或修改 keyframe_queue 时，互斥量确保只有一个线程能够访问该资源，避免数据竞争和并发问题
+      // 工作原理：当这行代码执行时，lock_guard 会锁住 keyframe_queue_mutex，使其他线程无法访问与之关联的资源（即 keyframe_queue） 当程序执行离开该作用域时，lock_guard 会自动释放锁，无需显式调用 unlock() 函数
+      // 为什么需要锁？   在多线程环境中（比如 ROS 回调函数可能被不同线程调用），如果多个线程同时读取或写入 keyframe_queue，可能导致数据不一致或崩溃。使用互斥量确保对 keyframe_queue 的操作是原子性的，即一个线程在操作时，其他线程必须等待
+      
+      if(keyframe_queue.empty()) {  // 如果关键帧队列是空的，发布一个 ROS 消息通知系统继续读取点云数据，直到指定时间点（stamp + ros::Duration(10, 0)）。这段代码确保在没有关键帧生成时，系统能够继续读取点云数据
         std_msgs::Header read_until;
         read_until.stamp = stamp + ros::Duration(10, 0);
         read_until.frame_id = points_topic;
@@ -166,16 +187,19 @@ private:
         read_until_pub.publish(read_until);
       }
 
-      return;
+      return; 
     }
 
-    double accum_d = keyframe_updater->get_accum_distance();
-    KeyFrame::Ptr keyframe(new KeyFrame(stamp, odom, accum_d, cloud));
+    double accum_d = keyframe_updater->get_accum_distance();  // 获取累计的运动距离，用于判断关键帧生成的条件
+    KeyFrame::Ptr keyframe(new KeyFrame(stamp, odom, accum_d, cloud));  // 创建一个新的关键帧 keyframe，其中包含当前的时间戳 stamp，里程计信息 odom，累计距离 accum_d，以及处理后的点云数据 cloud（这里的处理就是做了个消息类型的转换）
 
-    std::lock_guard<std::mutex> lock(keyframe_queue_mutex);
-    keyframe_queue.push_back(keyframe);
+    std::lock_guard<std::mutex> lock(keyframe_queue_mutex);   // 使用 std::lock_guard 加锁，确保对关键帧队列 keyframe_queue 的操作是线程安全的
+    keyframe_queue.push_back(keyframe);   // 将新生成的关键帧 keyframe 推入 keyframe_queue 队列，供后续的处理使用
   }
 
+
+
+
   /**
    * @brief this method adds all the keyframes in #keyframe_queue to the pose graph (odometry edges)
    * @return if true, at least one keyframe was added to the pose graph
@@ -192,7 +216,7 @@ private:
     Eigen::Isometry3d odom2map(trans_odom2map.cast<double>());
     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++) {
       num_processed = i;
@@ -208,7 +232,6 @@ private:
 
       // fix the first node
       if(keyframes.empty() && new_keyframes.size() == 1) {
-      
         if(private_nh.param<bool>("fix_first_node", false)) {
           Eigen::MatrixXd inf = Eigen::MatrixXd::Identity(6, 6);
           std::stringstream sst(private_nh.param<std::string>("fix_first_node_stddev", "1 1 1 1 1 1"));
@@ -808,7 +831,6 @@ private:
     return markers;
   }
 
-
   /**
    * @brief load all data from a directory
    * @param req
@@ -824,13 +846,12 @@ private:
 
     // Load graph.
     graph_slam->load(directory + "/graph.g2o");
-    
-    // Iterate over the items in this directory and count how many sub directories there are. 
+
+    // Iterate over the items in this directory and count how many sub directories there are.
     // This will give an upper limit on how many keyframe indexes we can expect to find.
     boost::filesystem::directory_iterator begin(directory), end;
-    int max_directory_count = std::count_if(begin, end,
-        [](const boost::filesystem::directory_entry & d) {
-            return boost::filesystem::is_directory(d.path()); // only return true if a direcotry
+    int max_directory_count = std::count_if(begin, end, [](const boost::filesystem::directory_entry& d) {
+      return boost::filesystem::is_directory(d.path());  // only return true if a direcotry
     });
 
     // Load keyframes by looping through key frame indexes that we expect to see.
@@ -847,8 +868,8 @@ private:
       KeyFrame::Ptr keyframe(new KeyFrame(key_frame_directory, graph_slam->graph.get()));
       keyframes.push_back(keyframe);
     }
-    std::cout << "loaded " << keyframes.size() << " keyframes" <<std::endl;
-    
+    std::cout << "loaded " << keyframes.size() << " keyframes" << std::endl;
+
     // Load special nodes.
     std::ifstream ifs(directory + "/special_nodes.csv");
     if(!ifs) {
@@ -863,21 +884,21 @@ private:
         anchor_node = static_cast<g2o::VertexSE3*>(graph_slam->graph->vertex(id));
       } else if(token == "anchor_edge") {
         int id = 0;
-        ifs >> id; 
+        ifs >> id;
         // We have no way of directly pulling the edge based on the edge ID that we have just read in.
-        // Fortunatly anchor edges are always attached to the anchor node so we can iterate over 
+        // Fortunatly anchor edges are always attached to the anchor node so we can iterate over
         // the edges that listed against the node untill we find the one that matches our ID.
-        if(anchor_node){
+        if(anchor_node) {
           auto edges = anchor_node->edges();
 
           for(auto e : edges) {
-            int edgeID =  e->id();
-            if (edgeID == id){
+            int edgeID = e->id();
+            if(edgeID == id) {
               anchor_edge = static_cast<g2o::EdgeSE3*>(e);
 
               break;
             }
-          } 
+          }
         }
       } else if(token == "floor_node") {
         int id = 0;
@@ -900,7 +921,7 @@ private:
       if(floor_plane_node->id()) {
         std::cout << " floor_node: " << floor_plane_node->id();
       }
-      
+
       // finish with a new line
       std::cout << std::endl;
     }
@@ -917,12 +938,11 @@ private:
 
     res.success = true;
 
-    std::cout << "snapshot updated" << std::endl << "loading successful" <<std::endl;
+    std::cout << "snapshot updated" << std::endl << "loading successful" << std::endl;
 
     return true;
   }
 
-
   /**
    * @brief dump all data to the current directory
    * @param req
@@ -948,7 +968,7 @@ private:
     }
 
     std::cout << "dumping data to:" << directory << std::endl;
-    // save graph 
+    // save graph
     graph_slam->save(directory + "/graph.g2o");
 
     // save keyframes
@@ -1012,11 +1032,11 @@ private:
     return true;
   }
 
-private:
+private:  // 一些参数的定义
   // ROS
-  ros::NodeHandle nh;
-  ros::NodeHandle mt_nh;
-  ros::NodeHandle private_nh;
+  ros::NodeHandle nh;          // 节点句柄
+  ros::NodeHandle mt_nh;       //
+  ros::NodeHandle private_nh;  // 私有节点句柄     申请多几个节点句柄主要是为了：命名空间隔离、参数隔离、代码组织、责任分离、调试和测试、灵活性和扩展性、并行处理（kimi解释）
   ros::WallTimer optimization_timer;
   ros::WallTimer map_publish_timer;
 
@@ -1098,7 +1118,9 @@ private:
   std::vector<KeyFrame::Ptr> keyframes;
   std::unordered_map<ros::Time, KeyFrame::Ptr, RosTimeHash> keyframe_hash;
 
-  std::unique_ptr<GraphSLAM> graph_slam;
+  // std::unique_ptr：一种独占所有权的智能指针，意味着同一时间内只能有一个std::unique_ptr指向某个对象
+  // 智能指针是一种自动管理动态分配内存的类，它们在对象不再使用时自动释放内存，从而帮助防止内存泄漏
+  std::unique_ptr<GraphSLAM> graph_slam;  // 声明一个指向GraphSLAM 类型的智能指针 graph_slam
   std::unique_ptr<LoopDetector> loop_detector;
   std::unique_ptr<KeyframeUpdater> keyframe_updater;
   std::unique_ptr<NmeaSentenceParser> nmea_parser;

apps/scan_matching_odometry_nodelet.cpp

@@ -26,7 +26,7 @@
 
 #include <hdl_graph_slam/ros_utils.hpp>
 #include <hdl_graph_slam/registrations.hpp>
-#include <hdl_graph_slam/ScanMatchingStatus.h>
+#include "hdl_graph_slam/ScanMatchingStatus.h"
 
 namespace hdl_graph_slam {
 

launch/start.launch

@@ -1,7 +1,8 @@
 <launch>
     <node pkg="hdl_graph_slam" type="laser_odom" name="laser_odom" output="log" />
     
-    
+        <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文件 -->

src/laser_odom copy.cpp

@@ -1,323 +0,0 @@
-#include "laser_odom.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 <new>
-
-
-
-  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;
-  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;
-
-
-#undef new
-int main(int argc, char* argv[]) {
-  // 执行 ros 节点初始化
-  ros::init(argc, argv, "laser_odom");
-  // 创建 ros 节点句柄
-  
-  Laser_Odom laser_odom;
-
-  // odom_pub = nh.advertise<nav_msgs::Odometry>("/laser_odom", 32);  // 发布机器人的里程计信息
-  ros::spin();
-
-  return 0;
-}
-
-Laser_Odom::Laser_Odom(){
-  cloud.reset(new pcl::PointCloud<PointT>());
-}
-
-Laser_Odom::~Laser_Odom(){
-
-}
-
-void Laser_Odom::init(){
-  ros::Subscriber sub = nh.subscribe<sensor_msgs::LaserScan>("/scan", 10, cloud_callback);
-
-
-}
-
-
-
-
-// ros::Publisher odom_pub;    // 里程计发布
-ros::Publisher status_pub;  // 匹配后的点云发布?
-
-//
-//
-//
-//
-//
-//
-//
-//
-//
-//
-//
-//
-//
-//
-//
-//
-//
-//
-//
-//
-//
-//
-//
-
-/**
- * @brief publish scan matching status
- */
-// void publish_scan_matching_status(const ros::Time& stamp, const std::string& frame_id, pcl::PointCloud<pcl::PointXYZI>::ConstPtr aligned, const std::string& msf_source, const Eigen::Isometry3f& msf_delta) {
-//   // 发布扫描的状态，包括匹配是否收敛、匹配误差、内点比例、相对位置等信息
-//   if(!status_pub.getNumSubscribers()) {
-//     return;
-//   }
-
-//   ScanMatchingStatus status;
-//   status.header.frame_id = frame_id;
-//   status.header.stamp = stamp;
-//   status.has_converged = registration->hasConverged();
-//   status.matching_error = registration->getFitnessScore();
-
-//   const double max_correspondence_dist = 0.5;
-
-//   int num_inliers = 0;
-//   std::vector<int> k_indices;
-//   std::vector<float> k_sq_dists;
-//   for(int i = 0; i < aligned->size(); i++) {
-//     const auto& pt = aligned->at(i);
-//     registration->getSearchMethodTarget()->nearestKSearch(pt, 1, k_indices, k_sq_dists);
-//     if(k_sq_dists[0] < max_correspondence_dist * max_correspondence_dist) {
-//       num_inliers++;
-//     }
-//   }
-//   status.inlier_fraction = static_cast<float>(num_inliers) / aligned->size();
-
-//   status.relative_pose = isometry2pose(Eigen::Isometry3f(registration->getFinalTransformation()).cast<double>());
-
-//   if(!msf_source.empty()) {
-//     status.prediction_labels.resize(1);
-//     status.prediction_labels[0].data = msf_source;
-
-//     status.prediction_errors.resize(1);
-//     Eigen::Isometry3f error = Eigen::Isometry3f(registration->getFinalTransformation()).inverse() * msf_delta;
-//     status.prediction_errors[0] = isometry2pose(error.cast<double>());
-//   }
-
-//   status_pub.publish(status);
-// }
-
-/**
- * @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
- */
-static geometry_msgs::TransformStamped 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 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);
-  // 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_frame_id;
-
-  odom.pose.pose.position.x = pose(0, 3);
-  odom.pose.pose.position.y = pose(1, 3);
-  odom.pose.pose.position.z = pose(2, 3);
-  odom.pose.pose.orientation = odom_trans.transform.rotation;
-
-  odom.child_frame_id = base_frame_id;
-  odom.twist.twist.linear.x = 0.0;
-  odom.twist.twist.linear.y = 0.0;
-  odom.twist.twist.angular.z = 0.0;
-
-  odom_pub.publish(odom);
-}
-
-/**
- * @brief downsample a point cloud
- * @param cloud  input cloud
- * @return downsampled point cloud
- */
-pcl::PointCloud<PointT>::ConstPtr 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 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 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 设置成关键帧
-    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();  // 获得当前点云和上一帧点云 关键帧 的仿射变换
-  Eigen::Matrix4f odom = keyframe_pose * trans;                    // 算出来 odom
-
-  prev_time = stamp;   // 当前帧的时间戳
-  prev_trans = trans;  // 当前帧的仿射变换
-
-  auto keyframe_trans = matrix2transform(stamp, keyframe_pose, odom_frame_id, "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) {  // 如果有一个超过阈值,更新关键帧
-    keyframe = filtered;
-    registration->setInputTarget(keyframe);
-
-    keyframe_pose = odom;
-    keyframe_stamp = stamp;
-    prev_time = stamp;
-    prev_trans.setIdentity();
-  }
-  // 
-
-  return odom;  // 返回里程计
-}
-
-void cloud_callback(const sensor_msgs::LaserScan::ConstPtr& laser_scan_msg) {
-  if(!ros::ok()) {
-    return;
-  }
-  // 消息类型转换
-  projector.projectLaser(*laser_scan_msg, cloud2);
-  pcl::fromROSMsg(cloud2, *cloud);  // 这里发布出去验证一下
-
-  // 匹配
-  Eigen::Matrix4f pose = matching(laser_scan_msg->header.stamp, cloud);                   // 点云匹配函数，返回
-  publish_odometry(laser_scan_msg->header.stamp, laser_scan_msg->header.frame_id, pose);  // 发布激光里程计数据
-}
-
-int main(int argc, char* argv[]) {
-  // 执行 ros 节点初始化
-  ros::init(argc, argv, "laser_odom");
-  // 创建 ros 节点句柄
-  // ros::NodeHandle nh;
-
-  ros::Subscriber sub = nh.subscribe<sensor_msgs::LaserScan>("/scan", 10, cloud_callback);
-  // odom_pub = nh.advertise<nav_msgs::Odometry>("/laser_odom", 32);  // 发布机器人的里程计信息
-  ros::spin();
-
-  return 0;
-}