hdl_graph_slam/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 <fast_gicp/gicp/fast_gicp.hpp>

    #include <queue>
    #include <thread>
    #include <mutex>
    #include <glog/logging.h>


    #include <new>
    #undef new
    
    
    typedef pcl::PointXYZI PointT;

    struct MatchPCL {
      ros::Time time;
      pcl::PointCloud<PointT> cloud;
    };

    // namespace laser_odom_ns {

    class Laser_Odom {
    private:
      laser_geometry::LaserProjection projector;
      sensor_msgs::PointCloud2 cloud2;             // 点云数据类型转换的中间变量
      pcl::PointCloud<PointT>::Ptr cloud;          // 点云数据
      pcl::PointCloud<PointT>::Ptr trans_cloud;          // 点云数据

      pcl::PointCloud<PointT>::ConstPtr keyframe;  // 关键帧
      ros::Time prev_time;
      Eigen::Matrix4f prev_trans;     // 地图起点到当前   帧  的放射变换
      Eigen::Matrix4f keyframe_pose;  // 地图起点到当前 关键帧 的仿射变换
      ros::Time keyframe_stamp;       // 关键帧的时间戳
      // pcl::Registration<PointT, PointT>::Ptr registration;
      fast_gicp::FastGICP<PointT, PointT>::Ptr registration;
      pcl::Filter<PointT>::Ptr downsample_filter;

      double keyframe_delta_trans = 0.25;
      double keyframe_delta_angle = 0.15;
      double keyframe_delta_time = 1.0;
      // std::string odom_frame_id = "odom";
      tf::TransformBroadcaster keyframe_broadcaster;
      tf::TransformBroadcaster odom_broadcaster;

      ros::NodeHandle nh;
      ros::Publisher odom_pub;      // 里程计发布
      ros::Publisher path_pub;
      // ros::Publisher read_until_pub;  //
      ros::Publisher aligned_points_pub;
      ros::Publisher map_points_pub;
      ros::Publisher keyframe_points_pub;

      static Laser_Odom* laser_odom;
      Eigen::Matrix4f odom;

      double downsample_resolution = 0.1;  // 下采样分辨率

      ros::NodeHandle private_nh;

    private:
      void procThread();

      std::mutex mutex_;

      std::queue<MatchPCL> clouds_;

    public:
      Laser_Odom();
      ~Laser_Odom();
      void init();
      void cloud_callback(const sensor_msgs::LaserScan::ConstPtr& laser_scan_msg);
      Eigen::Matrix4f matching(const ros::Time& stamp, const pcl::PointCloud<PointT>::ConstPtr& cloud);
      pcl::PointCloud<PointT>::ConstPtr downsample(const pcl::PointCloud<PointT>::ConstPtr& cloud);
      geometry_msgs::TransformStamped matrix2transform(const ros::Time& stamp, const Eigen::Matrix4f& pose, const std::string& frame_id, const std::string& child_frame_id);
      void publish_odometry(const ros::Time& stamp, const std::string& base_frame_id, const Eigen::Matrix4f& pose);
      void publish_keyfrrame(const ros::Time& stamp, const std::string& base_frame_id, const Eigen::Matrix4f& pose);  // 发布里程计数据
    };
    // };  // namespace laser_odom_ns