lio-livox是livox官方在2021年開源的一款Lidar+IMU算法，可以用于livox mid360, horizon, HAP等不同型號(hào)的激光雷達(dá)運(yùn)行LIO算法。然而，筆者并沒有在網(wǎng)上找到詳細(xì)的原理說明文檔，因此嘗試通過代碼分析算法原理。本文首先介紹特征提取的部分，由于能力有限，難免解讀出現(xiàn)錯(cuò)誤，請(qǐng)讀者即使批評(píng)指正。

作者：小L | 來源：3DCV

在公眾號(hào)「3DCV」后臺(tái)，回復(fù)「原論文」即可獲取論文pdf和代碼。

添加微信：dddvisiona，備注：SLAM，拉你入群。文末附行業(yè)細(xì)分群。

開源代碼：https://github.com/livox-SDK/LIO-Livox

1. 程序整體運(yùn)行

首先放上正常運(yùn)行時(shí)的rqt_graph，可以看出，lio-livox主要有兩個(gè)節(jié)點(diǎn)：ScanRegistration 和 PoseEstimation。顯然，第一個(gè)時(shí)特征提取部分，后面的是位姿軌跡部分。但有趣的是，前者發(fā)布了完整點(diǎn)云livox_full_cloud、不那么sharp和flat的點(diǎn)云、以及非特征點(diǎn)，但并沒有被后面節(jié)點(diǎn)接收。所以說，這個(gè)livox_full_cloud 就包含了特征點(diǎn)的信息。

2. 參數(shù)配置

在ScanRegistration.cpp中的main函數(shù)中，主要載入了相關(guān)的配置文件，和收發(fā)數(shù)據(jù)的節(jié)點(diǎn)。因此，首先了解下配置文件中有哪些基本配置。

首先在運(yùn)行代碼時(shí)，會(huì)運(yùn)行：

roslaunch?lio-livox?mid.launch

所以查看這個(gè)launch文件，文件截圖如下：

首先載入了mid360的配置文件，"mid360_config.yaml"，然后設(shè)置了msg_type為0，和其他參數(shù)。msg_type為激光雷達(dá)發(fā)出的數(shù)據(jù)格式，有l(wèi)ivox自定義數(shù)據(jù)類型格式CustomMsg和ros標(biāo)準(zhǔn)的PointCloud2格式，因此首先需要確定激光雷達(dá)端配置的是哪種格式。可以從激光雷達(dá)的ros驅(qū)動(dòng)中看到驅(qū)動(dòng)端有三種格式可以選，驅(qū)動(dòng)端的數(shù)字2對(duì)應(yīng)lio-livox中的1，驅(qū)動(dòng)端的0對(duì)應(yīng)lio-livox的格式0。

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

• lidar_type表示采用horizon還是mid360激光雷達(dá)。查了一下horizon的雷達(dá)掃描，雖然是固態(tài)的激光雷達(dá)，但是掃描是水平的，因此我推測(cè)應(yīng)該和mid360這種水平掃描的數(shù)據(jù)在特征提取這些方法上是類似的，因此這一個(gè)配置文件可以同時(shí)支持horizon和mid360兩種型號(hào)。

• Used_Line為采用的線束，mid360數(shù)據(jù)格式具有l(wèi)ine字段，可以輸出以下這個(gè)值是0~3，因此推測(cè)mid360的線束就是4，因此這里采用4條線進(jìn)行處理。

• Feature_Mode的參數(shù)值可選0或1，但代碼中并沒有用到這個(gè)變量，我推測(cè)可能是之前或之后版本想實(shí)現(xiàn)基于特征的配準(zhǔn)或直接不用特征的配準(zhǔn)吧。

• Use_Seg是是否啟用動(dòng)態(tài)物體分割，如果啟用，則特征提取前先經(jīng)過動(dòng)態(tài)物體濾波。

再往后的一些參數(shù)從名字可以看出含義，不多做解釋。

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函數(shù)中，可以看出，如果選用的是mid360，采用的默認(rèn)livox的數(shù)據(jù)格式，則進(jìn)入的是Horizon回調(diào)函數(shù)進(jìn)行數(shù)據(jù)處理lidarCallBackHorizon。

3. 特征提取

下面正式進(jìn)入特征提取。不展開介紹動(dòng)態(tài)物體分割部分的代碼，直接看特征提取?？梢钥吹?，在Horizon回調(diào)中，最核心的函數(shù)就是FeatureExtract。特征提取后，得到帶有特征標(biāo)記的完整點(diǎn)云，和角點(diǎn)、平面點(diǎn)點(diǎn)云（但這兩個(gè)沒有用到）。

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);??}???//?省略后續(xù)格式轉(zhuǎn)化}

void?LidarFeatureExtractor::FeatureExtract(){??//?省略一些預(yù)處理?...??//?多線程特征提取??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;????}??}??//~?省略后續(xù)處理}

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));