作者 | 密斯特李 编辑 | 汽车人
原文链接：https://zhuanlan.zhihu.com/p/511585767
点击下方卡片，关注“自动驾驶之心”公众号
ADAS巨卷干货，即可获取
后台回复【SLAM综述】获取视觉SLAM、激光SLAM、RGBD-SLAM等多篇综述！
0.预备基础
所谓“激光里程计”，是指利用激光雷达(LiDAR)进行递推地位姿状态估计的方法，其中最为著名的莫过于张楫博士的大作LOAM: LIDAR odometry and mapping in real-time（后续还有更为完整的期刊论文版本）。目前比较杰出的激光里程计解决方案基本都沿用了LOAM的整体架构和思路，并在某些方面进行了改进和完善。关于LOAM的要点介绍和代码解析可以去考古一下我当年的系列文章：
LOAM:3D激光里程计及环境建图的方法和实现（一）
LOAM:3D激光里程计及环境建图的方法和实现（二）
LOAM:3D激光里程计及环境建图的方法和实现（三）
LOAM:3D激光里程计及环境建图的方法和实现（四）
LOAM早期开源于github的代码由于没有使用第三方优化计算库，非线性优化计算完全独立编写，且求导方式也采用的是对旋转矩阵求导这种较为传统的模式，因此整体而言代码结构不够简洁。港科大沈劭劼老师团队在其基础上利用ceres对代码进行了重新整理并发布(A-LOAM)，代码的稳定性和可复用性大大提升，但在改进过程中也忽略了一个十分重要的部分，也必然会引起很多问题，这个我们后续再展开说。
总而言之，这个系列的文章面向的是对LOAM已经有一定认识的读者，因此很多细节就不再展开和赘述了，上车前请各位先自行了解相关预备基础。
今天介绍的F-LOAM是新加坡南洋理工大学的Wang Han博士的工作（其他工作将在后续介绍）论文(F-LOAM : Fast LiDAR Odometry and Mapping)发表于2020年的IROS，代码开源于他的github主页(wh200720041 / floam)。这个工作本质上是A-LOAM的进一步整理，技术方法上基本没有太大变化，重点在于代码架构上的进一步优化，使系统的运行效率有了显著地提升。
1、主要创新点及系统架构
1.1 主要创新点
根据论文中的描述，笔者将这一工作的主要创新点归纳为几点：
相较于LOAM的第一个区别：采用两步法对原始点云的畸变进行校正，第一步在位姿计算中进行，第二步在点云地图的保存和可视化时进行。
相较于LOAM的第二个区别：在位姿估计的非线性优化计算过程中对特征点约束定权，即根据特征点平滑度(smothness)的大小确定特征匹配点对在非线性优化中的作用，特征越明显，其对优化计算结果的影响越大。
相较于LOAM的第三个区别：摒弃了LOAM中odometry和mapping两线程并行的思路，将odometry的scan-to-scan的匹配移除，仅保留scan-to-map的匹配。因此，mapping线程不再进行优化计算，仅处理点云地图的保存和可视化等工作。
1.2 系统架构
系统的整体架构可以利用rosgraph清晰地看出，如下图所示：
系统在三个进程中分别运行floam_laser_processing_node，floam_odom_estimation_node，floam_laser_mapping_node等三个节点，在实际无人平台运行时由于不用可视化，因此只需运行前两个即可，对平台算力的要求显著下降。具体而言
floam_laser_processing_node节点负责从LiDAR采集到的原始点云中提取线、面特征点，一方面起到点云数据降采样的作用，另一方面提升点云匹配的准确性和鲁棒性floam_odom_estimation_node节点负责将特征点与局部特征点云地图匹配并更新局部地图，以scan-to-map的方式估计当前LiDAR的位姿。
2、主要方法及代码实现
F-LOAM中所采用的方法与LOAM基本完全一致，主要包括特征提取、畸变消除、位姿估计和地图构建等四个模块。
2.1 特征提取
for(int i = 0; i < N_SCANS; i++){ //逐点云行遍历
if(laserCloudScans[i]->points.size()<131){ // 跳过点个数较少的行
continue;
}
std::vector<Double2d> cloudCurvature;
int total_points = laserCloudScans[i]->points.size()-10;
// 逐点计算平滑度，对应式(1)。由于点数始终为定值10，因此实际计算时没有再除以10
for(int j = 5; j < (int)laserCloudScans[i]->points.size() - 5; j++){
double diffX = laserCloudScans[i]->points[j - 5].x + laserCloudScans[i]->points[j - 4].x + laserCloudScans[i]->points[j - 3].x + laserCloudScans[i]->points[j - 2].x + laserCloudScans[i]->points[j - 1].x - 10 * laserCloudScans[i]->points[j].x + laserCloudScans[i]->points[j + 1].x + laserCloudScans[i]->points[j + 2].x + laserCloudScans[i]->points[j + 3].x + laserCloudScans[i]->points[j + 4].x + laserCloudScans[i]->points[j + 5].x;
double diffY = laserCloudScans[i]->points[j - 5].y + laserCloudScans[i]->points[j - 4].y + laserCloudScans[i]->points[j - 3].y + laserCloudScans[i]->points[j - 2].y + laserCloudScans[i]->points[j - 1].y - 10 * laserCloudScans[i]->points[j].y + laserCloudScans[i]->points[j + 1].y + laserCloudScans[i]->points[j + 2].y + laserCloudScans[i]->points[j + 3].y + laserCloudScans[i]->points[j + 4].y + laserCloudScans[i]->points[j + 5].y;
double diffZ = laserCloudScans[i]->points[j - 5].z + laserCloudScans[i]->points[j - 4].z + laserCloudScans[i]->points[j - 3].z + laserCloudScans[i]->points[j - 2].z + laserCloudScans[i]->points[j - 1].z - 10 * laserCloudScans[i]->points[j].z + laserCloudScans[i]->points[j + 1].z + laserCloudScans[i]->points[j + 2].z + laserCloudScans[i]->points[j + 3].z + laserCloudScans[i]->points[j + 4].z + laserCloudScans[i]->points[j + 5].z;
Double2d distance(j,diffX * diffX + diffY * diffY + diffZ * diffZ);
cloudCurvature.push_back(distance);
}
// 将一行点云分为6个子块
for(int j=0;j<6;j++){
int sector_length = (int)(total_points/6);
int sector_start = sector_length *j;
int sector_end = sector_length *(j+1)-1;
if (j==5){
sector_end = total_points - 1;
}
std::vector<Double2d> subCloudCurvature(cloudCurvature.begin()+sector_start,cloudCurvature.begin()+sector_end);
// 对子块点云进行特征提取
featureExtractionFromSector(laserCloudScans[i],subCloudCurvature, pc_out_edge, pc_out_surf);
}
}线面特征提取代码可以参看代码中laserProcessingClass.cpp的featureExtractionFromSector函数，笔者就不在此赘述了。
2.2 畸变消除
这一部分是论文中提到的创新点之一，但实际在代码实现上与A-LOAM是相同的。论文中说畸变校正需要在位姿估计优化迭代中反复进行，因此比较耗时。我们从A-LOAM的代码中看一下，便可理解这一点：
for (size_t opti_counter = 0; opti_counter < 2; ++opti_counter)
{
//......省略部分代码
// find correspondence for corner features
for (int i = 0; i < cornerPointsSharpNum; ++i)
{
// 根据scan-to-scan的odom预测值将所有特征点统一至scan的起始时刻，即完成了畸变的校正
TransformToStart(&(cornerPointsSharp->points[i]), &pointSel);
//......省略部分代码
}
//......省略部分代码
// 非线性优化计算
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;
options.max_num_iterations = 4;
options.minimizer_progress_to_stdout = false;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
}从代码中可以看出，在每轮迭代的循环中都是要逐点进行畸变校正的，但是请注意，最外层的循环数只有2，也就是说只进行了两次逐点的畸变校正计算。在完成位姿估计的优化迭代后可以得到更为精确的到 的位姿变换，从而可以将畸变的特征点统一至scan的终止时刻，实现精确的畸变校正
// transform corner features and plane features to the scan end point
if (0)
{
int cornerPointsLessSharpNum = cornerPointsLessSharp->points.size();
for (int i = 0; i < cornerPointsLessSharpNum; i++)
{
TransformToEnd(&cornerPointsLessSharp->points[i], &cornerPointsLessSharp->points[i]);
}
int surfPointsLessFlatNum = surfPointsLessFlat->points.size();
for (int i = 0; i < surfPointsLessFlatNum; i++)
{
TransformToEnd(&surfPointsLessFlat->points[i], &surfPointsLessFlat->points[i]);
}
int laserCloudFullResNum = laserCloudFullRes->points.size();
for (int i = 0; i < laserCloudFullResNum; i++)
{
TransformToEnd(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]);
}
}然而，这段代码显然是不会被执行的，也就是说实际上在LOAM中没有对畸变点云进行最终的精确校正。在laserMapping线程中与上述实现类似，也是在位姿估计的迭代计算中由pointAssociateToMap函数进行畸变校正，但循环同样只有2次。
那么接下来在对比来看一下F-LOAM中的两步法畸变校正，畸变校正的第一步在odomEstimationClass.cpp中实现。首先，根据匀速运动模型对当前位姿进行预测：
// 这里odom为T_w_{k-1},last_odom为T_w_{k-2}.因此last_odom.inverse() * odom的结果为T_{k-2}_{k-1}
// 在匀速运动模型下T_{k-2}_{k-1} = T_{k-1}_{k}
// 因此odom * (last_odom.inverse() * odom)可以表示T_w_{k}的预测值
Eigen::Isometry3d odom_prediction = odom * (last_odom.inverse() * odom);
last_odom = odom;
odom = odom_prediction;
q_w_curr = Eigen::Quaterniond(odom.rotation());
t_w_curr = odom.translation();而后同样是在位姿估计的迭代计算中由pointAssociateToMap函数进行畸变校正：
for (int iterCount = 0; iterCount < optimization_count; iterCount++){
ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);
ceres::Problem::Options problem_options;
ceres::Problem problem(problem_options);
problem.AddParameterBlock(parameters, 7, new PoseSE3Parameterization());
// 在非线性优化方程中添加线和面特征约束，函数中包含了pointAssociateToMap的畸变校正过程
addEdgeCostFactor(downsampledEdgeCloud,laserCloudCornerMap,problem,loss_function);
addSurfCostFactor(downsampledSurfCloud,laserCloudSurfMap,problem,loss_function);
// 非线优化计算
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;
options.max_num_iterations = 4;
options.minimizer_progress_to_stdout = false;
options.check_gradients = false;
options.gradient_check_relative_precision = 1e-4;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
}在优化计算得到精确地后，可将原始的畸变点云由pointAssociateToMap函数进行畸变校正，这就是第二步。从代码对比可以看出其实现与A-LOAM中的laserMapping并无差异，但由于代码结构上丢弃了laserOdometry线程，因此必然执行效率有所提升。
2.3 位姿估计
这一部分是论文中提到的又一创新点，但实际在代码实现上也与A-LOAM是相同的。论文中叙述的根据各点平滑度定权的策略并未在开源的代码版本中体现(见上一代码块中ceres非线性优化计算部分)，仅仅只是采用了Huber函数作为损失函数。
2.4 地图构建
这一部分只进行了局部地图的管理和可视化，就不再赘述了。
3、总结
实验结果在KITTI和AGV室内仓库数据集上进行了测试，从结果上看精度与A-LOAM相差不大，略有提升，但时间消耗上大大缩减，证实了F-LOAM能够在不影响精度的前提下大幅提升运行效率，能够很轻量化的部署在算力有限的无人平台上。结合上述代码实现和对论文的解读，我们也可以得出一个结论：LOAM的laserOdometry线程并非十分必要，在多数地面平台（如：地面机器人，AGV，无人车等）中并不能为精度提升带来帮助，反而影响了运行效率。请记得这个结论，后续可能会有其他问题的探讨。
【自动驾驶之心】全栈技术交流群
自动驾驶之心是首个自动驾驶开发者社区，聚焦目标检测、语义分割、全景分割、实例分割、关键点检测、车道线、目标跟踪、3D目标检测、BEV感知、多传感器融合、SLAM、光流估计、深度估计、轨迹预测、高精地图、规划控制、模型部署落地、自动驾驶仿真测试、硬件配置、AI求职交流等方向；
加入我们：自动驾驶之心技术交流群汇总！
自动驾驶之心【知识星球】
想要了解更多自动驾驶感知（分类、检测、分割、关键点、车道线、3D目标检测、多传感器融合、目标跟踪、光流估计、轨迹预测）、自动驾驶定位建图（SLAM、高精地图）、自动驾驶规划控制、领域技术方案、AI模型部署落地实战、行业动态、岗位发布，欢迎扫描下方二维码，加入自动驾驶之心知识星球（三天内无条件退款），日常分享论文+代码，这里汇聚行业和学术界大佬，前沿技术方向尽在掌握中，期待交流！
扫一扫
4126
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
