前面聊了LOAM、LeGO-LOAM和Cartographer三种不同的激光SLAM方案。这篇来看另一个重要方案——HDL-Graph-SLAM。HDL是Handheld LiDAR的缩写最初是为手持激光扫描仪设计的后来发展成了通用的激光SLAM框架。它是日本早稻田大学的Kenji Koide在2018年发表的后来在2021年又发表了改进版HDL-Graph-SLAM发表在IEEE RA-L上。这个方案的核心特点是用因子图Factor Graph来组织所有的约束包括里程计、回环、IMU、地面平面等。因子图的灵活性让它很容易扩展你可以方便地加入GPS、轮式里程计等其他传感器。Kenji Koide的开源代码质量很高是目前学习因子图SLAM的好材料。面试中HDL-Graph-SLAM经常被拿来和LeGO-LOAM对比你得清楚两者的架构差异。HDL-Graph-SLAM的整体架构HDL-Graph-SLAM的架构很清晰也是前后端分离的。前端NDT配准做里程计。用NDTNormal Distributions Transform做帧到帧的配准得到位姿估计。NDT不需要特征提取直接用点云的统计分布做匹配。后端因子图优化。把所有的约束都建模成因子用g2o做图优化。因子包括NDT里程计因子、回环因子、IMU因子、地面平面因子。g2o是开源的图优化库支持多种因子类型效率很高。# HDL-Graph-SLAM的核心流程 class HDL_Graph_SLAM: def process(self, cloud, imu_data): # 前端NDT配准 pose ndt_matching(cloud, prev_cloud) # 添加里程计因子 factor_graph.add_odometry_factor(pose) # 添加IMU因子 factor_graph.add_imu_factor(imu_data) # 添加地面因子 factor_graph.add_floor_factor(cloud) # 回环检测 if detect_loop_closure(cloud): factor_graph.add_loop_factor() # 后端图优化 factor_graph.optimize() return pose因子图的设计思想因子图是HDL-Graph-SLAM的核心。因子图是一种概率图模型用图来表示变量之间的约束关系。节点Node状态变量比如每一帧的位姿。因子Factor约束关系比如两帧之间的相对位姿里程计因子、回环检测到的约束回环因子、IMU测量的加速度和角速度IMU因子。因子图的好处是灵活可以方便地加入各种约束。比如你有GPS数据就加个GPS因子你有轮式里程计就加个轮式因子。每种因子有自己的误差函数和优化权重。常见的因子类型里程计因子相邻帧之间的相对位姿约束由NDT配准得到。回环因子检测到回环时当前帧和历史帧之间的约束。IMU因子IMU测量的加速度和角速度约束位姿的变化。地面平面因子如果检测到地面约束位姿的roll、pitch和高度。GPS因子GPS提供的全局位置约束。每种因子的权重不同反映了你对这个传感器的信任程度。比如GPS的噪声大权重就设小一点IMU的短期精度高权重就设大一点。# 因子图的核心概念 class FactorGraph: def __init__(self): self.nodes {} # 位姿节点 self.factors [] # 约束因子 def add_node(self, pose_id, pose): self.nodes[pose_id] pose def add_factor(self, factor): self.factors.append(factor) def optimize(self): # 用g2o优化所有因子 g2o.optimize(self.nodes, self.factors)NDT配准的原理HDL-Graph-SLAM的前端用的是NDT配准而不是ICP或者特征匹配。NDT的核心思想是把点云分成小的体素voxel每个体素内的点用高斯分布来近似。这样点云就变成了多个高斯分布的集合。配准的时候最大化当前点云在目标点云高斯分布下的似然概率。NDT的好处是不需要找对应点避免了ICP的对应点搜索开销。而且NDT对噪声的鲁棒性比ICP好因为用的是统计分布而不是单个点。NDT的体素分辨率是个关键参数。分辨率太大每个体素内的点分布太散高斯近似不准确。分辨率太小体素数量太多计算量大而且有些体素内点太少高斯分布不稳定。一般根据点云的密度来调室内场景1-2米比较合适室外场景可以大一些。HDL-Graph-SLAM用的NDT实现来自PCLPoint Cloud Library这是点云处理的标准库。PCL的NDT实现效率不错支持多线程加速。# NDT配准的核心思想 def ndt_matching(source, target): # 目标点云体素化 voxels voxelize(target, resolution1.0) for voxel in voxels: voxel.mean compute_mean(voxel.points) voxel.cov compute_covariance(voxel.points) # 最大化似然概率 pose optimize(lambda T: ndt_likelihood(source, T, voxels)) return pose面试追问环节面试官HDL-Graph-SLAM和LeGO-LOAM的主要区别是什么两个主要区别。一是前端配准方式不同HDL用NDTLeGO-LOAM用特征匹配。二是后端优化框架不同HDL用因子图g2oLeGO-LOAM用iSAM2。因子图更灵活可以方便地加入各种约束。iSAM2是增量式的适合实时更新。面试官HDL-Graph-SLAM适合什么场景HDL-Graph-SLAM最初是为手持设备设计的适合室内建图、地下矿井、隧道巡检这些场景。它的因子图框架可以融合多种传感器扩展性很好。后来也被用在自动驾驶和机器人上效果都不错。特别是需要融合多种传感器的场景HDL-Graph-SLAM的因子图框架很有优势。面试官因子图优化和位姿图优化有什么区别位姿图优化是因子图优化的一个特例。位姿图只有一种节点位姿和一种因子相对位姿约束。因子图更通用可以有多种节点和多种因子。比如HDL-Graph-SLAM里除了位姿节点还有速度节点、IMU偏置节点等。因子也不只是相对位姿还有IMU因子、地面因子等。面试官NDT和ICP有什么区别ICP需要找对应点计算量大容易陷入局部最优。NDT不需要找对应点用体素的高斯分布做匹配对噪声更鲁棒。但NDT的体素分辨率要调好太大匹配精度差太小计算量大。另外NDT对初始位姿也有要求初始偏差太大会收敛到错误解。面试官实际项目中用HDL-Graph-SLAM要注意什么NDT的体素分辨率要调不同场景下最优分辨率不一样。因子图的权重也要调不同因子的置信度不同权重设置要合理。还有HDL-Graph-SLAM的内存消耗比较大长时间运行要注意内存管理。地面平面因子的前提是场景有明确的地面如果没有地面比如楼梯这个因子就不适用了。回环检测的参数也要调太敏感会误检太迟钝会漏检。HDL-Graph-SLAM展示了因子图在SLAM中的强大应用能力。因子图的灵活性让它很容易扩展加入新的传感器或者约束。这种基于因子图的思路现在已经成为SLAM的主流架构之一。Kenji Koide后来还发表了FastSLAM和IRIS等改进版本性能越来越强。学HDL-Graph-SLAM建议直接看源码代码结构很清晰文档也很完善。跑一下官方的demo感受因子图优化的效果。上一篇第252篇 Cartographer深度解析——Google出品的激光SLAM方案下一篇我们聊回环检测这是SLAM中消除累积误差的关键技术。如果这篇文章对你有帮助欢迎点赞支持一下你的鼓励是我持续更新的动力