hdl_graph_slam/include/hdl_backend.hpp

#include <ros/ros.h>
#include <string>
#include <Eigen/Dense>
#include <memory>
#include <message_filters/subscriber.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/PointCloud2.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>
// #include "hdl_graph_slam/keyframe_updater.hpp"
#include <hdl_graph_slam/keyframe_updater.hpp>
#include <hdl_graph_slam/keyframe.hpp>
#include <hdl_graph_slam/SaveMap.h>
#include <hdl_graph_slam/map_cloud_generator.hpp>
#include <boost/format.hpp>
#include <hdl_graph_slam/graph_slam.hpp>
#include <hdl_graph_slam/ros_time_hash.hpp>
#include <g2o/types/slam3d/vertex_se3.h>
#include <hdl_graph_slam/information_matrix_calculator.hpp>


// 直接把头文件都复制过来

#include <ctime>
#include <mutex>
#include <atomic>
#include <memory>
#include <iomanip>
#include <iostream>
#include <unordered_map>
#include <boost/format.hpp>
#include <boost/thread.hpp>
#include <boost/filesystem.hpp>
#include <boost/algorithm/string.hpp>
#include <Eigen/Dense>
#include <pcl/io/pcd_io.h>

#include <ros/ros.h>
#include <geodesy/utm.h>
#include <geodesy/wgs84.h>
#include <pcl_ros/point_cloud.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <tf_conversions/tf_eigen.h>
#include <tf/transform_listener.h>

#include <std_msgs/Time.h>
#include <nav_msgs/Odometry.h>
#include <nmea_msgs/Sentence.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/NavSatFix.h>
#include <sensor_msgs/PointCloud2.h>
#include <geographic_msgs/GeoPointStamped.h>
#include <visualization_msgs/MarkerArray.h>
#include <hdl_graph_slam/FloorCoeffs.h>

#include <hdl_graph_slam/SaveMap.h>
#include <hdl_graph_slam/LoadGraph.h>
#include <hdl_graph_slam/DumpGraph.h>

#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>

#include <hdl_graph_slam/ros_utils.hpp>
#include <hdl_graph_slam/ros_time_hash.hpp>

#include <hdl_graph_slam/graph_slam.hpp>
#include <hdl_graph_slam/keyframe.hpp>
#include <hdl_graph_slam/keyframe_updater.hpp>
#include <hdl_graph_slam/loop_detector.hpp>
#include <hdl_graph_slam/information_matrix_calculator.hpp>
#include <hdl_graph_slam/map_cloud_generator.hpp>
#include <hdl_graph_slam/nmea_sentence_parser.hpp>

#include <g2o/types/slam3d/edge_se3.h>
#include <g2o/types/slam3d/vertex_se3.h>
#include <g2o/edge_se3_plane.hpp>
#include <g2o/edge_se3_priorxy.hpp>
#include <g2o/edge_se3_priorxyz.hpp>
#include <g2o/edge_se3_priorvec.hpp>
#include <g2o/edge_se3_priorquat.hpp>

#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;

class hdl_backend {
private:
  ros::NodeHandle nh;             // ros 的节点句柄

  // std::string published_odom_topic;



  int max_keyframes_per_update;  // 限制每次更新中可以处理的最大关键帧数量

  std::unique_ptr<message_filters::Subscriber<nav_msgs::Odometry>> odom_sub;
  std::unique_ptr<message_filters::Subscriber<sensor_msgs::PointCloud2>> cloud_sub;
  std::unique_ptr<message_filters::Synchronizer<ApproxSyncPolicy>> sync;

  void cloud_callback(const nav_msgs::OdometryConstPtr& odom_msg, const sensor_msgs::PointCloud2::ConstPtr& cloud_msg);
  void map_points_publish_timer_callback(const ros::WallTimerEvent& event) ;




  static hdl_backend instance;  // 静态实例, 保证唯一
  // 静态成员变量与类的所有实例共享，而不仅仅属于某个对象实例。因为它是私有的，所以外部不能直接访问或修改这个变量
  // 该声明并不会分配内存空间。静态变量的定义和初始化必须在类的外部进行,因为类的声明不会为静态成员分配内存，只有类外定义才会分配空间。    仅是声明,没有分配内存
  // 静态成员变量的生命周期与类的生命周期一致，而不是与对象的生命周期一致
  // C++ 语言允许在类外部对静态成员变量进行定义和初始化，这是因为静态成员变量的存储空间在全局内存中分配，而不是在对象实例中。


  ros::ServiceServer save_map_service_server;  // 一个 ROS 服务服务器，用于保存地图

  // bool save_map_service(hdl_graph_slam::SaveMapRequest& req, hdl_graph_slam::SaveMapResponse& res);



  boost::optional<Eigen::Vector3d> zero_utm;


  ros::WallTimer optimization_timer;
  ros::WallTimer map_publish_timer;
 

  // int max_keyframes_per_update;             // 限制每次更新中可以处理的最大关键帧数量


  // g2o::VertexPlane* floor_plane_node;    // 用于表示地面平面节点的指针。
  // std::vector<hdl_graph_slam::KeyFrame::Ptr> keyframes;  // 存储当前系统中的所有关键帧。
  std::unordered_map<ros::Time, hdl_graph_slam::KeyFrame::Ptr, RosTimeHash> keyframe_hash;  // 用于快速查找关键帧的哈希表








public:
  hdl_backend(){};   // 构造函数
  ~hdl_backend(){};  // 析构函数
  void init();       // 初始化函数
  std::string base_frame_id;

  std::unique_ptr<hdl_graph_slam::KeyframeUpdater> keyframe_updater;
  std::mutex keyframe_queue_mutex;                           // std::mutex 是一种独占式互斥量
  std::deque<hdl_graph_slam::KeyFrame::Ptr> keyframe_queue;  // 关键帧队列

  // static hdl_backend& getInstance() {
  //   return instance;
  // }

  std::string points_topic;  // 点云数据的ROS话题名称，SLAM系统将订阅这个话题来获取点云数据    这个不要?
  ros::Publisher read_until_pub;

  std::mutex main_thread_mutex;  // 互斥量

  bool flush_keyframe_queue();

  std::unique_ptr<hdl_graph_slam::LoopDetector> loop_detector;
  std::vector<hdl_graph_slam::KeyFrame::Ptr> keyframes;  // 存储当前系统中的所有关键帧。
  std::deque<hdl_graph_slam::KeyFrame::Ptr> new_keyframes;  // 存储新生成的关键帧指针
  std::unique_ptr<hdl_graph_slam::GraphSLAM> graph_slam;  // 声明一个指向GraphSLAM 类型的智能指针 graph_slam

  g2o::VertexSE3* anchor_node;           // 一个指向 g2o::VertexSE3 类型的指针，表示位姿图中的一个特定节点（锚点节点）。锚点节点通常是用来约束图的某个部分，使得图优化时可以保持稳定。
  g2o::EdgeSE3* anchor_edge;             // 一个指向 g2o::EdgeSE3 类型的指针，表示连接锚点节点和其他节点的边。这条边用于描述锚点与其他位姿节点之间的相对约束关系。

  std::unique_ptr<hdl_graph_slam::InformationMatrixCalculator> inf_calclator;
  std::mutex trans_odom2map_mutex;
  Eigen::Matrix4f trans_odom2map;    // 表示从 里程计坐标系（odom frame）到地图坐标系（map frame）的变换矩阵       这个变换矩阵是跟机械臂变换矩阵类似的 齐次变换矩阵
  
  std::mutex keyframes_snapshot_mutex;                                     // 互斥量
  std::vector<hdl_graph_slam::KeyFrameSnapshot::Ptr> keyframes_snapshot;   // 用来存储关键帧的快照。这个变量很重要，发布 map_point 和 保存 pcd 地图都是用这个变量！！
  std::atomic_bool graph_updated;                                          // 用于标识 位资图（pose graph）是否更新，确保多线程环境下的线程安全

  ros::Publisher odom2map_pub;
  ros::Publisher markers_pub;

  visualization_msgs::MarkerArray create_marker_array(const ros::Time& stamp) const;

  std::string map_frame_id;
  std::string odom_frame_id;
  ros::Publisher map_points_pub;  // 话题通常包含了所有之前扫描过的点云，随着机器人在环境中移动，新的点云数据会被不断添加到这个地图中，从而提供一个逐步完善的环境表示
  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.02;

  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;
  ros::Publisher odom_pub;

  void publishMatrixAsPose(ros::Publisher& publisher, const Eigen::Matrix4f& matrix);
  bool save_map_service(hdl_graph_slam::SaveMapRequest& req, hdl_graph_slam::SaveMapResponse& res);

};