如今，越来越多的激光雷达方法如雨后春笋般涌现。FAST-LIO代表性系列逐渐被公众接受。而FAST-LIO工作越来越为公众所熟知和研究。作者最近也在研究和学习FAST-LIO这里将以长文的形式介绍2的相关知识FAST-LIO2.0学习经验。本文将通过代码的形式与论文结合，直观的来分析论文的核心算法部分。也作为自己的学习笔记留给后来人学习。

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

// 枚举类型：表示支持的雷达类型 enum LID_TYPE { 
           AVIA = 1,   VELO16,   OUST64 }; //{1, 2, 3}  // 枚举类型：表示特征点的类型 enum Feature { 
           Nor,        // 正常点   Poss_Plane, // 可能的平面点   Real_Plane, // 确定的平面点   Edge_Jump,  // 有跨越的边   Edge_Plane, // 边缘平面点   Wire,       // 线段 这也许是无效点？也就是空间中的小线段？   ZeroPoint   // 无效点 未在程序中使用 }; // 枚举类型：位置标识 enum Surround { 
           Prev, // 前一个   Next  // 后一个 };  // 枚举类型：表示有跨越边的类型 enum E_jump { 
           Nr_nor,  // 正常   Nr_zero, // 0   Nr_180,  // 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
{ 
        
  struct EIGEN_ALIGN16 Point
  { 
        
    PCL_ADD_POINT4D;                // 4D点坐标类型
    float intensity;                // 强度
    float time;                     // 时间
    uint16_t ring;                  // 点所属的圈数
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW // 进行内存对齐
  };
} // namespace velodyne_ros

// 注册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
{ 
        
  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 // 进行内存对齐
  };
} // namespace ouster_ros

// clang-format off
// 注册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的主体函数了，主要作用是用于对激光雷达点云数据进行预处理。

// Preproscess类：用于对激光雷达点云数据进行预处理
class Preprocess
{ 
        
  public:
// EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  Preprocess();  // 构造函数
  ~Preprocess(); // 析构函数
  
  void process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out);  // 对Livox自定义Msg格式的激光雷达数据进行处理
  void process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out);     // 对ros的Msg格式的激光雷达数据进行处理
  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
  int lidar_type, point_filter_num, N_SCANS, SCAN_RATE;  // 雷达类型、采样间隔、扫描线数、扫描频率
  double blind;  // 最小距离阈值(盲区)
  bool feature_enabled, given_offset_time;  // 是否提取特征、是否进行时间偏移
  ros::Publisher pub_full, pub_surf, pub_corn;  // 发布全部点、发布平面点、发布边缘点
    

  private:
  void avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg);  // 用于对Livox激光雷达数据进行处理
  void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);   // 用于对ouster激光雷达数据进行处理
  void velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); // 用于对velodyne激光雷达数据进行处理
  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;
};

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

Preprocess::Preprocess()
    : feature_enabled(0), lidar_type(AVIA), blind(0.01), point_filter_num(1)
{ 
        
  inf_bound = 10;      // 有效点集合,大于10m则是盲区
  N_SCANS = 6;         //多线激光雷达的线数
  group_size = 8;      // 8个点为一组
  disA = 0.01;         // 点集合的距离阈值,判断是否为平面
  disB = 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倍，则认为是平面
}

下面一个函数是set函数，这里可以在laserMapping.cpp被使用，可是该函数并未被使用。

void Preprocess::set(bool feat_en, int lid_type, double bld, int pfilt_num)
{ 
        
  feature_enabled = feat_en;    //是否提取特征点
  lidar_type = lid_type;        //雷达的种类
  blind = bld;                  //最小距离阈值，即过滤掉0～blind范围内的点云
  point_filter_num = pfilt_num; //采样间隔，即每隔point_filter_num个点取1个点
}

下面的Preprocess预处理函数主要包含了不同激光雷达的处理，这里在laserMapping.cpp被调用，从而拿到处理后的点云，并初步完成了筛选。下面两个大同小异就不仔细说了

/** * @brief Livox激光雷达点云预处理函数 * * @param msg livox激光雷达点云数据，格式为livox_ros_driver::CustomMsg * @param pcl_out 输出处理后的点云数据，格式为pcl::PointCloud */
void Preprocess::process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out)
{ 
        
  avia_handler(msg);
  *pcl_out = pl_surf;
}

void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out)
{ 
        
  switch (lidar_type)
  { 
        
  case OUST64:
    oust64_handler(msg);
    break;

  case VELO16:
    velodyne_handler(msg);
    break;

  default:
    printf("Error LiDAR Type");
    break;
  }
  *pcl_out = pl_surf;
}

下面就是handle函数来预处理发出的点云数据。这里我们展示了Livox激光雷达点云数据进行预处理。这里的操作是拿到livox原始数据，然后通过线束和反射率重新拼接了