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 <new>
#undef new

// namespace laser_odom_ns {
typedef pcl::PointXYZI PointT;

class Laser_Odom {
private:
  laser_geometry::LaserProjection projector;
  sensor_msgs::PointCloud2 cloud2;             // 点云数据类型转换的中间变量
  pcl::PointCloud<PointT>::Ptr cloud;          // 点云数据
  pcl::PointCloud<PointT>::ConstPtr keyframe;  // 关键帧
  ros::Time prev_time;
  Eigen::Matrix4f prev_trans;     // 地图起点到当前   帧  的放射变换
  Eigen::Matrix4f keyframe_pose;  // 地图起点到当前 关键帧 的仿射变换
  ros::Time keyframe_stamp;       // 关键帧的时间戳
  // pcl::Registration<PointT, PointT>::Ptr registration;
  fast_gicp::FastGICP<PointT, PointT>::Ptr registration;
  pcl::Filter<PointT>::Ptr downsample_filter;

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

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

  static Laser_Odom* laser_odom;
  Eigen::Matrix4f odom;

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

  ros::NodeHandle private_nh;

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