SLAM技术全景拆解：从滤波到图优化与激光视觉融合

最近在整理无人驾驶和移动机器人定位相关的内容时，发现很多初学者对 SLAM（Simultaneous Localization and Mapping，同步定位与建图）这套体系最大的困惑，不是单点算法不会用，而是坐标系、滤波、图优化、传感器融合这几条线串不起来。网上的资料大多是“视觉 SLAM 十四讲”式的分章讲解，或者直接丢一个开源框架让你跑，真正能打通“为什么用滤波、为什么转图优化、激光和视觉怎么融合”的闭环教程并不多。

这篇文章我会按照一条主线来拆解：坐标系与标定 → 滤波类 SLAM → 图优化与因子图 → 激光视觉融合 → 实战 Demo。不管你是刚接触 SLAM 的学生，还是已经在做机器人、无人驾驶相关项目的开发者，这篇文章的目标都是让你读完能建立起完整的技术框架，并且能自己动手跑一个简单的融合定位示例。

文章会覆盖高频知识点：卡尔曼滤波、扩展卡尔曼滤波（EKF）、粒子滤波、滑动窗口滤波、因子图优化、外参标定、多帧点云积累、前端里程计与后端优化的关系等。涉及代码的部分我会给出可运行的示例，尽量做到“拿到就能跑，跑完能理解”。

1. SLAM 与无人驾驶定位：为什么它是核心中的核心

1.1 什么是 SLAM

SLAM 全称是 Simultaneous Localization and Mapping，翻译过来就是“同步定位与建图”。它要解决的是一个经典问题：一个移动的传感器平台（车、机器人、无人机）在未知环境中，如何一边确定自己的位置，一边构建环境地图？

这两个问题互相依赖：定位需要地图，建图需要准确的位姿。所以 SLAM 必须同时求解，而不是先定位再建图。

在无人驾驶场景中，定位精度直接决定车辆能不能安全行驶在城市道路、隧道、地下停车场等复杂场景中。对于 L3 级以上的自动驾驶系统，业内人士对定位精度的要求通常是厘米级到分米级。单纯靠 GPS 远远不够——城市峡谷里卫星信号被遮挡，误差可能直接到十几米；隧道里干脆没有信号。这时候就需要 SLAM 结合视觉、激光雷达、IMU（惯性测量单元）等多种传感器来提供高精度的连续定位。

1.2 SLAM 在无人驾驶中的定位链路

完整的无人驾驶定位系统通常分为两层：

1. 全局定位：通过 GPS、高精地图匹配等方式，确定车辆在全球坐标系中的大致位置。
2. 局部定位与建图：通过 SLAM 持续估计车辆相对于周围环境的位姿，并生成/更新局部地图，弥补 GPS 信号丢失或误差过大的场景。

SLAM 输出的位姿（位置 + 姿态）还会被下游路径规划、决策控制模块使用。因此，SLAM 的实时性和鲁棒性直接关系到整个系统的安全。

1.3 视觉 SLAM 与激光 SLAM

按传感器类型划分，SLAM 主要有两大分支：

两者结合就是激光视觉融合 SLAM，用视觉弥补激光的语义与纹理不足，用激光弥补视觉的深度与光照敏感问题，这也是当前无人驾驶和高级辅助驾驶系统中比较主流的发展方向。

2. 坐标系与标定：所有融合定位的第一步

2.1 常用坐标系

做 SLAM 和传感器融合，首先要弄清楚各个坐标系之间的转换关系。常见的有：

• 世界坐标系（World/W）：固定在地面上的全局坐标系，是所有位姿估计的基准。
• 车体坐标系（Body/Base）：固定在被测车辆/机器人本体上，通常原点在车辆后轴中心或 IMU 中心。
• 传感器坐标系（Sensor）：相机、激光雷达、IMU 各自有独立的坐标系。
• 像素坐标系（Pixel）：视觉图像中的二维像素坐标。

SLAM 的核心工作之一，就是把不同传感器坐标系下的测量值统一到同一个坐标系下。

2.2 刚体变换与位姿表示

两个坐标系之间的关系可以用一个 4×4 的齐次变换矩阵表示：

T = | R t | | 0 1 |

其中 R 是 3×3 旋转矩阵，t 是 3×1 平移向量。一个三维点 P 从一个坐标系变换到另一个坐标系：

P' = T * P

在代码中，工程上通常用四元数（Quaternion）表示旋转，避免欧拉角的万向锁问题。比如一个常见的变换示例：

import numpy as np from scipy.spatial.transform import Rotation as R # 构造一个平移 (1, 2, 3)，旋转为绕 Z 轴 90 度 t = np.array([1.0, 2.0, 3.0]) quat = [0.0, 0.0, 0.7071, 0.7071] # [x, y, z, w] rot = R.from_quat(quat) T = np.eye(4) T[:3, :3] = rot.as_matrix() T[:3, 3] = t # 原始点 p = np.array([0.0, 0.0, 0.0, 1.0]) p_transformed = T @ p print("变换后的点：", p_transformed[:3])

这段代码演示了如何用四元数构造齐次变换矩阵并完成点云的坐标变换。实际项目中，这个操作会被大量重复执行，所以一定要理解清晰。

2.3 外参标定：以相机-激光雷达为例

说到激光视觉融合，必须面对外参标定问题。所谓外参，就是两个传感器坐标系之间的相对位姿。

使用 Kalibr 做相机内参标定是比较通用的流程。大体步骤如下：

1. 打印棋盘格或 AprilGrid 标定板。
2. 录制一段相机拍摄标定板的 rosbag 数据。
3. 使用 Kalibr 工具检查数据质量并标定内参（焦距、主点、畸变系数）。

对于激光雷达与相机的外参标定，更常用的开源工具是lidar_camera_calibration或者Autoware 的标定工具。核心思路是：通过提取标定板在点云中的平面和图像中的角点，将 3D-2D 对应点代入 PnP 求解，得到外参矩阵。

强调一点：标定结果的精度直接影响后续所有融合算法的效果。外参误差哪怕只有 1 厘米、1 度，在远距离融合时都会被放大成很大的投影误差。所以工程上建议定期校核外参，特别是车辆收到颠簸或碰撞后。

3. 滤波类 SLAM：从卡尔曼滤波到粒子滤波

3.1 为什么 SLAM 需要滤波

传感器数据都带有噪声。激光雷达测距有高斯噪声，相机位姿估计有漂移，IMU 有零偏和积分累积误差。滤波的作用，就是把多个带噪声的测量融合起来，估计出更准确的系统状态（通常是位姿）。

在 SLAM 中，“滤波”这个词有两大含义：

1. 状态估计滤波：如卡尔曼滤波（KF）、扩展卡尔曼滤波（EKF）、粒子滤波（PF），用于在线递归地估计机器人位姿。
2. 信号滤波：如中值滤波、高斯滤波、低通滤波，用于对原始传感器数据进行去噪和数据预处理。

这两类滤波你都会在 SLAM 工程中遇到。本节我们先讲状态估计滤波，信号滤波会放到第 5 节传感器数据处理中结合实战讲解。

3.2 卡尔曼滤波（KF）

卡尔曼滤波适用于线性高斯系统。它的核心思想是：用状态预测和观测更新两步交替进行，得到最优状态估计。

• 预测：利用运动模型预测当前状态和协方差。
• 更新：利用观测模型校正预测结果。

下面是一个一维卡尔曼滤波的完整 Python 示例：

import numpy as np import matplotlib.pyplot as plt # 真实值：位置随时间匀速变化 dt = 0.1 v = 1.0 # 速度 true_pos = np.arange(0, 100) * v * dt # 带噪声的观测 np.random.seed(42) noise_std = 0.3 measurements = true_pos + np.random.normal(0, noise_std, size=len(true_pos)) # 卡尔曼滤波参数 x = 0.0 # 初始状态 P = 1.0 # 初始协方差 A = 1.0 # 状态转移矩阵（简单位置模型） H = 1.0 # 观测矩阵 Q = 0.01 # 过程噪声 R = 0.09 # 观测噪声（即 noise_std^2） filtered_pos = [] for z in measurements: # 预测 x_pred = A * x P_pred = A * P * A + Q # 更新 K = P_pred * H / (H * P_pred * H + R) x = x_pred + K * (z - H * x_pred) P = (1 - K * H) * P_pred filtered_pos.append(x) # 绘制对比 plt.figure(figsize=(10, 4)) plt.plot(true_pos, label='True', linewidth=2) plt.plot(measurements, label='Measurement', alpha=0.6) plt.plot(filtered_pos, label='Filtered', linewidth=2) plt.legend() plt.title('1D Kalman Filter') plt.xlabel('Step') plt.ylabel('Position') plt.grid(True) plt.savefig('kalman_demo.png', dpi=120)

运行这段代码后，你会得到三条曲线：真实位置、带噪声的观测、滤波后的位置。滤波输出明显比原始观测平滑，这就是卡尔曼滤波的效果。

参数 Q 和 R 的意义：

• Q 是过程噪声协方差，表示你有多信任运动模型。Q 越大，滤波越跟随观测。
• R 是观测噪声协方差，表示你有多信任传感器。R 越大，滤波越平滑但响应越慢。

3.3 扩展卡尔曼滤波（EKF）

真实 SLAM 系统中，运动模型和观测模型都是非线性的。比如车辆的运动模型包含角度更新θ' = θ + ω*dt，这里 cos/sin 就是非线性项。直接把卡尔曼滤波套用会失效，因此需要对非线性函数做一阶泰勒展开，也就是求雅可比矩阵，这就是扩展卡尔曼滤波（EKF）。

EKF 的步骤和 KF 基本一致，只是状态转移和观测方程变成了非线性函数，并在当前估计点做线性化：

x_pred = f(x, u) P_pred = F * P * F^T + Q

其中 F 是 f 对 x 的雅可比矩阵。在 SLAM 中，EKF 通常维护一个包含位置、姿态、速度、陀螺零偏等状态的大向量，计算量随路标点数量增长呈平方级，因此在小场景 SLAM 中适用，但在大规模环境中会力不从心。

3.4 粒子滤波

粒子滤波不假设系统是高斯分布，而是用一组带权重的随机粒子来近似后验概率分布。它特别适合非高斯、非线性系统，也适合全局定位问题。

粒子滤波的核心步骤：

1. 初始化：在地图范围内随机撒粒子。
2. 预测：每个粒子按运动模型移动。
3. 权重更新：根据激光、视觉等观测计算每个粒子的权重。
4. 重采样：去掉低权重粒子，复制高权重粒子，维持粒子数量。

2D SLAM 中，经典的Gmapping算法就是基于粒子滤波实现的，适合小场景室内建图。不过在大规模环境中，粒子滤波需要的粒子数会指数增长，容易退化和耗尽，这也是它后来被图优化方法取代的原因之一。

3.5 滑动窗口滤波

滑动窗口滤波在 SLAM 中的含义是：只维护最近 N 帧位姿和观测，丢掉更早的信息。

这本质上是一种“有界记忆”的优化策略，常用于视觉惯性里程计（VIO）中。它比 EKF 更接近非线性优化，比全局 BA（光束平差法）计算量更小，专门用于实时性要求高的场景。

滑动窗口滤波器有两大关键操作：

• 边缘化（Marginalization）：当某个关键帧被移出窗口时，把它包含的信息通过 Schur 补变成先验约束保留下来，而不是直接丢弃。
• 信息矩阵维护：窗口内的状态量构成信息矩阵，增量更新时只修改局部子块，提升计算效率。

这里的“滑动窗口”和你可能查到的信号处理中的“滑动窗口滤波”不同。前者是状态估计的窗口化优化策略，后者是对一段连续信号取均值/中值的滤波算法。两者名字相似，但完全不是一件事。

4. 图优化与因子图：主流 SLAM 的后端核心

4.1 为什么从滤波转向图优化

EKF-SLAM 在路标数量多时计算量是平方级增长，且线性化只做一次，容易累积线性化误差。粒子滤波则面临粒子耗尽问题。

图优化把 SLAM 后端建模成一个图的最小二乘问题：

• 图的节点：相机/机器人位姿、路标点。
• 图的边：两个节点之间由运动模型或观测模型产生的约束。

后端优化的目标，就是调整所有节点的位姿和路标位置，使所有边的残差平方和最小。图优化的优势在于：

1. 可以反复线性化，迭代求解，精度更高。
2. 利用稀疏性，可以处理大规模场景。
3. 容易引入回环检测边的约束，消除累积漂移。

4.2 图优化的数学形式

假设一共有 m 个约束边，每个约束边的残差为 e_ij，图优化的目标就是：

min Σ e_ij^T Ω_ij e_ij

其中 Ω_ij 是信息矩阵（即协方差矩阵的逆），表示这条边的置信度。信息矩阵越大，这条约束在优化中被信任的程度越高。

求解方式通常是用高斯牛顿法（Gauss-Newton）或列文伯格-马夸尔特法（Levenberg-Marquardt）。每次迭代会构建一个线性方程：

H * Δx = -b

其中 H 是 Hessian 矩阵的近似。得益于 SLAM 图的稀疏性，H 矩阵可以被分解成稀疏结构，使用 Schur 消元等技术加速求解。

4.3 因子图与滑动窗口的关系

因子图（Factor Graph）是图优化的一种特殊表示，用二分图建模：

• 圆圈代表变量节点（位姿、路标）。
• 方块代表因子节点（约束）。

因子图比普通位姿图表达更灵活，可以自然地表达 IMU 预积分因子、回环因子、先验因子等。GTSAM 库就是以因子图为核心开发的。

在视觉惯性导航系统中，因子图配合滑动窗口使用：窗口内的位姿作为变量节点，IMU 预积分、视觉重投影、回环检测形成因子节点。当新帧加入时，窗口最老的帧被边缘化，它的信息变成先验因子加入优化。

这就是现代 SLAM 后端的经典范式：滑窗 + 因子图 + 边缘化。

4.4 常用图优化工具

这里不深入讲每个库的 API，实际工程中建议从LIO-SAM / FAST-LIO / ORB-SLAM3这几个开源框架入手，阅读它们的后端实现，比单纯看理论更快建立工程直觉。

5. 激光视觉融合：高精度定位建图的关键路径

5.1 为什么要融合激光与视觉

激光雷达直接获取高精度三维点云，但点云稀疏、缺颜色和语义信息；相机能获取稠密纹理，但对光照和运动模糊敏感。两者融合的互补性非常明显：

• 激光提供深度与几何结构，为视觉特征提供尺度。
• 视觉提供纹理与语义，帮助激光点云做识别和动态物体剔除。
• 视觉特征可以形成重投影约束，校正激光里程计的漂移。

融合定位之后，建图也能获得更好的效果，例如三维语义地图、动态物体滤除后的高精点云地图等。

5.2 松耦合与紧耦合

激光视觉融合按耦合程度可分为：

紧耦合的精度通常更高，但计算量也更大。工程上如果算力受限，松耦合也能满足部分场景需求。

5.3 时间同步与数据预处理

在写融合代码之前，必须先解决两个工程问题：时间同步和空间同步。

• 时间同步：激光雷达、相机、IMU 各自的帧率不同，触发时间也不同。需要找到每个相机帧对应的最近一帧点云，通常通过时间戳差值查找。
• 空间同步：所有传感器的数据都要变换到同一个坐标系，一般以车体坐标系或 IMU 坐标系为参考。

另外，点云在车辆运动过程中会畸变——激光雷达扫描一圈需要时间，车辆已经向前移动了一段距离。因此需要用 IMU 或里程计数据对点云做运动畸变校正，也就是去畸变处理。

5.4 多帧点云积累与 SLAM 效果可视化

标题中提到的“ARS548 多帧积累来做 SLAM 的效果可视化”是一个很实用的调试技巧。ARS548 是 4D 毫米波雷达，输出的是目标级或点云级数据，单帧点云往往比较稀疏。为了直观看到 SLAM 建图效果，通常会把连续多帧点云按里程计位姿变换到世界坐标系下叠加起来。

下面是一个简化思路的 Python 示例，演示如何用位姿将多帧点云转换到世界坐标系并绘制：

import numpy as np import matplotlib.pyplot as plt def transform_points(points, T): """将 Nx3 点云按齐次变换矩阵 T (4x4) 变换""" ones = np.ones((points.shape[0], 1)) points_h = np.hstack([points, ones]) transformed_h = (T @ points_h.T).T return transformed_h[:, :3] # 模拟三帧点云 np.random.seed(0) cloud_frames = [] for i in range(3): cloud = np.random.rand(100, 3) * 5 cloud_frames.append(cloud) # 模拟位姿：每帧平移 (1, 0, 0) poses = [np.eye(4)] for i in range(1, 3): T = np.eye(4) T[0, 3] = i * 1.0 poses.append(T) # 多帧积累 world_points = [] for i, cloud in enumerate(cloud_frames): cloud_world = transform_points(cloud, poses[i]) world_points.append(cloud_world) world_points = np.vstack(world_points) # 可视化 fig = plt.figure(figsize=(8, 6)) ax = fig.add_subplot(111, projection='3d') ax.scatter(world_points[:, 0], world_points[:, 1], world_points[:, 2], s=0.5, c='r', label='accumulated cloud') ax.set_title('Multi-frame Point Cloud Accumulation') ax.set_xlabel('X') ax.set_ylabel('Y') ax.set_zlabel('Z') plt.legend() plt.savefig('accumulation_demo.png', dpi=120)

运行结果可以看到三帧点云在 X 方向上平移叠加，形成更大范围的点云地图。这只是可视化思路，实际 SLAM 中的位姿来自前端里程计估计，如果你使用 ARS548 雷达，还需要处理目标航迹与点云的匹配关系。

5.5 信号滤波在传感器预处理中的角色

虽然这里讲的是 SLAM，但点云去噪、IMU 信号平滑、图像降噪同样离不开“滤波”这个基本功。

• 中值滤波：常用于去除图像椒盐噪声或点云离群点。
• 高斯滤波：常用图像平滑，减少边缘检测时的噪声响应。
• 低通滤波（LPF）：对 IMU 加速度和角速度信号做平滑，消除高频振动干扰。
• 高通滤波（HPF）：提取车辆加速/减速的趋势，用于运动模型。

在工程调试中，如果发现 SLAM 轨迹抖动明显，首先要检查的就是传感器预处理链路：点云有没有去畸变？IMU 有没有低通滤波？图像有没有抑制运动模糊？这些问题不解决，后端的图优化再厉害也救不回来。

6. 实战：一个简单的激光视觉融合定位 Demo

这一节我们做一个简化但完整可运行的融合定位 Demo。核心思路：分别用激光点云计算帧间位姿（激光里程计），用视觉特征计算另一个位姿（视觉里程计），最后在因子图后端里融合两个位姿约束。

为了便于演示，我们把范围缩小为：读取模拟数据 → 构造激光约束 → 构造视觉约束 → 联合优化 → 输出位姿轨迹。

6.1 环境准备与项目结构

示例语言使用 Python 3，核心依赖为：

• numpy
• scipy
• g2o 的 Python 绑定（或使用 gtsam 的 Python 接口）

如果你不想安装 g2o 绑定，也可以先跳过优化部分，直接用加权平均模拟融合结果。这里为了展示图优化，我以 g2o 文件格式作为演示。

建议项目结构：

slam_fusion_demo/ ├── data/ │ ├── lidar_poses.npy │ ├── camera_poses.npy │ └── landmarks.npy ├── fuse.py ├── optimize.g2o └── README.md

6.2 生成模拟数据

import numpy as np np.random.seed(42) # 模拟 50 帧位姿 N = 50 true_poses = [] for i in range(N): T = np.eye(4) T[0, 3] = i * 0.5 T[1, 3] = 0.5 * np.sin(i * 0.1) true_poses.append(T) # 激光里程计：真实位姿 + 小噪声 lidar_poses = [] for T in true_poses: T_noisy = T.copy() T_noisy[:3, 3] += np.random.normal(0, 0.02, 3) lidar_poses.append(T_noisy) # 视觉里程计：真实位姿 + 较大噪声 camera_poses = [] for T in true_poses: T_noisy = T.copy() T_noisy[:3, 3] += np.random.normal(0, 0.08, 3) camera_poses.append(T_noisy) np.save('data/lidar_poses.npy', lidar_poses) np.save('data/camera_poses.npy', camera_poses)

6.3 构造 g2o 文件并优化

g2o 文件是一种文本格式的图优化描述文件。每一行：

VERTEX_SE3: id x y z qx qy qz qw EDGE_SE3: id1 id2 dx dy dz dqx dqy dqz dqw I11 I12 I13 ... I66

下面我们把激光和视觉的帧间约束写成 g2o 边：

import numpy as np from scipy.spatial.transform import Rotation as R def to_g2o_quat(T): rot = R.from_matrix(T[:3, :3]) qx, qy, qz, qw = rot.as_quat() return qx, qy, qz, qw def write_pose_graph(frames, edges, filename): with open(filename, 'w') as f: for i, T in enumerate(frames): qx, qy, qz, qw = to_g2o_quat(T) f.write(f"VERTEX_SE3:QUAT {i} {T[0,3]:.6f} {T[1,3]:.6f} {T[2,3]:.6f} " f"{qx:.6f} {qy:.6f} {qz:.6f} {qw:.6f}\n") for (i, j, T_rel, info_val) in edges: dx = T_rel[0, 3] dy = T_rel[1, 3] dz = T_rel[2, 3] qx, qy, qz, qw = to_g2o_quat(T_rel) f.write(f"EDGE_SE3:QUAT {i} {j} " f"{dx:.6f} {dy:.6f} {dz:.6f} " f"{qx:.6f} {qy:.6f} {qz:.6f} {qw:.6f} ") for k in range(6): for l in range(6): f.write(f"{info_val if k == l else 0:.2f} ") f.write("\n")

边信息矩阵的值（上面代码里的info_val）对应约束权重。激光约束的信息矩阵可以设大一些（例如 100.0），视觉约束设小一些（例如 10.0），代表我们更信任激光里程计。

6.4 运行优化

实际运行 g2o 优化时，你可以直接用命令行工具，或在 Python 中使用g2o库：

import g2o solver = g2o.SolverOptimizableGraph(g2o.LinearSolverEigenDense) optimizer = g2o.SparseOptimizer() optimizer.set_solver(solver) optimizer.load('optimize.g2o') optimizer.initialize_optimization() optimizer.set_verbose(True) optimizer.optimize(30) optimizer.save('optimized.g2o')

优化后，你可以从optimized.g2o中读取优化后的位姿，绘制轨迹与真值、激光里程计、视觉里程计对比，通常会看到图优化融合后的轨迹比单一传感器更接近真值。

6.5 结果说明

这个 Demo 的核心并不是教你写一个产品级的 SLAM 系统，而是帮你建立“激光约束 + 视觉约束 + 后端优化”的最小闭环。真实工程中，你会用 ICP（迭代最近点）或 NDT（正态分布变换）计算激光帧间位姿，用特征匹配 + PnP 计算视觉位姿，再用 IMU 预积分提供帧间约束，最后在因子图后端统一优化。

7. 常见问题与排查思路

做 SLAM 开发时，很多问题看起来玄学，其实都有规律可循。

这里特别强调一下外参和时间同步问题。在很多项目中，你会发现代码逻辑完全正确，但融合结果就是不对，最后排查下来 70% 是传感器时间戳没对齐，20% 是外参标定结果有偏差，只有 10% 是算法本身的问题。

8. SLAM 工程落地的最佳实践

8.1 传感器选型与配置

• 室外大场景优先选择 3D 激光雷达（机械式/固态式），配置时确认线数、视场角和帧率。
• 视觉传感器注意帧率、曝光方式和畸变类型。
• IMU 要关注加速度计和陀螺仪的量程与零偏稳定性。

8.2 标定与校核流程

在工程团队中，建议把传感器标定做成例行流程，而不是只做一次。具体做法：

1. 出厂/装机后执行完整内外参标定。
2. 每次车辆维修、碰撞后复检外参。
3. 使用自动标定工具定期校核，避免人工标定误差。

8.3 时间同步与数据链路

时间同步是融合定位中的隐形杀手。工程上常见方案：

• 使用 PTP（精确时间协议）或 NTP 统一设备时间。
• 在数据采集端记录各传感器时间戳，处理时按时间戳查找邻域对应帧。
• 使用硬件同步信号触发，如激光雷达通过 PPS 信号同步相机曝光。

8.4 退化场景处理

激光雷达在长直走廊、隧道、空旷广场等几何退化场景中，点云的约束会退化，导致位姿估计漂移。对策有：

• 融合视觉特征，在几何退化时保留视觉约束。
• 融合 IMU，用 IMU 短时积分撑过退化区域。
• 增加轮速计约束。
• 提前在场景中布置反光柱等人工特征。

8.5 可视化与调试

好的可视化能极大提高排查效率。推荐方案：

• Rviz2 / Foxglove：ROS 生态常用可视化工具。
• EVO：用于评估 SLAM 轨迹精度，支持绘制 ATE/RPE 曲线。
• PlotJuggler：快速查看 IMU、里程计的时序数据。

如果你在做 ARS548 等多帧积累可视化，建议把单帧点云、累计点云、估计轨迹一起显示，便于观察建图效果。

9. 总结与后续学习路线

通过对坐标系、滤波、图优化、激光视觉融合这几个模块的拆解，你应该已经建立了 SLAM 的完整技术地图：

• 先通过坐标变换把不同传感器统一到同一空间中。
• 用滤波或非线性优化持续估计最优位姿。
• 在后端用图优化或因子图统一处理多源约束。
• 用视觉纹理和激光几何互补，提升定位建图的精度与鲁棒性。

下一步进阶学习，建议按以下顺序投入：

1. 精读《视觉 SLAM 十四讲》：把李群李代数、非线性优化基础补扎实。
2. 通读一个开源激光 SLAM 框架（如 A-LOAM、LIO-SAM、FAST-LIO），掌握激光里程计实现。
3. 通读一个视觉惯性 SLAM 框架（如 VINS-Mono、ORB-SLAM3），重点理解滑窗与边缘化。
4. 动手做一个多传感器融合 Demo，把外参标定、点云去畸变、位姿估计、图优化串成完整链路。
5. 阅读因子图与增量优化的论文（如 iSAM2），从理论上理解为什么现代 SLAM 选择图优化而不是滤波。

如果说有什么忠告，那就是：不要只调包，一定要亲手把坐标变换和残差求导写一遍，这个过程能帮你避掉一半的工程坑。如果这篇文章对你搭建 SLAM 知识体系有帮助，可以收藏备用；也欢迎在实际项目中遇到问题时回来对照排查思路，逐步形成自己的一套 SLAM 调试方法。