R3live笔记：从代码看lio线程

r3live的LIO部分主要继承r2live、fast-lio部分，ros中主要体现在前端r3live_LiDAR_front_end和后端r3live_mapping节点中，对应代码

• src/loam/LiDAR_front_end.cpp
• src/r3live.cpp

/************************************************************************************************************************************************/

r3live_LiDAR_front_end

特征提取总体概述：

原始点云输入->对其进行初步筛选

• 1.按线分，只取线内点（如avia是6线），其他点判定为异常点
• 2.x、y、z的值超1e8的点判定为异常点
• 3.x<0.7，nearing点，异常，x>2.0, 异常
• 4.tag,tag是livox雷达custom数据类型下的一个标签，是一个二进制，表征的是回波信息和造点信息
  官网显示avia好像这个量没有实值，所以代码中这块意义不大
• 5.| 与上一点的距离 | > 1e7， 异常

筛选完之后得到的全是有效点，然后将其按线存储起来
接下来，按线提取特征
{
间隔3个点采样
判断点是不是在盲区，是则为异常点（只针对第一个点）
面特征直接得出，给到lio线程
}

1.获取参数, 定义相关变量，定义了几个全局变量角度值，下面会用到

    jump_up_limit = cos( jump_up_limit / 180 * M_PI );      // cos(170度)
    jump_down_limit = cos( jump_down_limit / 180 * M_PI );  // cos(8度)
    cos160 = cos( cos160 / 180 * M_PI );                    // cos(160度)
    smallp_intersect = cos( smallp_intersect / 180 * M_PI );// cos(172.5度)

2. 接收原始激光点云并处理， 提取角点特征和面特征，以MID为例：

订阅/livox/lidar 原始数据,在回调函数中转换数据， 并 提 取 角点特征和面特征

        sub_points = n.subscribe( "/livox/lidar", 1000, mid_handler, ros::TransportHints().tcpNoDelay() );

mid_handler：
点云格式转换（ros下sensor_msgs::PointCloud2格式 -> pcl::PointCloud< PointType >）

   pcl::PointCloud< PointType > pl;    // 存放Lidar点的一维数组
    pcl::fromROSMsg( *msg, pl ); // 通过PointCloud2格式的msg初始化PointXYZINormal的pl
    // 其中PointType为pcl::PointXYZINormal

定义提取的角点和平面点变量

   pcl::PointCloud< PointType > pl_corn, pl_surf;  // 保存提取的角点和平面点
    uint plsize = pl.size() - 1;    // Lidar点数量
    pl_corn.reserve( plsize );  // 预留size
    pl_surf.reserve( plsize );  // 预留size
    types.resize( plsize + 1 );

遍历msg中的前n-1个点,保存在types中

    for ( uint i = 0; i < plsize; i++ )
    {
   
        types[ i ].range = pl[ i ].x;   // 
        vx = pl[ i ].x - pl[ i + 1 ].x; // 相邻两点做差
        vy = pl[ i ].y - pl[ i + 1 ].y; // 得到两点构成
        vz = pl[ i ].z - pl[ i +