FAST-LIO2代码解析(一)

感谢FAST-LIO2代码解析(一) - 古月居，代码的注释内容主要参考古月居同志，特此声明，如有异议，欢迎反馈！

一、数据的处理

• 对于FAST-LIO2代码来说，主要的数据是通过preprocess与IMU_Processing拿到相应的传感器数据，并在主程序当中调用ESF与IKD-Tree完成优化。

1.1 激光雷达部分

(1)  preprocess.h

• 首先在最上面先定义了一系列enum的信息，通过enum选择不同的激光雷达类型，特征点特征等，并通过orgtype存放一些激光雷达点的一些其他属性。

//1、枚举类型：表示支持的雷达类型;
enum LID_TYPE{AVIA = 1, VELO16, OUST64}; //{1, 2, 3}
//2、枚举类型：表示时间的类型;
enum TIME_UNIT{SEC = 0, MS = 1, US = 2, NS = 3};
//3、枚举类型：表示特征点的类型;
enum Feature{Nor, Poss_Plane, Real_Plane, Edge_Jump, Edge_Plane, Wire, ZeroPoint};
//4、枚举类型：位置标识
enum Surround{Prev, Next};
//5、枚举类型：表示有跨越边的类型
enum E_jump{Nr_nor, Nr_zero, Nr_180, Nr_inf, Nr_blind};


//orgtype类：用于存储激光雷达点的一些其他属性;  
struct orgtype 
{
  double range;     //点云在xy平面离雷达中心的距离;
  double dista;     //当前点与后一个点之间的距离;假设雷达原点为O 前一个点为M,当前点为A,后一个点为N. 
  double angle[2];  //这个是角OAM和角OAN的cos值;
  double intersect; //这个是角MAN的cos值;
  E_jump edj[2];    //前后两点的类型;
  Feature ftype;    //点类型;
  
  // 构造函数;
  orgtype()
  {
    range = 0;
    edj[Prev] = Nr_nor;
    edj[Next] = Nr_nor;
    ftype = Nor;  //默认为正常点;
    intersect = 2;
  }
};

• 其中 LID_TYPE 这些与下面的数据结构有关系。通过enum选择不同的激光雷达数据，来设置不同的数据结构。

//velodyne数据结构-namespace velodyne_ros;
namespace velodyne_ros {
  struct EIGEN_ALIGN16 Point {
      PCL_ADD_POINT4D;  //4D点坐标类型;
      float intensity;  //强度;
      float time;       //时间;
      uint16_t ring;    //点所属的圈数;
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW //进行内存对齐;
  };
}

//注册velodyne_ros的Point类型;
POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point,
    (float, x, x)
    (float, y, y)
    (float, z, z)
    (float, intensity, intensity)
    (float, time, time)
    (uint16_t, ring, ring)
)

//ouster数据结构-namespace ouster_ros;
namespace ouster_ros {
  struct EIGEN_ALIGN16 Point {
      PCL_ADD_POINT4D;                //4D点坐标类型;
      float intensity;                //强度;
      uint32_t t;                     //时间;
      uint16_t reflectivity;          //反射率;
      uint8_t  ring;                  //点所属的圈数;
      uint16_t ambient;               //没用到;
      uint32_t range;                 //距离;
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW //进行内存对齐;
  };
}  

//注册ouster的Point类型;
POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point,
    (float, x, x)
    (float, y, y)
    (float, z, z)
    (float, intensity, intensity)
    // use std::uint32_t to avoid conflicting with pcl::uint32_t
    (std::uint32_t, t, t)
    (std::uint16_t, reflectivity, reflectivity)
    (std::uint8_t, ring, ring)
    (std::uint16_t, ambient, ambient)
    (std::uint32_t, range, range)
)

• 下面的这些是Preprocess的主体函数，主要作用是用于对激光雷达点云数据进行预处理。

class Preprocess
{
  public:
//   EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  Preprocess();  //构造函数;
  ~Preprocess(); //析构函数;
  
  //对Livox自定义Msg格式的激光雷达数据进行处理;
  void process(const livox_ros_driver2::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out);
  //对ros的Msg格式的激光雷达数据进行处理;
  void process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out);
  void set(bool feat_en, int lid_type, double bld, int pfilt_num);

  //全部点、边缘点、平面点; sensor_msgs::PointCloud2::ConstPtr pointcloud;
  PointCloudXYZI pl_full, pl_corn, pl_surf;
  PointCloudXYZI pl_buff[128]; //maximum 128 line lidar
  vector<orgtype> typess[128]; //maximum 128 line lidar
  float time_unit_scale;
  
  //雷达类型、采样间隔、扫描线数、扫描频率;
  int lidar_type, point_filter_num, N_SCANS, SCAN_RATE, time_unit;
  //最小距离阈值(盲区);
  double blind; 
  //是否提取特征、是否进行时间偏移;
  bool feature_enabled, given_offset_time;
  //发布全部点、发布平面点、发布边缘点;
  ros::Publisher pub_full, pub_surf, pub_corn;
    

  private:
  //用于对Livox激光雷达数据进行处理;    
  void avia_handler(const livox_ros_driver2::CustomMsg::ConstPtr &msg);
  //用于对ouster激光雷达数据进行处理;
  void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
  //用于对velodyne激光雷达数据进行处理l;
  void velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
  void give_feature(PointCloudXYZI &pl, vector<orgtype> &types);
  void pub_func(PointCloudXYZI &pl, const ros::Time &ct);
  int  plane_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct);
  bool small_plane(const PointCloudXYZI &pl, vector<orgtype> &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct);
  bool edge_jump_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, Surround nor_dir);
  
  int group_size;
  double disA, disB, inf_bound;
  double limit_maxmid, limit_midmin, limit_maxmin;
  double p2l_ratio;
  double jump_up_limit, jump_down_limit;
  double cos160;
  double edgea, edgeb;
  double smallp_intersect, smallp_ratio;
  double vx, vy, vz;
};

(2)  preprocess.cpp

• 上面是头文件，详细的代码在preprocess.cpp中，这里我们来详细介绍每一个函数，首先是构造函数和析构函数，里面主要存放一些雷达的参数

Preprocess::Preprocess()
  :feature_enabled(0), lidar_type(AVIA), blind(0.01), point_filter_num(1)
{
  inf_bound = 10;        //有效点集合,大于10m则是盲区;
  N_SCANS   = 6;         //多线激光雷达的线数;
  SCAN_RATE = 10;        //平面斜率？
  group_size = 8;        //8个点为一组;
  disA = 0.01;           //点集合的距离阈值,判断是否为平面;
  disA = 0.1;            //点集合的距离阈值,判断是否为平面;
  p2l_ratio = 225;       //点到线的距离阈值，需要大于这个值才能判断组成面;
  limit_maxmid =6.25;    //中点到左侧的距离变化率范围;
  limit_midmin =6.25;    //中点到右侧的距离变化率范围;
  limit_maxmin = 3.24;   //左侧到右侧的距离变化率范围;
  jump_up_limit = 170.0;
  jump_down_limit = 8.0;
  cos160 = 160.0;
  edgea = 2;                //点与点距离超过两倍则认为遮挡;
  edgeb = 0.1;              //点与点距离超过0.1m则认为遮挡
  smallp_intersect = 172.5;
  smallp_ratio = 1.2;       //三个点如果角度大于172.5度，且比例小于1.2倍，则认为是平面
  given_offset_time = false;//是否提供时间偏移;

  jump_up_limit = cos(jump_up_limit/180*M_PI);      //角度大于170度的点跳过;
  jump_down_limit = cos(jump_down_limit/180*M_PI);  //角度小于8度的点跳过;
  cos160 = cos(cos160/180*M_PI);                    //夹角限制;
  smallp_intersect = cos(smallp_intersect/180*M_PI);//三个点如果角度大于172.5度，且比例小于1.2倍，则认为是平面;
}