lio-livox是livox官方在2021年开源的一款Lidar+IMU算法，可以用于livox mid360, horizon, HAP等不同型号的激光雷达运行LIO算法。然而，笔者并没有在网上找到详细的原理说明文档，因此尝试通过代码分析算法原理。本文首先介绍特征提取的部分，由于能力有限，难免解读出现错误，请读者即使批评指正。

作者：小L | 来源：3DCV

在公众号「3DCV」后台，回复「原论文」即可获取论文pdf和代码。

添加微信：dddvisiona，备注：SLAM，拉你入群。文末附行业细分群。

开源代码：https://github.com/livox-SDK/LIO-Livox

1. 程序整体运行

首先放上正常运行时的rqt_graph，可以看出，lio-livox主要有两个节点：ScanRegistration 和 PoseEstimation。显然，第一个时特征提取部分，后面的是位姿轨迹部分。但有趣的是，前者发布了完整点云livox_full_cloud、不那么sharp和flat的点云、以及非特征点，但并没有被后面节点接收。所以说，这个livox_full_cloud 就包含了特征点的信息。

2. 参数配置

在ScanRegistration.cpp中的main函数中，主要载入了相关的配置文件，和收发数据的节点。因此，首先了解下配置文件中有哪些基本配置。

首先在运行代码时，会运行：

roslaunch lio-livox mid.launch

所以查看这个launch文件，文件截图如下：

首先载入了mid360的配置文件，"mid360_config.yaml"，然后设置了msg_type为0，和其他参数。msg_type为激光雷达发出的数据格式，有livox自定义数据类型格式CustomMsg和ros标准的PointCloud2格式，因此首先需要确定激光雷达端配置的是哪种格式。可以从激光雷达的ros驱动中看到驱动端有三种格式可以选，驱动端的数字2对应lio-livox中的1，驱动端的0对应lio-livox的格式0。

接着在"mid360_config.yaml"文件中定义了一些配置，如下：

• lidar_type表示采用horizon还是mid360激光雷达。查了一下horizon的雷达扫描，虽然是固态的激光雷达，但是扫描是水平的，因此我推测应该和mid360这种水平扫描的数据在特征提取这些方法上是类似的，因此这一个配置文件可以同时支持horizon和mid360两种型号。

• Used_Line为采用的线束，mid360数据格式具有line字段，可以输出以下这个值是0~3，因此推测mid360的线束就是4，因此这里采用4条线进行处理。

• Feature_Mode的参数值可选0或1，但代码中并没有用到这个变量，我推测可能是之前或之后版本想实现基于特征的配准或直接不用特征的配准吧。

• Use_Seg是是否启用动态物体分割，如果启用，则特征提取前先经过动态物体滤波。

再往后的一些参数从名字可以看出含义，不多做解释。

if (Lidar_Type == 0)  {    customCloud = nodeHandler.subscribe<livox_ros_driver::CustomMsg>("/livox/lidar", 100, &lidarCallBackHorizon);  }  else if (Lidar_Type == 1)  {    customCloud = nodeHandler.subscribe<livox_ros_driver::CustomMsg>("/livox/lidar", 100, &lidarCallBackHAP);  }  else if(Lidar_Type==2){      if (msg_type==0)          customCloud = nodeHandler.subscribe<livox_ros_driver::CustomMsg>("/livox/lidar", 100, &lidarCallBackHorizon);      else if(msg_type==1)          pc2Cloud=nodeHandler.subscribe<sensor_msgs::PointCloud2>("/livox/lidar", 100, &lidarCallBackPc2);  }

在接下来的main函数中，可以看出，如果选用的是mid360，采用的默认livox的数据格式，则进入的是Horizon回调函数进行数据处理lidarCallBackHorizon。

3. 特征提取

下面正式进入特征提取。不展开介绍动态物体分割部分的代码，直接看特征提取。可以看到，在Horizon回调中，最核心的函数就是FeatureExtract。特征提取后，得到带有特征标记的完整点云，和角点、平面点点云（但这两个没有用到）。

void lidarCallBackHorizon(const livox_ros_driver::CustomMsgConstPtr &msg) {  sensor_msgs::PointCloud2 msg2;  if(Use_seg){    lidarFeatureExtractor->FeatureExtract_with_segment(msg, laserCloud, laserConerCloud, laserSurfCloud, laserNonFeatureCloud, msg2,N_SCANS);  }  else{    lidarFeatureExtractor->FeatureExtract(msg, laserCloud, laserConerCloud, laserSurfCloud,N_SCANS,Lidar_Type);  }   // 省略后续格式转化}

void LidarFeatureExtractor::FeatureExtract(){  // 省略一些预处理 ...  // 多线程特征提取  std::thread threads[N_SCANS];  for(int i=0; i<N_SCANS; ++i){    threads[i] = std::thread(&LidarFeatureExtractor::detectFeaturePoint, this, std::ref(vlines[i]), std::ref(vcorner[i]), std::ref(vsurf[i]));  }  for(int i=0; i<N_SCANS; ++i){    // 线程合并    threads[i].join();  }  for(int i=0; i<N_SCANS; ++i){    for(int j=0; j<vcorner[i].size(); ++j){      laserCloud->points[_float_as_int(vlines[i]->points[vcorner[i][j]].normal_z)].normal_z = 1.0;    }    for(int j=0; j<vsurf[i].size(); ++j){      laserCloud->points[_float_as_int(vlines[i]->points[vsurf[i][j]].normal_z)].normal_z = 2.0;    }  }  //~ 省略后续处理}

laserCloud->points[_float_as_int(vlines[i]->points[vcorner[i][j]].normal_z)].normal_z = 1.0;

if(CloudFeatureFlag[i] == 2){    pointsLessFlat.push_back(i);    num_surf++;    continue;  }if(CloudFeatureFlag[i] == 100 || CloudFeatureFlag[i] == 150){ //  pointsLessSharp_ori.push_back(i);  laserCloudCorner->push_back(_laserCloud->points[i]);}

threads[0] = std::thread(&Estimator::processPointToLine, this,                         std::ref(edgesLine[f]),                         std::ref(vLineFeatures[f]),                         std::ref(laserCloudCornerStack[f]),                         std::ref(laserCloudCornerFromLocal),                         std::ref(kdtreeCornerFromLocal),                         std::ref(exTlb),                         std::ref(transformTobeMapped));threads[1] = std::thread(&Estimator::processPointToPlanVec, this,                         std::ref(edgesPlan[f]),                         std::ref(vPlanFeatures[f]),                         std::ref(laserCloudSurfStack[f]),                         std::ref(laserCloudSurfFromLocal),                         std::ref(kdtreeSurfFromLocal),                         std::ref(exTlb),                         std::ref(transformTobeMapped));