2023面试高手

这个屌丝很懒，什么也没留下！

热门标签

详解Lio-livox中的建图与地图管理

作者：2023面试高手 | 2024-05-02 09:51:37

踩

lio-livox

lio-livox是livox官方在2021年开源的一款Lidar+IMU算法，可以用于livox mid360, horizon, HAP等不同型号的激光雷达运行LIO算法。然而，笔者并没有在网上找到详细的原理说明文档，因此尝试通过代码分析算法原理。

之前一次的推送介绍了特征提取部分，本次介绍建图和地图管理部分。由于能力有限，难免解读出现错误，请读者即使批评指正。

1. PoseEstimation节点总览

1.1 rqt_graph

还是再看一下Lio-Livox整个代码的rqt_graph，可以看到在特征提取之后就进行了姿态估计PoseEstimation。这个节点接收特征提取后带特征标签信息的完整点云/livox_full_cloud和IMU的原始数据/livox/imu，然后处理后输出里程计和点云。

1.2 一些整体说明

在介绍后面细节之前，先谈一下前面的特征提取、IMU数据和输出点云。

在特征提取部分，将一次扫描到的点云的点分成了：角点、平面点和非特征点三种，类别放在了每个点的normal_z字段，因此并不需要姿态估计节点单独接收角点、平面点等点云，只是收到完整点云后读取normal_z字段后判断特征类别种类即可。

Lio-livox根据IMU的使用方式，分为三种运行模式（如下图），分别是：不用IMU、仅用IMU做去畸变、紧耦合模式，这三种由IMU_Mode的值决定。本次推文不展开介绍紧耦合模式的处理，以IMU去畸变模式为主。

在姿态估计输出中，仅有livox_full_cloud_mapped这一个关于地图的数据，即仅有将当次扫描scan的数据映射到当前lidar坐标系后的输出，并没有输出完整的点云。而在官方可视化的rivz界面中，这个输出设置了decay即延迟消失的时间，因此看起来是有一个较为丰富的地图。所以如果需要完整地图的发布和报错，需要自己修改源码。

1.3 main函数与线程

在PoseEstimation.cpp中的main函数，可以大致了解程序运行流程。


int main(){
  ...  // 省略一些初始化、参数读取与ros的消息收发
  laserCloudFullRes.reset(new pcl::PointCloud<PointType>);
  estimator = new Estimator(filter_parameter_corner, filter_parameter_surf);
  lidarFrameList.reset(new std::list<Estimator::LidarFrame>);
  std::thread thread_process{process};
  ros::spin();
}

首先初始化了一个Estimator类，在这个类的构造函数结束时，创建了一个地图更新线程。这是第一个线程。这个线程负责地图大小等维护。


Estimator::Estimator(const float& filter_corner, const float& filter_surf){
  ...  // 省略部分内容
  threadMap = std::thread(&Estimator::threadMapIncrement, this);
}

除此之外，main的结尾启动了一个process线程，这个线程负责了建图部分。

下面详细介绍两个线程的具体工作。

2. 建图线程

建图线程位于process()函数，整个建图线程的代码精简逻辑如下：


// 关于IMU不同模式下的不同操作
if(!vimMsg.empty()){   // 有IMU数据，是IMU去畸变模式，或紧耦合模式。
 if(!LidarIMUInited){}  // 有IMU数据，没有初始化，仅用IMU的gyro积分得到旋转，平移用上次的推测，服务后面的去畸变。 - 此时的lidar的位姿用IMU的gyro积分得到旋转，用之前的平移做位置更新
 else{}    // 有IMU数据， 已经初始化了，走紧耦合这一步。 - 此时的lidar用IMU的预积分做更新
else{     // 没有IMU数据
 if(LidarIMUInited) break; // IMU和LidAR已经初始化了（只能是紧耦合模式），但没有数据，则啥也不干。
 else{}    // 没有IMU则仅执行LiDAR的LO   - 此时的lidar位姿用上次运动增量做预测；
}
 
RemoveLidarDistortion();    // 去畸变
EstimateLidarPose();        // 估计雷达姿态
// update delta transformation
 
if(IMU_Mode > 1 && !LidarIMUInited){ // 紧耦合且没有初始化时，进行初始化操作
 if(!LidarIMUInited && ...)
  LidarIMUInited = ture
}

最开始是根据IMU模式和数据状态进行一些操作，首先根据是否有IMU数据进行处理。注意有无IMU数据并不是指lidar端是否发了IMU，而是这个姿态节点在IMU模式是1和2时才会接收IMU数据，因此有IMU意味着此时为1或2模式（IMU去畸变或紧耦合）。

2.1 利用IMU估计旋转增量

如果有IMU数据，则根据一个状态量LidarIMUInited进行处理，这个初始化只有在紧耦合模式下初始化完成后才会设定为true。因此，我们在意的IMU去畸变模式（模式1）下这个值一直是false的。此时通过IMU的积分得到lidar的旋转和平移增量：


lidarFrame.imuIntegrator.PushIMUMsg(vimuMsg);
lidarFrame.imuIntegrator.GyroIntegration(time_last_lidar);
delta_Rb = lidarFrame.imuIntegrator.GetDeltaQ().toRotationMatrix();
delta_Rl = exTlb.topLeftCorner(3, 3) * delta_Rb * exTlb.topLeftCorner(3, 3).transpose
// predict current lidar pose
lidarFrame.P = transformAftMapped.topLeftCorner(3,3) * delta_tb + transformAftMapped.topRightCorner(3,1);
Eigen::Matrix3d m3d = transformAftMapped.topLeftCorner(3,3) * delta_Rb;
lidarFrame.Q = m3d;

注意到这里的delta_Rb,delta_Rl指的是一次scan时body系(IMU)和lidar系的旋转增量。IMU的旋转增量由imuIntegrator.GyroIntegration实现。考虑到旋转运动对畸变的影响远大于平移，因此此时并没有做平移的相应计算。在获取一次scan的IMU旋转增量后，转到lidar系，并预测下一时刻的位姿。从代码中可以推测，lidarFrame的P字段表示的应该是body系在world得平移。所以可以得出结论，在1模式下，IMU的数据主要是计算了旋转增量。

2.2 点云去畸变

紧接着是进行点云去畸变操作RemoveLidarDistortion，输入的变量是delta_Rl和delta_tl，即这次scan的估计旋转和平移。如果是0模式，即仅利用lidar做odometry，则这两个参数是通过下方代码计算的：


// update delta transformation
delta_Rb = transformAftMapped.topLeftCorner(3, 3).transpose() * lidar_list->front().Q.toRotationMatrix();           //~ 在纯lidar时没有用到。得到的是IMU的旋转。
delta_tb = transformAftMapped.topLeftCorner(3, 3).transpose() * (lidar_list->front().P - transformAftMapped.topRightCorner(3, 1));
Eigen::Matrix3d Rwlpre = transformAftMapped.topLeftCorner(3, 3) * exRbl;
Eigen::Vector3d Pwlpre = transformAftMapped.topLeftCorner(3, 3) * exPbl + transformAftMapped.topRightCorner(3, 1);
delta_Rl = Rwlpre.transpose() * transformTobeMapped.topLeftCorner(3,3);                 //~ 计算激光雷达两次之间的增量，用于下次去畸变；带入 Rwlpre和ToBeMap的表达式，应该为：exRbl'*AftMaped'*ToBeMap*exRbl，中间为Rb的增量，这里为增量的旋转到lidar系。
delta_tl = Rwlpre.transpose() * (transformTobeMapped.topRightCorner(3,1) - Pwlpre);     //~ 最终结果是Pre到当前ToBeMapped的lidar的平移。

即通过上上次和上一次间的位姿变换，转到lidar系，得到的，即我们常说的匀速运动模型去畸变。但对于IMU=1模式，旋转增量是上一小节IMU数据陀螺仪积分得到的，但平移量依旧是利用这段匀速模型代码处理的，delta_tl并没有被更新，应该是担心IMU的加速度计有漂移积分，加速度积分得到的平移并不准确。如果再回过头关注IMU=2即紧耦合模式，可以看到紧耦合下通过预积分得到的平移量是更新了delta_tl的，因为紧耦合IMU的预积分能够处理噪声。

再看具体的去畸变代码，比较简单不展开介绍：每个点的normal_x存储了这个点位于这圈scan的百分比，再根据这个比例对旋转和平移量进行插值后坐标变换即可。

2.3 激光雷达姿态估计

现在进入最复杂的雷达姿态估计函数，estimator->EstimateLidarPose()。这个函数干了三件事：1.根据特征类别分类存放，2.迭代优化计算位姿，3.更新局部地图

2.3.1 特征分类存放


for(const auto& l : lidarFrameList){
  laserCloudCornerLast[stack_count]->clear();
  for(const auto& p : l.laserCloud->points){
    if(std::fabs(p.normal_z - 1.0) < 1e-5)
      laserCloudCornerLast[stack_count]->push_back(p);
  }
  ...
}

即根据每个点的normal_z字段，是1/2/3分别判定为角点、平面和非特征点，然后分类存储即可，不复杂。

2.3.2 迭代优化计算位姿

获得各种特征后，在Estimate()函数中计算里程计。

首先从map_manager获取每个分块区域patch的角点、平面点、非特征点的kd树地图（用于寻找近邻）与完整点云地图。

接下来采用ceres优化库对姿态进行求解运算。ceres优化的内容在这里只是简单带过：首先构建参数块AddParameterBlock包括6维姿态和IMU的bias，然后在IMU模式为0或1时不需要考虑边缘化等，因此直接开辟了3个线程同时寻找点-线、点-面、点-点ICP的近邻关系，保存到对应的kdtree中。这一步同时构建了不同类型配准时的代价函数。


threads[0] = std::thread(&Estimator::processPointToLine, this,// 寻找点到线的匹配关系，将结果存到 vLineFeatures 中，edgesLine 是误差函数。
             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,
             ...
threads[2] = std::thread(&Estimator::processNonFeatureICP, this,
             ...

构建对应关系后，就开始构造残差AddResidualBlock（残差的计算方式在上面构建损失函数时已经实现），最后对ceres优化问题进行求解ceres::Solve。

上述过程在循环中进行，直到达到迭代次数或收敛（旋转平移增量很小）后结束迭代。至此，得到了这次扫描在整个地图中的位姿信息。

2.3.3 局部地图更新

在EstimateLidarPose函数的最后，调用了MapIncrementLocal()，用于更新局部地图。这个函数就是把角点、平面、非特征点通过坐标变换到world系，降采样然后存到laserCloudXXXFromLocal这个变量中，XXX为Corner/Surf/NonFeature。用于2.3.2中构建kdtree与计算ICP配准。

3. 地图管理线程

地图管理线程在PoseEstimation节点中创建Estimator时的构造函数里面，启动了名为threadMapIncrement线程。该函数定义为了[noreturn]函数，即告诉编译器该函数不会返回任何值。这个函数里面在while中不断执行两个操作：将lidar系得地图点投影到map当中、更新map。

投影部分是函数featureAssociateToMap，只是将每个点通过当前计算的姿态变换坐标投影到world系然后保存。

地图更新是函数MapIncrement，将投影后保存的各种点更新地图。整个地图采用了LOAM算法的地图存储策略，划分为了21*21*11大小的地图块，如果lidar的位置位于地图的边缘，则进行MapMove重新调整整个地图的索引坐标。之后，根据需要新增地图点的坐标判定处于哪个地图块，存入对应地图块并对有更新的地图块进行下采样滤波。

由于这部分内容借鉴的是LOAM算法，网上也有很多详细的介绍，因此这里不做展开。

小结

Lio-livox在完成特征提取后，通过一个建图线程，完整特征与地图的ICP配准，获得姿态参数，再通过一个地图维护线程，不断更新地图。

—END—

高效学习3D视觉三部曲

第一步加入行业交流群，保持技术的先进性

目前工坊已经建立了3D视觉方向多个社群，包括SLAM、工业3D视觉、自动驾驶方向，细分群包括：

[工业方向]三维点云、结构光、机械臂、缺陷检测、三维测量、TOF、相机标定、综合群；

[SLAM方向]多传感器融合、ORB-SLAM、激光SLAM、机器人导航、RTK|GPS|UWB等传感器交流群、SLAM综合讨论群；

[自动驾驶方向]深度估计、Transformer、毫米波|激光雷达|视觉摄像头传感器讨论群、多传感器标定、自动驾驶综合群等。

[三维重建方向]NeRF、colmap、OpenMVS、MVSNet等。

[无人机方向]四旋翼建模、无人机飞控等。

除了这些，还有求职、硬件选型、视觉产品落地等交流群。

大家可以添加小助理微信: dddvisiona，备注：加群+方向+学校|公司, 小助理会拉你入群。

添加小助理微信：cv3d007, 拉你入群

第二步加入知识星球，问题及时得到解答

3.1 「3D视觉从入门到精通」技术星球

针对3D视觉领域的视频课程（三维重建、三维点云、结构光、手眼标定、相机标定、激光/视觉SLAM、自动驾驶等）、源码分享、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答等进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业、项目对接为一体的铁杆粉丝聚集区，6000+星球成员为创造更好的AI世界共同进步，知识星球入口：「3D视觉从入门到精通」

学习3D视觉核心技术，扫描查看，3天内无条件退款