从数据流视角解析 loam_livox

loam_livox 和 loam 的原理很多是类似的，想了解原理可以看这篇文章：LOAM 细节分析。

解析一套代码，一般有两种视角。一种是按类(class)解析，另一种则是从数据流向解析。本文就从数据流的视角来解析一下 loam_livox 。
代码地址：https://github.com/hku-mars/loam_livox
配置就按它的 github 上面的教程就行了，注意 pcl 用 v1.9 的，版本低了运行时 ceres 会报错。

这篇博客是之前写的简介，可以看出，loam_livox 的整体结构并不复杂，核心节点就两个：livox_scanRegistration 主要用于读入数据、特征提取；livox_laserMapping 主要用于位姿估计、建图。

下面就用 3 张流程图来解析数据流向：

第1部分

第1步，就是用 rosbag play 读入数据，原始数据发布在 /livox/lidar 话题上。

第2步，livox_scanRegistration 节点，是由 laser_feature_extractor.cpp/hpp 生成的，主要是用原始数据进行特征提取。与特征提取紧密相关的文件还有 livox_feature_extractor.hpp.

第3步，livox_scanRegistration 节点直接通过构造函数调用init_ros_env()函数，初始化一些参数，并且创建一些发布者和订阅者。其中就订阅了 /laser_points 话题，它其实在launch文件中被remap成了 /livox/lidar 话题（launch文件代码如下），刚好就是 rosbag play 发布的话题。

<node pkg="loam_livox" type="livox_scanRegistration" name="livox_scanRegistration">
    <remap from="/laser_points" to="/livox/lidar" />
</node>

不熟悉remap的戳这。

第4步，laserCloudHandler()是 /laser_points 话题的回调函数，如下所示：

m_sub_input_laser_cloud = nh.subscribe<sensor_msgs::PointCloud2>(
  "/laser_points", 10000, &Laser_feature::laserCloudHandler, this );

这个函数两个作用：把ros消息转换为点云，存放到变量laserCloudIn中；调用extract_laser_features()和get_features()，把变量laserCloudIn进行特征提取，并且分为三类，livox_corners、livox_surface、livox_full，大概流程就是：

把特征分成corners与surface两类，这和 LOAM 是类似的。

LOAM的特征提取基于曲率，只提取两种特征点，corner和surface，分别对应场景的平面区域和曲折区域。LOAM没有使用特征描述子(连曲率都没有参与后续的匹配)。从代码中的corner与surface的曲率判断阈值可以看出，LOAM提取的corner和surface特征点的曲率， 并没有特别大的差别，这使得LOAM有较强的场景适应性，在场景中比较曲折的区域，corner点会占据主导，而在较为平缓的区域，surface点占据主导. 在激光扫描到的一块区域，总会提取出几个特征点。（引自：LOAM 细节分析）

livox_full 包含了所有的点云，是经过筛选的 good points。筛点的策略用原文中的图来简单说明一下：

筛点策略：

• 在视角范围内的边缘处（ fringe ），如大于17度。这类点有强畸变，所以舍去
• 强度（ intensity ），太大或太小
• 与平面的夹角接近0或180度的，如上图的点f
• 即将被遮挡的点，如上图点e

第5步，把上一步获得的3类数据转换为ros消息，再把ros消息又发布出去，转换过程如下图：

发布出来的3类消息，供下一部分的节点 livox_laserMapping 使用。

第1部分总结：主要就是对原始数据进行坏点剔除、特征提取，获取两类特征（coners 特征和 surface 特征）、三组数据（ /pc2_coners、/pc2_surface、/pc2_full）

第2部分

第1步，节点 livox_laserMapping 中的3个订阅者（m_sub_laser_cloud_corner_last、m_sub_laser_cloud_surf_last、m_sub_laser_cloud_full_res ）订阅了第1部分产生的3个话题（/pc2_coners、/pc2_surface、/pc2_full），并调用各自对应的函数形成数据对（data_pair）。其中都要调用 get_data_pair 函数。然后把获取到的 data_pair 推进队列 m_queue_avail_data 中。

第2步，核心函数 process ，处理从上一步得到的数据队列（m_queue_avail_data）。进入 while 循环，逐个处理队列中的数据，当前数据是 current_data_pair 。然后再把ROS消息转换为点云，流程如图所示。对应的代码在 process 函数中，如下：

m_mutex_querypointcloud.lock();

m_laser_cloud_corner_last->clear();
pcl::fromROSMsg( *( current_data_pair->m_pc_corner ), *m_laser_cloud_corner_last );

m_laser_cloud_surf_last->clear();
pcl::fromROSMsg( *( current_data_pair->m_pc_plane ), *m_laser_cloud_surf_last );

m_laser_cloud_full_res->clear();
pcl::fromROSMsg( *( current_data_pair->m_pc_full ), *m_laser_cloud_full_res );
m_mutex_querypointcloud.unlock();

delete current_data_pair;

注意，图中的 m_laser_cloud_full 意思不是指历史的全部点云，而是当前帧的全部点云数据。是相比于 corner、surf 的 full 。

第3步，还是在 process 函数的 while 循环中。上一步逐个处理了队列中的数据，把ROS消息转换为点云变量。前面的步骤就是把数据转成ros消息，再把ros消息转成变量，再把变量转成ros消息……主要用到的两个函数就是 pcl::fromROSMsg 和 pcl::toROSMsg。如此反复折腾，有必要吗？应该是有它的道理的，只不过我暂时不懂……
那么此步继续折腾，调用 process_new_scan 函数，又把数据转换到 current_laser_cloud_corner_last、current_laser_cloud_surf_last、current_laser_cloud_full 这三个变量中，具体如上图所示。当然，process_new_scan 函数还做了其他事，我这只关心数据流向，就只列出了这部分。调用 process_new_scan 函数的代码如下，在 process 函数的 while 循环中。

std::future<int> *thd = new std::future<int>(
  std::async( std::launch::async, &Laser_mapping::process_new_scan, this ) );

第4步，继续“折腾”，此步是最最最原始的数据进化到优秀原始数据的大结局，从上图可以看到，优秀原始数据是上一步经过滤波、降采样得到的（似乎称为精英原始数据更好），终于不用反复折腾了。这里所谓的优秀原始数据，就是指：已经经过坏点剔除的，分好了类的（分为coners、surface、full）。那么，优秀原始数据就可以享有两项特权：

• 被保存的特权，被处理好的当前帧的所有点云，都保存到 raw_xxx.pcd 文件中（xxx 是当前帧的ID）。代码如下，这句代码就是马上下一部分要讲到的关键语句之一。

m_pcl_tools_raw.save_to_pcd_files( "raw", current_laser_cloud_full, m_current_frame_index );

• 进入下一关的特权。只有优秀的数据才能进入下一步，继续被处理、做里程计、回环检测等等。

第2部分总结：把第1部分拿到的ros消息推进队列，在循环中逐个进行精化处理，获得优秀原始数据，保存它们并进入下一部分。

第3部分

引言： 这部分关键的两条语句就是上图中标黄的部分，分别是保存 raw 数据和 aft_map 数据的。如果你运行过 loam_livox 的回环检测的程序，就会发现在 Loam_livox_loop 目录下会生成 raw 和 aft_map 这两类数据。raw 数据就是上一部分提到的“优秀”原始数据，aft_map 数据就是经过后端优化。
注意，注意， aft_map 数据只是经过后端优化的，如果你开了回环检测，它并不是经过回环检测优化后的点云。想获得经过回环检测优化的点云可以自己改代码，它保存在 laser_mapping.hpp 中的变量 pts_opm 中，代码修改如下：
（其中 map_id_pc_keyframes 表示关键帧的点云，在修改后的代码中一共有3处，都是紧跟在map_id_pc之后）

std::map<int, pcl::PointCloud<PointType>> map_id_pc;
std::map<int, pcl::PointCloud<PointType>> map_id_pc_keyframes;
// ......
map_id_pc.insert( std::make_pair( map_id_pc.size(), keyframe_vec.back()->m_accumulated_point_cloud ) );
map_id_pc_keyframes.insert( std::make_pair( map_id_pc_keyframes.size(), m_current_laser_cloud_full ) );
// ......
auto refined_pt = map_rfn.refine_pointcloud( map_id_pc, pose3d_map_ori, temp_pose_3d_map, pc_idx, 0 );
auto pts_opm = map_rfn.refine_pointcloud( map_id_pc_keyframes, pose3d_map_ori, temp_pose_3d_map, pc_idx);
if( m_if_save_to_pcd_files && PCD_SAVE_RAW)
{
    m_pcl_tools_aftmap.save_to_pcd_files("keyframe_opm", pts_opm, pc_idx);
}

上面这样即把经过了回环检测优化的点云也保存在 Loam_livox_loop 目录下，并以 keyframe_opm 为前缀。
因为回环检测优化是针对关键帧的，所以上面保存的也只有关键帧的，并没有所有帧的。上面代码中 pts_opm 是该关键帧经过回环优化后的点云数据， pc_idx 是关键帧的ID号，./Loam_livox_loop/file_name.txt 文件中有关键帧的ID号与所有帧的ID号的映射关系。
数量关系：raw 数据和 aft_map 数据的数量与最最最原始的数据的帧数是一样的，比如最最最原始的数据有4800帧，raw 和 aft_map 也有4800帧，但是关键帧可能只有48帧，程序设定为间隔100帧选一个关键帧，代码为证如下：

// 在 laser_mapping.hpp 中的 service_pub_surround_pts 函数中
while ( 1 )
{
    while ( m_current_frame_index - last_update_index < 100 )
    {
        std::this_thread::sleep_for( std::chrono::milliseconds( 1500 ) );
    }
    last_update_index = m_current_frame_index;
    // ……
}

当然了，程序中的选关键帧策略除了这个固定间隔100帧的，还有其他策略，感兴趣的可以自行研究。

第1步，从第2部分获得了 laserCloudCornerStack、laserCloudSurfStack、current_laser_cloud_full 三类数据，除了把它们保存为 raw 数据，当然还要做里程计，就用 Corner 特征和 Surf 特征，对应的就是两个数据栈 laserCloudCornerStack、laserCloudSurfStack ，它们两个是作为当前帧的数据输入到 find_out_incremental_transfrom 函数中，这个函数需要三类参数共6个，每类参数都是一样的，分为Corner 和 Surf 两类。find_out_incremental_transfrom 函数需要的参数，除了刚刚提到的当前帧数据，还需要历史的地图数据，就是用当前帧数据与历史地图数据进行配准，得到当前帧相对于历史地图坐标系的最优位姿，也就是相对于全局坐标系的。为了加快索引速度，还会把历史数据建立一个 kd 树。此步的求得的位姿结果，就是用 ceres 优化过的位姿。

第2步，用 pointAssociateToMap 函数把当前帧的数据对齐到全局坐标系下，并把它们加入到历史地图中，并更新历史地图。具体过程如上图所示。

第3步，随着数据的不断读入，历史地图也不断更新，所以第1步中 find_out_incremental_transfrom 函数的历史地图那类参数也是不断更新。

第4步，最后获得 aft_map 数据，注意，注意， aft_map 数据只是经过后端优化的，并没有经过回环优化（想获得经过回环优化的点云，回看第3部分的引言）。
这个loam_livox程序跑完，生成的一帧一帧的点云数据，存在 aft_map 数据中，如果想获得所有aft_map拼接之后的点云，可以用如下简陋的代码生成，自己改一下分辨率和保存路径，以及程序运行结束的标志（我用的是ID号运行到4800帧的时候，自己根据情况改，虽然这样比较low，但是目前我暂时懒得改……）：
代码添加在laser_mapping.hpp的m_pcl_tools_aftmap.save_to_pcd_files(...)后面

if ( m_if_save_to_pcd_files )
{
    m_pcl_tools_aftmap.save_to_pcd_files( "aft_mapp", current_laser_cloud_full, m_current_frame_index );
    *( m_logger_pcd.get_ostream() ) << "Save to: " << m_pcl_tools_aftmap.m_save_file_name << endl;
}
//保存所有点云，代码参考：https://www.cnblogs.com/gaoxiang12/p/4719156.html
m_all_pcd_atf_mapped += current_laser_cloud_full;
// Voxel grid 滤波降采样
pcl::VoxelGrid<PointType> voxel;
// 点云分辨率(表示多大的格子有一个点，值越大，点云越稀疏)
// 自己根据需要调整，点云太密，pcl_viewer可能无法加载
double gridsize = 0.1; 
voxel.setLeafSize( gridsize, gridsize, gridsize );
voxel.setInputCloud( m_all_pcd_atf_mapped.makeShared() );
pcl::PointCloud<PointType>::Ptr tmp( new pcl::PointCloud<PointType>() );
voxel.filter( *tmp );
m_all_pcd_atf_mapped = *tmp;
//暂时先用数字来判断运行结束
if( m_current_frame_index==4800 )
{
    pcl::io::savePCDFile( "/home/XXXXXX/all_pcd_aft_mapp.pcd", m_all_pcd_atf_mapped );
}

其中，m_all_pcd_atf_mapped 的定义为pcl::PointCloud<PointType> m_all_pcd_atf_mapped; ，它是 Laser_mapping 类的public变量，自行加到类变量定义的位置。

第3部分总结：这两条关键语句是在 process_new_scan 函数中，其中包含了前端激光里程计、后端优化、回环检测等关键步骤。是算法原理的核心，需要好好研究。

结尾：从数据流视角解析 loam_livox 就到这了，相信大家现在可以比较清晰地在脑海中想像出原始数据是怎么被反复“折腾”的。

最后的最后，看代码时发现一个问题有点疑惑，于是在github上面提了一个 issue，结果作者没理我就关了问题。麻烦知道的朋友可以在评论区给我个解答，谢谢～

按文件、函数、原理解析，to be continued…