前言A-LOAM 最精妙的设计在于将复杂的激光SLAM问题解耦为两个并行的模块——高频10Hz低精度的激光里程计 低频1Hz高精度的建图优化。这种快慢搭配的架构思想后来被 LeGO-LOAM、LIO-SAM、Fast-LIO 等大量工作继承。本文将深入剖析 A-LOAM 的源码结构与数据流。1. 整体架构总览┌─────────────────────────────────────────────────────────────────┐ │ A-LOAM 系统架构 │ │ │ │ ┌─────────────────────────┐ │ │ │ scanRegistration │ ← 节点1特征提取 (10Hz) │ │ │ 曲率计算 → 角点/面点分类 │ │ │ └───────────┬─────────────┘ │ │ │ 发布 topic │ │ ┌────────┼────────┐ │ │ ▼ ▼ ▼ │ │ ┌─────────────────────────┐ │ │ │ laserOdometry │ ← 节点2激光里程计 (10Hz) │ │ │ Scan-to-Scan 匹配 │ │ │ │ 点云去畸变 Ceres优化 │ │ │ └───────────┬─────────────┘ │ │ │ 发布 /laser_cloud_corner_last, │ │ │ /laser_cloud_surf_last, │ │ │ /laser_odom_to_init │ │ ▼ │ │ ┌─────────────────────────┐ │ │ │ laserMapping │ ← 节点3建图优化 (1Hz) │ │ │ Scan-to-Map 匹配 │ │ │ │ 局部子地图 Ceres优化 │ │ │ └───────────┬─────────────┘ │ │ │ │ │ ┌────────┴────────┐ │ │ ▼ ▼ │ │ /laser_cloud_map /aft_mapped_path │ │ (全局点云地图) (高精度轨迹) │ └─────────────────────────────────────────────────────────────────┘2. 源码文件结构A-LOAM 的核心代码不到 2000 行分布在 5 个文件中src/ ├── scanRegistration.cpp (333行) 特征提取节点 ├── laserOdometry.cpp (420行) 激光里程计节点 ├── laserMapping.cpp (567行) 建图优化节点 ├── lidarFactor.hpp (215行) Ceres 残差结构体定义 ├── kittiHelper.cpp (可选) KITTI → ROS bag 转换 └── CMakeLists.txt3. scanRegistration —— 特征提取节点3.1 节点职责输入 /velodyne_points (sensor_msgs::PointCloud2) 输出 /laser_cloud_sharp —— 强角点 (每扫描线最多2个/段) /laser_cloud_less_sharp —— 弱角点 (用于 Mapping) /laser_cloud_flat —— 强面点 (每扫描线最多4个/段) /laser_cloud_less_flat —— 弱面点 (Mapping用体素降采样) /velodyne_cloud_2 —— 全部点云 (带scanID和时间戳)3.2 处理流程原始点云 (sensor_msgs::PointCloud2) │ ▼ [1] 转换为 pcl::PointCloudPointType │ ▼ [2] 预处理去除 NaN 和 距离0.1m 的噪点 │ ▼ [3] 计算扫描线 ID (scanID) 公式angle atan2(z, sqrt(x²y²)) * 180 / π scanID (angle 15) / 2 0.5 // VLP-16: -15°~15°, 16线 │ ▼ [4] 计算相对时间戳 ratio (存入 intensity 字段) ratio (scanID * 水平分辨率 水平角度偏移) / 360 │ ▼ [5] 计算曲率连续8行核心代码 │ ▼ [6] 特征分类 ├── 每条扫描线分为 6 个等份 ├── 每份按曲率从大到小排序 ├── 取曲率最大的 2 个 → 角点 ├── 取曲率最小的 4 个 → 面点 └── 避免选取与已选角点相邻的点隔离保护 │ ▼ [7] 发布5个话题3.3 曲率计算公式c1∣S∣⋅∥XiL∥∥∑j∈S,j≠i(XiL−XjL)∥c \frac{1}{|S| \cdot \|X_i^L\|} \left\| \sum_{j \in S, j \neq i} (X_i^L - X_j^L) \right\|c∣S∣⋅∥XiL∥1j∈S,ji∑(XiL−XjL)其中SSS是当前点前后各 5 个点的邻域集合共 10 个点。// scanRegistration.cpp 核心代码for(inti5;icloudSize-5;i){floatdiffXlaserCloud-points[i-5].x...laserCloud-points[i5].x-10*laserCloud-points[i].x;floatdiffYlaserCloud-points[i-5].y...laserCloud-points[i5].y-10*laserCloud-points[i].y;floatdiffZlaserCloud-points[i-5].z...laserCloud-points[i5].z-10*laserCloud-points[i].z;cloudCurvature[i]diffX*diffXdiffY*diffYdiffZ*diffZ;}直观理解如果某点与周围邻域点的位置差异大 → 曲率大 → 是角点如果与邻域平滑过渡 → 曲率小 → 是面点。3.4 特征均匀化 —— 分块策略一条扫描线被分为6段 ┌────┬────┬────┬────┬────┬────┐ │ 0 │ 1 │ 2 │ 3 │ 4 │ 5 │ └────┴────┴────┴────┴────┴────┘ 每段独立取 top2 角点 top4 面点 → 确保特征在360°视野中均匀分布 → 避免特征集中导致优化退化4. laserOdometry —— 激光里程计节点4.1 节点职责输入 /laser_cloud_sharp, /laser_cloud_flat, /velodyne_cloud_2 输出 /laser_cloud_corner_last —— 上一帧角点去畸变后 /laser_cloud_surf_last —— 上一帧面点去畸变后 /laser_odom_to_init —— 里程计累积位姿 /laser_odom_path —— 里程计轨迹NavPath /laser_odom_cloud —— 去畸变后的全部点云4.2 核心算法Scan-to-Scan 配准上一帧特征点 cloudCornerLast / cloudSurfLast 当前帧特征点 cloudCornerCurr / cloudSurfCurr │ ▼ ┌──────────────┐ │ 建立 KD-Tree │ ← 用上一帧点云构建 └──────┬───────┘ │ ▼ ┌──────────────┐ │ 寻找对应关系 │ │ 角点→找2最近邻 │ (构成直线) │ 面点→找3最近邻 │ (构成平面) └──────┬───────┘ │ ▼ ┌──────────────┐ │ Ceres 优化 │ ← 迭代4次每次更新后重新找对应 │ 求解 T_k1 │ └──────────────┘4.3 点到直线距离角点残差当前帧角点 P_curr │ │ distance ▼ ──────●──────── (由上一帧的2个最近邻角点构成的直线) P_a P_b 残差 |P_curr 到直线(P_a→P_b)的距离|dE∣(X~k1,i−Xˉk,j)×(X~k1,i−Xˉk,l)∣∣Xˉk,j−Xˉk,l∣d_{\mathcal{E}} \frac{|(\tilde{X}_{k1,i} - \bar{X}_{k,j}) \times (\tilde{X}_{k1,i} - \bar{X}_{k,l})|}{|\bar{X}_{k,j} - \bar{X}_{k,l}|}dE∣Xˉk,j−Xˉk,l∣∣(X~k1,i−Xˉk,j)×(X~k1,i−Xˉk,l)∣4.4 点到平面距离面点残差当前帧面点 P_curr │ │ distance ▼ ┌──────────────┐ (由上一帧的3个最近邻面点构成的平面) │ ●P_l │ │ P_j ●P_m │ └──────────────┘ 残差 |P_curr 到平面(P_j, P_l, P_m)的距离|dH∣(X~k1,i−Xˉk,j)⋅((Xˉk,j−Xˉk,l)×(Xˉk,j−Xˉk,m))∣∣(Xˉk,j−Xˉk,l)×(Xˉk,j−Xˉk,m)∣d_{\mathcal{H}} \frac{|(\tilde{X}_{k1,i} - \bar{X}_{k,j}) \cdot ((\bar{X}_{k,j} - \bar{X}_{k,l}) \times (\bar{X}_{k,j} - \bar{X}_{k,m}))|}{|(\bar{X}_{k,j} - \bar{X}_{k,l}) \times (\bar{X}_{k,j} - \bar{X}_{k,m})|}dH∣(Xˉk,j−Xˉk,l)×(Xˉk,j−Xˉk,m)∣∣(X~k1,i−Xˉk,j)⋅((Xˉk,j−Xˉk,l)×(Xˉk,j−Xˉk,m))∣4.5 点云去畸变激光雷达在扫描过程中自身也在运动同一帧内不同时刻采集的点参考坐标系不同。扫描开始 ←────── 360°旋转 ──────→ 扫描结束 │ │ t_start t_end 问题t_start 时刻采集的点在 LiDAR 坐标系 L_start t_end 时刻采集的点在 LiDAR 坐标系 L_end L_start ≠ L_end因为雷达在运动 解决A-LOAM 的做法 1. 假设两帧之间匀速运动 2. 已知上一帧到当前帧的相对位姿 T 3. 对每个点根据其时间戳 ratio线性插值位姿 T_ratio interpolate(I, T, ratio) 4. 将所有点变换回扫描开始时刻的坐标系5. laserMapping —— 建图优化节点5.1 节点职责输入 /laser_cloud_corner_last (里程计输出去畸变角点) /laser_cloud_surf_last (里程计输出去畸变面点) /laser_odom_to_init (里程计累积位姿) /velodyne_cloud_2 (全部点云) 输出 /laser_cloud_surround (局部子地图) /laser_cloud_map (全局地图) /aft_mapped_to_init (高精度位姿) /aft_mapped_path (高精度轨迹)5.2 核心算法Scan-to-Map 配准与 Odometry 的 Scan-to-Scan 不同Mapping 将当前帧与历史累积的局部子地图进行匹配精度更高但计算量更大。当前帧特征点 │ ▼ ┌──────────────────┐ │ 提取局部子地图 │ 21×21×11 栅格 (每格50m, 共约10km³) │ 以当前位姿为中心 │ └────────┬─────────┘ │ ▼ ┌──────────────────┐ │ KD-Tree 寻对应 │ │ 角点→5近邻→SVD │ 判断是否为直线 │ 面点→5近邻→SVD │ 判断是否为平面 └────────┬─────────┘ │ ▼ ┌──────────────────┐ │ Ceres 优化 │ 迭代2次 │ 求解全局位姿 │ └────────┬─────────┘ │ ▼ ┌──────────────────┐ │ 更新全局地图 │ 每20帧做一次体素降采样 └──────────────────┘5.3 SVD 验证特征类型为什么要 SVD在全局地图中5个最近邻不一定真的构成直线或平面需要进行验证。5个最近邻的协方差矩阵 → SVD分解 → 3个特征值 λ1 ≥ λ2 ≥ λ3 角点验证λ1 λ2 ≈ λ3 → 点分布在一条直线上 (√λ1对应直线方向) 面点验证λ1 ≈ λ2 λ3 → 点分布在一个平面上 (√λ3对应法向量方向) 不满足条件 → 放弃这个匹配点到直线距离激光建图中的版本dE∣(XL−pa)×(XL−pc)∣∣pa−pc∣d_{\mathcal{E}} \frac{\left|(X^L - p_a) \times (X^L - p_c)\right|}{|p_a - p_c|}dE∣pa−pc∣(XL−pa)×(XL−pc)其中pap_apa和pcp_cpc是直线上距离最远的两个点由 SVD 的方向向量确定。6. ROS 通信结构完整拓扑scanRegistration (10Hz) │ ┌──────────────┼──────────────┐ ▼ ▼ ▼ /laser_cloud_ /laser_cloud_ /velodyne_ sharp / flat less_sharp / cloud_2 less_flat │ │ │ └──────┬───────┘ │ ▼ │ laserOdometry (10Hz) ←───────┘ │ ┌───────────┼───────────┐ ▼ ▼ ▼ /laser_cloud_ /laser_cloud_ /laser_odom _corner_last _surf_last _to_init path │ │ │ └─────┬─────┘ │ ▼ ▼ laserMapping (1Hz) ←──┘ │ ┌─────┴─────┐ ▼ ▼ /laser_cloud_ /laser_cloud_ _surround _map /aft_mapped_path关键设计里程计10Hz 保证实时性建图1Hz 保证精度。7. 局部子地图管理7.1 栅格结构// 21×21×11 的立方体栅格intlaserCloudCenWidth21;// X方向 21 格intlaserCloudCenHeight21;// Y方向 21 格intlaserCloudCenDepth11;// Z方向 11 格// 每格边长 50m// → 覆盖范围±525m (X/Y) × ±275m (Z)7.2 滑动窗口更新当 LiDAR 移动到栅格边界时更新子地图窗口 if (当前位置距中心 50m) { 移除远离的边界栅格 添加新前进方向的栅格 重新构建 KD-Tree }8. A-LOAM 与 ORB-SLAM2/3 架构对比维度A-LOAMORB-SLAM2/3传感器3D 激光雷达相机前端曲率特征提取ORB 特征提取帧间匹配点到线/点到面距离重投影误差后端优化Ceres LM (自动求导)g2o LM优化变量6DoF 位姿6/7DoF 位姿 地图点回环检测无DBoW2 词袋模型地图类型点云 (稠密)稀疏地图点运行频率10Hz 1Hz帧率 后台线程模型3个独立ROS节点3个并行线程IMU 支持无ORB-SLAM3 有代码量~2000行~15000-25000行9. 总结A-LOAM 架构的三大设计精髓高频里程计 低频建图的解耦设计里程计 10Hz 保证实时跟踪建图 1Hz 追求极致精度两者互不阻塞Scan-to-Scan → Scan-to-Map 的精度递进先做快速的帧间匹配获得初始位姿再做慢速的帧与地图匹配精化位姿滑动窗口子地图21×21×11 的栅格子地图既避免了全局匹配的计算爆炸又保留了足够的邻域信息这套架构直接影响了后续几乎所有激光SLAM系统LeGO-LOAM 加了地面分割LIO-SAM 加了IMU和GPS因子Fast-LIO 用了迭代卡尔曼滤波替代优化是理解激光SLAM的必经之路。
A-LOAM 系统架构深度解析：解耦设计的艺术
发布时间：2026/5/30 11:19:42
前言A-LOAM 最精妙的设计在于将复杂的激光SLAM问题解耦为两个并行的模块——高频10Hz低精度的激光里程计 低频1Hz高精度的建图优化。这种快慢搭配的架构思想后来被 LeGO-LOAM、LIO-SAM、Fast-LIO 等大量工作继承。本文将深入剖析 A-LOAM 的源码结构与数据流。1. 整体架构总览┌─────────────────────────────────────────────────────────────────┐ │ A-LOAM 系统架构 │ │ │ │ ┌─────────────────────────┐ │ │ │ scanRegistration │ ← 节点1特征提取 (10Hz) │ │ │ 曲率计算 → 角点/面点分类 │ │ │ └───────────┬─────────────┘ │ │ │ 发布 topic │ │ ┌────────┼────────┐ │ │ ▼ ▼ ▼ │ │ ┌─────────────────────────┐ │ │ │ laserOdometry │ ← 节点2激光里程计 (10Hz) │ │ │ Scan-to-Scan 匹配 │ │ │ │ 点云去畸变 Ceres优化 │ │ │ └───────────┬─────────────┘ │ │ │ 发布 /laser_cloud_corner_last, │ │ │ /laser_cloud_surf_last, │ │ │ /laser_odom_to_init │ │ ▼ │ │ ┌─────────────────────────┐ │ │ │ laserMapping │ ← 节点3建图优化 (1Hz) │ │ │ Scan-to-Map 匹配 │ │ │ │ 局部子地图 Ceres优化 │ │ │ └───────────┬─────────────┘ │ │ │ │ │ ┌────────┴────────┐ │ │ ▼ ▼ │ │ /laser_cloud_map /aft_mapped_path │ │ (全局点云地图) (高精度轨迹) │ └─────────────────────────────────────────────────────────────────┘2. 源码文件结构A-LOAM 的核心代码不到 2000 行分布在 5 个文件中src/ ├── scanRegistration.cpp (333行) 特征提取节点 ├── laserOdometry.cpp (420行) 激光里程计节点 ├── laserMapping.cpp (567行) 建图优化节点 ├── lidarFactor.hpp (215行) Ceres 残差结构体定义 ├── kittiHelper.cpp (可选) KITTI → ROS bag 转换 └── CMakeLists.txt3. scanRegistration —— 特征提取节点3.1 节点职责输入 /velodyne_points (sensor_msgs::PointCloud2) 输出 /laser_cloud_sharp —— 强角点 (每扫描线最多2个/段) /laser_cloud_less_sharp —— 弱角点 (用于 Mapping) /laser_cloud_flat —— 强面点 (每扫描线最多4个/段) /laser_cloud_less_flat —— 弱面点 (Mapping用体素降采样) /velodyne_cloud_2 —— 全部点云 (带scanID和时间戳)3.2 处理流程原始点云 (sensor_msgs::PointCloud2) │ ▼ [1] 转换为 pcl::PointCloudPointType │ ▼ [2] 预处理去除 NaN 和 距离0.1m 的噪点 │ ▼ [3] 计算扫描线 ID (scanID) 公式angle atan2(z, sqrt(x²y²)) * 180 / π scanID (angle 15) / 2 0.5 // VLP-16: -15°~15°, 16线 │ ▼ [4] 计算相对时间戳 ratio (存入 intensity 字段) ratio (scanID * 水平分辨率 水平角度偏移) / 360 │ ▼ [5] 计算曲率连续8行核心代码 │ ▼ [6] 特征分类 ├── 每条扫描线分为 6 个等份 ├── 每份按曲率从大到小排序 ├── 取曲率最大的 2 个 → 角点 ├── 取曲率最小的 4 个 → 面点 └── 避免选取与已选角点相邻的点隔离保护 │ ▼ [7] 发布5个话题3.3 曲率计算公式c1∣S∣⋅∥XiL∥∥∑j∈S,j≠i(XiL−XjL)∥c \frac{1}{|S| \cdot \|X_i^L\|} \left\| \sum_{j \in S, j \neq i} (X_i^L - X_j^L) \right\|c∣S∣⋅∥XiL∥1j∈S,ji∑(XiL−XjL)其中SSS是当前点前后各 5 个点的邻域集合共 10 个点。// scanRegistration.cpp 核心代码for(inti5;icloudSize-5;i){floatdiffXlaserCloud-points[i-5].x...laserCloud-points[i5].x-10*laserCloud-points[i].x;floatdiffYlaserCloud-points[i-5].y...laserCloud-points[i5].y-10*laserCloud-points[i].y;floatdiffZlaserCloud-points[i-5].z...laserCloud-points[i5].z-10*laserCloud-points[i].z;cloudCurvature[i]diffX*diffXdiffY*diffYdiffZ*diffZ;}直观理解如果某点与周围邻域点的位置差异大 → 曲率大 → 是角点如果与邻域平滑过渡 → 曲率小 → 是面点。3.4 特征均匀化 —— 分块策略一条扫描线被分为6段 ┌────┬────┬────┬────┬────┬────┐ │ 0 │ 1 │ 2 │ 3 │ 4 │ 5 │ └────┴────┴────┴────┴────┴────┘ 每段独立取 top2 角点 top4 面点 → 确保特征在360°视野中均匀分布 → 避免特征集中导致优化退化4. laserOdometry —— 激光里程计节点4.1 节点职责输入 /laser_cloud_sharp, /laser_cloud_flat, /velodyne_cloud_2 输出 /laser_cloud_corner_last —— 上一帧角点去畸变后 /laser_cloud_surf_last —— 上一帧面点去畸变后 /laser_odom_to_init —— 里程计累积位姿 /laser_odom_path —— 里程计轨迹NavPath /laser_odom_cloud —— 去畸变后的全部点云4.2 核心算法Scan-to-Scan 配准上一帧特征点 cloudCornerLast / cloudSurfLast 当前帧特征点 cloudCornerCurr / cloudSurfCurr │ ▼ ┌──────────────┐ │ 建立 KD-Tree │ ← 用上一帧点云构建 └──────┬───────┘ │ ▼ ┌──────────────┐ │ 寻找对应关系 │ │ 角点→找2最近邻 │ (构成直线) │ 面点→找3最近邻 │ (构成平面) └──────┬───────┘ │ ▼ ┌──────────────┐ │ Ceres 优化 │ ← 迭代4次每次更新后重新找对应 │ 求解 T_k1 │ └──────────────┘4.3 点到直线距离角点残差当前帧角点 P_curr │ │ distance ▼ ──────●──────── (由上一帧的2个最近邻角点构成的直线) P_a P_b 残差 |P_curr 到直线(P_a→P_b)的距离|dE∣(X~k1,i−Xˉk,j)×(X~k1,i−Xˉk,l)∣∣Xˉk,j−Xˉk,l∣d_{\mathcal{E}} \frac{|(\tilde{X}_{k1,i} - \bar{X}_{k,j}) \times (\tilde{X}_{k1,i} - \bar{X}_{k,l})|}{|\bar{X}_{k,j} - \bar{X}_{k,l}|}dE∣Xˉk,j−Xˉk,l∣∣(X~k1,i−Xˉk,j)×(X~k1,i−Xˉk,l)∣4.4 点到平面距离面点残差当前帧面点 P_curr │ │ distance ▼ ┌──────────────┐ (由上一帧的3个最近邻面点构成的平面) │ ●P_l │ │ P_j ●P_m │ └──────────────┘ 残差 |P_curr 到平面(P_j, P_l, P_m)的距离|dH∣(X~k1,i−Xˉk,j)⋅((Xˉk,j−Xˉk,l)×(Xˉk,j−Xˉk,m))∣∣(Xˉk,j−Xˉk,l)×(Xˉk,j−Xˉk,m)∣d_{\mathcal{H}} \frac{|(\tilde{X}_{k1,i} - \bar{X}_{k,j}) \cdot ((\bar{X}_{k,j} - \bar{X}_{k,l}) \times (\bar{X}_{k,j} - \bar{X}_{k,m}))|}{|(\bar{X}_{k,j} - \bar{X}_{k,l}) \times (\bar{X}_{k,j} - \bar{X}_{k,m})|}dH∣(Xˉk,j−Xˉk,l)×(Xˉk,j−Xˉk,m)∣∣(X~k1,i−Xˉk,j)⋅((Xˉk,j−Xˉk,l)×(Xˉk,j−Xˉk,m))∣4.5 点云去畸变激光雷达在扫描过程中自身也在运动同一帧内不同时刻采集的点参考坐标系不同。扫描开始 ←────── 360°旋转 ──────→ 扫描结束 │ │ t_start t_end 问题t_start 时刻采集的点在 LiDAR 坐标系 L_start t_end 时刻采集的点在 LiDAR 坐标系 L_end L_start ≠ L_end因为雷达在运动 解决A-LOAM 的做法 1. 假设两帧之间匀速运动 2. 已知上一帧到当前帧的相对位姿 T 3. 对每个点根据其时间戳 ratio线性插值位姿 T_ratio interpolate(I, T, ratio) 4. 将所有点变换回扫描开始时刻的坐标系5. laserMapping —— 建图优化节点5.1 节点职责输入 /laser_cloud_corner_last (里程计输出去畸变角点) /laser_cloud_surf_last (里程计输出去畸变面点) /laser_odom_to_init (里程计累积位姿) /velodyne_cloud_2 (全部点云) 输出 /laser_cloud_surround (局部子地图) /laser_cloud_map (全局地图) /aft_mapped_to_init (高精度位姿) /aft_mapped_path (高精度轨迹)5.2 核心算法Scan-to-Map 配准与 Odometry 的 Scan-to-Scan 不同Mapping 将当前帧与历史累积的局部子地图进行匹配精度更高但计算量更大。当前帧特征点 │ ▼ ┌──────────────────┐ │ 提取局部子地图 │ 21×21×11 栅格 (每格50m, 共约10km³) │ 以当前位姿为中心 │ └────────┬─────────┘ │ ▼ ┌──────────────────┐ │ KD-Tree 寻对应 │ │ 角点→5近邻→SVD │ 判断是否为直线 │ 面点→5近邻→SVD │ 判断是否为平面 └────────┬─────────┘ │ ▼ ┌──────────────────┐ │ Ceres 优化 │ 迭代2次 │ 求解全局位姿 │ └────────┬─────────┘ │ ▼ ┌──────────────────┐ │ 更新全局地图 │ 每20帧做一次体素降采样 └──────────────────┘5.3 SVD 验证特征类型为什么要 SVD在全局地图中5个最近邻不一定真的构成直线或平面需要进行验证。5个最近邻的协方差矩阵 → SVD分解 → 3个特征值 λ1 ≥ λ2 ≥ λ3 角点验证λ1 λ2 ≈ λ3 → 点分布在一条直线上 (√λ1对应直线方向) 面点验证λ1 ≈ λ2 λ3 → 点分布在一个平面上 (√λ3对应法向量方向) 不满足条件 → 放弃这个匹配点到直线距离激光建图中的版本dE∣(XL−pa)×(XL−pc)∣∣pa−pc∣d_{\mathcal{E}} \frac{\left|(X^L - p_a) \times (X^L - p_c)\right|}{|p_a - p_c|}dE∣pa−pc∣(XL−pa)×(XL−pc)其中pap_apa和pcp_cpc是直线上距离最远的两个点由 SVD 的方向向量确定。6. ROS 通信结构完整拓扑scanRegistration (10Hz) │ ┌──────────────┼──────────────┐ ▼ ▼ ▼ /laser_cloud_ /laser_cloud_ /velodyne_ sharp / flat less_sharp / cloud_2 less_flat │ │ │ └──────┬───────┘ │ ▼ │ laserOdometry (10Hz) ←───────┘ │ ┌───────────┼───────────┐ ▼ ▼ ▼ /laser_cloud_ /laser_cloud_ /laser_odom _corner_last _surf_last _to_init path │ │ │ └─────┬─────┘ │ ▼ ▼ laserMapping (1Hz) ←──┘ │ ┌─────┴─────┐ ▼ ▼ /laser_cloud_ /laser_cloud_ _surround _map /aft_mapped_path关键设计里程计10Hz 保证实时性建图1Hz 保证精度。7. 局部子地图管理7.1 栅格结构// 21×21×11 的立方体栅格intlaserCloudCenWidth21;// X方向 21 格intlaserCloudCenHeight21;// Y方向 21 格intlaserCloudCenDepth11;// Z方向 11 格// 每格边长 50m// → 覆盖范围±525m (X/Y) × ±275m (Z)7.2 滑动窗口更新当 LiDAR 移动到栅格边界时更新子地图窗口 if (当前位置距中心 50m) { 移除远离的边界栅格 添加新前进方向的栅格 重新构建 KD-Tree }8. A-LOAM 与 ORB-SLAM2/3 架构对比维度A-LOAMORB-SLAM2/3传感器3D 激光雷达相机前端曲率特征提取ORB 特征提取帧间匹配点到线/点到面距离重投影误差后端优化Ceres LM (自动求导)g2o LM优化变量6DoF 位姿6/7DoF 位姿 地图点回环检测无DBoW2 词袋模型地图类型点云 (稠密)稀疏地图点运行频率10Hz 1Hz帧率 后台线程模型3个独立ROS节点3个并行线程IMU 支持无ORB-SLAM3 有代码量~2000行~15000-25000行9. 总结A-LOAM 架构的三大设计精髓高频里程计 低频建图的解耦设计里程计 10Hz 保证实时跟踪建图 1Hz 追求极致精度两者互不阻塞Scan-to-Scan → Scan-to-Map 的精度递进先做快速的帧间匹配获得初始位姿再做慢速的帧与地图匹配精化位姿滑动窗口子地图21×21×11 的栅格子地图既避免了全局匹配的计算爆炸又保留了足够的邻域信息这套架构直接影响了后续几乎所有激光SLAM系统LeGO-LOAM 加了地面分割LIO-SAM 加了IMU和GPS因子Fast-LIO 用了迭代卡尔曼滤波替代优化是理解激光SLAM的必经之路。
