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 + 1 ].z; // 的向量
        types[ i ].dista = vx * vx + vy * vy + vz * vz;// 向量模长(少开根号)
    }
        types[ plsize ].range = sqrt( pl[ plsize ].x * pl[ plsize ].x + pl[ plsize ].y * pl[ plsize ].y );

提取角点特征(pl_corn)和面特征(pl_surf)

	give_feature( pl, types, pl_corn, pl_surf );

give_feature函数：
pl : Lidar帧的原始点容器
types : 对Lidar原始点计算了相邻两点距离后的点容器
pl_corn : 存放提取到的点特征
pl_surf : 存放提取到的面特征
(1)预先判断点数量和点的距离

    uint plsize = pl.size();
    if ( plsize == 0 )  // 判断点数量
    {
        printf( "something wrong\n" );
        return;
    }
        uint head = 0;
    while ( types[ head ].range < blind )   // blind = 0.1 : default
    {           // types[i].range = pl[i].x
        head++; //这里有点没看懂
    }     
        plsize2 = ( plsize > group_size ) ? ( plsize - group_size ) : 0;//group_size = 8 点数大于8,则后续处理范围为 head ~ 点数-8,

(2)对head到plsize2进行计算

   for ( uint i = head; i < plsize2; i += g_LiDAR_sampling_point_step )    
    {
    // g_LiDAR_sampling_point_step = 3
    这里以间隔3个点的形式赋值了面特征，证明了后面并没有进行特征提取
        
        PointType ap;// 充当中间交换量的临时容器
   		 if ( types[ i ].range > blind )
        {
            ap.x = pl[ i ].x;
            ap.y = pl[ i ].y;
            ap.z = pl[ i ].z;
            ap.curvature = pl[ i ].curvature;
            pl_surf.push_back( ap );        //pl_surf : 存放提取到的面特征
	        if ( g_if_using_raw_point )	//g_if_using_raw_point=1
	        {
	            continue;
	        }
本来还要执行plane_judge，然后根据plane_type进行状态更新 现在无了
        plane_type = plane_judge( pl, types, i, i_nex, curr_direct );
		if ( g_if_using_raw_point )   //1
		    {
		        return;
		    }
        }
    }

所以只有pl_surf有值，还是间隔3个点的形式提取的点，pl_corn无值

3. 通过pub_full,pub_surf,pub_corn 将消息发布出去

定义发布话题

    pub_full = n.advertise< sensor_msgs::PointCloud2 >( "/laser_cloud", 100 );  //发布转换后的消息
    pub_surf = n.advertise< sensor_msgs::PointCloud2 >( "/laser_cloud_flat", 100 ); //发布面特征消息
    pub_corn = n.advertise< sensor_msgs::PointCloud2 >( "/laser_cloud_sharp", 100 );//发布角点特征消息

将转换后的pl,提取到的pl_surf,pl_corn发布出去.

	ros::Time ct( ros::Time::now() );
	pub_func( pl, pub_full, msg->header.stamp );
	pub_func( pl_surf, pub_surf, msg->header.stamp );
	pub_func( pl_corn, pub_corn, msg->header.stamp );

其中，pl_surf有值、pl_corn无值、pub_full为原始点云
/************************************************************************************************************************************************/

LIO线程主循环:

lio概述：

对输入的点云和camera帧首先考虑时间上对齐

通过measures计算lio状态并去畸变

构建立方体、体素管理地图点

体素栅格下采样滤波

下采样后的点->构造ikd树
（这里第一帧直接构建ikdtree，后面的点不直接加到树上而是找树上的最近5个点构建平面，然后计算点面残差，EKF更新后再添加到树上）

之后进行迭代kalman
{
找最近点，PCA拟合，点面loss
IKF更新，得到kalman增益后的状态结果=先验+kalman增益
收敛判断，协方差更新
} kalman结束

R3LIVE::service_LIO_update()

在r3live.cpp
main 函数中实例化 R3LIVE 对象,在构造函数开启LIO 子线程，具体参考R3live笔记：r3live的几个线程分析
其本质是 LOAM 面特征+fast-lio 的 ESIKF

        m_thread_pool_ptr->commit_task(&R3LIVE::service_LIO_update, this);//开启lio线程

定义变量：

    	nav_msgs::Path path;    // Lidar的路径 
    	
        Eigen::Matrix< double, DIM_OF_STATES, DIM_OF_STATES > G, H_T_H, I_STATE;
		G : 用于传递协方差用的矩阵, 
		H_T_H : 用于计算kalman增益用的, 
		I_STATE : 单位阵
		
	    PointCloudXYZINormal::Ptr feats_undistort( new PointCloudXYZINormal() );//去除畸变后的点云
	    PointCloudXYZINormal::Ptr feats_down( new PointCloudXYZINormal() );     // 保存下采样后的点云
	    PointCloudXYZINormal::Ptr coeffSel( new PointCloudXYZINormal() );// 存放M个最近平面信息的容器: 平面方程,点-面残差
	    PointCloudXYZINormal::Ptr laserCloudOri( new PointCloudXYZINormal() );// 存放找到了最近平面的M个点的容器
	    
        std::shared_ptr< ImuProcess > p_imu( new ImuProcess() );    // 定义用于前向/后向传播的IMU处理器

1.camera和lidar在频率上的时间对齐,判断当前时间是否可以处理lidar数据, 不可以则sleep一会.

while ( g_camera_lidar_queue.if_lidar_can_process() == false )
{  
    ros::spinOnce();
    std::this_thread::yield();
    std::this_thread::sleep_for( std::chrono::milliseconds( THREAD_SLEEP_TIM ) );
}

2.从lidar_buffer和imu_buffer_lio中分别获取雷达和imu数据,存放至Measures中

  if ( sync_packages( Measures ) == 0 )
    {
        continue;   // 提取数据失败
    }

3.通过测量数据Measures(IMU数据和Lidar数据)计算LIO的状态和去畸变后的Lidar点云数据
LIO状态结果保存在g_lio_state,去运动畸变后的点保存在feats_undistort中

p_imu->Process( Measures, g_lio_state, feats_undistort );

Process函数：
判断IMU数据、Lidar数据是否为空，然后初始化IMU数据
/** 1. initializing the gravity, gyro bias, acc and gyro covariance
* 初始化重力,陀螺仪bias,加速度和陀螺仪协方差
* 2. normalize the acceleration measurenments to unit gravity
* 归一化加速度测量值到单位重力下
**/

 IMU_Initial( meas, stat, init_iter_num );

去畸变: 第一个点看作为基坐标,使用IMU旋转矩阵补偿Lidar点(仅使用旋转)

        // ***IMU前向传播计算,Lidar点后向传播去畸变***
        if ( 1 )    
        {
            lic_point_cloud_undistort( meas, stat, *cur_pcl_un_ );
        }

        // ***状态传播***
        lic_state_propagate( meas, stat );

如果没有成功去除点云运动畸变

            if ( feats_undistort->empty() || ( feats_undistort == NULL ) )  // 没有成功去除点云运																																					动畸变
            {
                // 重置当前处理lidar帧的起始时间 : 比如当前处理t1-t2间的lidar但失败了
                // 因此后面处理时间为t1-t3,所以需要把t1保存进frame_first_pt_time
                frame_first_pt_time = Measures.lidar_beg_time;
                std::cout << "not ready for odometry" << std::endl;
                continue;
            }

4.以lidar帧最后一个状态位置构建cube，cube结果放置在featsArray[484848]中,表明cube长宽高为48,一共有48^3个体素

            lasermap_fov_segment();

5.根据给定去畸变后的点云构造三维体素栅格,对体素栅格进行下采样达到滤波的目的(一个体素用重心点来近似,减少点的数量,保持点云形状),结果保存在feats_down中

     downSizeFilterSurf.setInputCloud( feats_undistort );//构建三维体素栅格 
     downSizeFilterSurf.filter( *feats_down );           //下采样滤波

6.使用下采样后得到的特征点云构造ikd树

(下采样后特征点数量大于1) && (ikd树根节点为空) : 
	     if ( ( feats_down->points.size() > 1 ) && ( ikdtree.Root_Node == nullptr ) )
	       {
	           // std::vector<PointType> points_init = feats_down->points;
	           ikdtree.set_downsample_param( filter_size_map_min ); // filter_size_map_min默认=0.4
	           ikdtree.Build( feats_down->points );    // 构造idk树
	           flg_map_initialized = true;
	           continue;   // 进入下一次循环
	       }
	       if ( ikdtree.Root_Node == nullptr ) // 构造ikd树失败
            {
                flg_map_initialized = false;
                std::cout << "~~~~~~~ Initialize Map iKD-Tree Failed! ~~~~~~~" << std::endl;
                continue;
            }
            int featsFromMapNum = ikdtree.size();   // ikd树的节点数
            int feats_down_size = feats_down->points.size();    // 下采样过滤后的点数

7.ICP and iterated Kalman filter update : ICP迭代 + Kalman更新

		PointCloudXYZINormal::Ptr coeffSel_tmpt( new PointCloudXYZINormal( *feats_down ) );
        PointCloudXYZINormal::Ptr feats_down_updated( new PointCloudXYZINormal( *feats_down ) );
         std::vector< double >     res_last( feats_down_size, 1000.0 ); // initial : 存放每个特征点的残差值
         if ( featsFromMapNum >= 5 ) // ***重点*** : 正式开始ICP和迭代Kalman : ikd树上至少有5个点才进行操作
         {
	        在ros上发布ikd特征点云数据，默认不发布
	
			遍历所有(下采样后)特征点,搜索每个点在树上的最近点集(5个点),
	                      由最近点集通过PCA方法拟合最近平面,再计算点-面残差，残差定义为点到平面的距离，对应FAST-LIO公式(12)
	
			从找到平面的点中筛选满足条件的点,用新的变量保存该点和对应平面(下标对齐)
	
			计算测量Jacobian矩阵和测量向量. 
			
			Iterative Kalman Filter Update 
			
			收敛判断和协方差更新 
			
			更新维持的固定大小的map立方体
			
			将Kalman更新后的新lidar帧特征点先转世界坐标系,再加入ikd树
			
			ICP迭代+Kalman更新完成
         }

8.发布当前帧的点云数据

将laserCloudFullRes2的点转到世界坐标系下,再存入laserCloudFullResColor

      *laserCloudFullRes2 = dense_map_en ? ( *feats_undistort ) : ( *feats_down );//去畸变or下采样点
       int laserCloudFullResNum = laserCloudFullRes2->points.size();// 发布点数量
       for ( int i = 0; i < laserCloudFullResNum; i++ )
       {  
               RGBpointBodyToWorld( &laserCloudFullRes2->points[ i ], &temp_point );
               laserCloudFullResColor->push_back( temp_point );
       }
     sensor_msgs::PointCloud2 laserCloudFullRes3;// 将laserCloudFullResColor转为发布形式
     pcl::toROSMsg( *laserCloudFullResColor, laserCloudFullRes3 );
     laserCloudFullRes3.header.stamp.fromSec( Measures.lidar_end_time );
    laserCloudFullRes3.header.frame_id = "world"; // world; camera_init
    pubLaserCloudFullRes.publish( laserCloudFullRes3 );

9.将点云添加至世界地图中

10.Publish Maps: 发布地图

11.Publish Odometry 发布里程计

12.Publish Path 发布路径

13.save debug variables 保存debug变量