源码级解读cuVSLAM ：CUDA加速的光流跟踪+紧耦合VIO,凭什么成为Jetson上的SLAM首选？

一、聊聊机器狗导航

1.1 机器狗导航和 AMR/AGV 导航的区别

轮式 AGV/AMR 在平整地面上跑，最常见方案是 2D 激光雷达 + 占据栅格地图（GMapping/Cartographer/Nav2），跑起来稳定又便宜。

机器狗的难点在于：

会上下楼梯、跨越障碍 → 2D 激光雷达扫不到地面和天花板，2D 栅格地图直接失效

姿态变化剧烈 → 走楼梯时 pitch 能到 ±30°，IMU 和相机的"水平"参考都在变

运动颠簸 → 图像容易糊，特征跟踪容易丢

算力受限 → 板载 Jetson Orin 级别，不可能跑桌面级 SLAM

所以机器狗导航需要的是 3D SLAM + 鲁棒的多模态融合 + 嵌入式友好。

1.2 业界主流方案速览

机器狗的典型组合是：

腿足里程计（leg odometry）+ IMU + 3D 激光 SLAM（LIO-SAM 系）—— 稳，但激光雷达贵

腿足里程计 + IMU + 视觉 SLAM（VINS / cuVSLAM）—— 便宜，但视觉在弱纹理、强光变化时容易丢

全部融合（多激光 + 多相机 + IMU）—— 大厂方案

1.3 cuVSLAM 在这个生态里的定位

cuVSLAM 是 NVIDIA 开源的视觉 SLAM 库，它的"卖点"对应了上面这些痛点的几个：

1. CUDA 加速：Bundle Adjustment、光流这些计算密集步骤放 GPU，Jetson Orin 能实时跑

2. 多相机 + IMU 一体化：原生支持 1~32 个相机 + 0~1 个 IMU 的任意混搭，省去自己写融合

3. 零调参理念：官方宣称"开箱即用"，所有内部参数自适应

4. 和 NVIDIA Isaac ROS 生态无缝集成：Jetson 上仅需一条命令装好

对机器狗来说，cuVSLAM 提供的是"视觉惯性里程计 + 局部地图 + 回环检测"这一层。完整的导航栈还需要：

局部避障（Nvblox、Costmap 2D）

全局路径规划（Nav2、RRT）

腿足控制（WBC、MPC）

上下楼梯逻辑（自己写或厂商提供）

cuVSLAM 解决的是**"我在哪"这个问题，不解决"我去哪、怎么走"**。

二、视觉 SLAM 到底在干什么

2.1 SLAM 问题的本质

SLAM = Simultaneous Localization And Mapping（同时定位与建图）。

机器人面对陌生环境时，两个问题互为依赖：

• 定位：我在哪？→ 需要地图作为参考

• 建图：地图长什么样？→ 需要知道自己的位置才能把观测拼起来

SLAM 算法就是鸡生蛋蛋生鸡地同时解决这两个问题。

输出通常是两个东西：

1. 轨迹：每个时刻机器人在世界坐标系下的位姿（位置 + 朝向）

2. 地图：环境特征点（路标）在世界坐标系下的 3D 位置

2.2 视觉 SLAM 的标准流水线

几乎所有现代视觉 SLAM 系统都遵循这个流水线（参考高翔《视觉 SLAM 十四讲》）：

相机图像   │   ▼┌─────────────────┐│ 1. 传感器数据读取  │  ← 同步时间戳、去畸变└─────────────────┘   │   ▼┌─────────────────┐│ 2. 前端: 视觉里程计│  ← 帧间位姿估计 (VO)│   (Front-end)    │└─────────────────┘   │   ▼┌─────────────────┐│ 3. 后端: 优化     │  ← Bundle Adjustment / 位姿图优化│   (Back-end)     │└─────────────────┘   │   ▼┌─────────────────┐│ 4. 回环检测       │  ← 发现"我来过这里"└─────────────────┘   │   ▼┌─────────────────┐│ 5. 建图          │  ← 路标点云 / 占据栅格 / TSDF└─────────────────┘

前端：实时性强，每帧都要算。回答"相对于上一帧我移动了多少"

后端：累计误差靠它修正。慢一点没关系，但要做到全局一致

回环检测：发现"我又回到了原点"，给后端一个"拉回"的约束

cuVSLAM 完全遵循这个流水线，接下来几章我们就按"前端 → 后端 → 回环 → 多模态融合"的顺序逐层展开 cuVSLAM 的具体方案。

三、cuVSLAM 的整体方案：

3.1 特征点法 + CUDA 加速

视觉 SLAM 前端有两条主流路线：

特征点法：检测角点 → 跨帧匹配 → 几何求解算位姿。代表：ORB-SLAM、cuVSLAM

直接法：不提取特征，直接最小化像素灰度误差。代表：DSO、LSD-SLAM

cuVSLAM 选特征点法，五步流水线全部 CUDA 化：

特征检测：在图像里找几百个显著角点

特征跟踪：用稀疏光流（SOF）在下一帧找到这些点对应到哪了

位姿求解：用 PnP 算相机位姿

三角化：得到 3D 路标点

Bundle Adjustment：联合优化所有位姿和路标，消除累积误差

几个名词一句话解释（不用深究）：

• 稀疏光流（SOF）：跟踪图像里几百个特征点在帧间的移动，假设"同一个点在两帧里亮度不变"。用图像金字塔应对大运动。cuVSLAM 在 libs/sof/ 实现，GPU 加速。

• PnP：已知 3D 路标和它们在图像里的 2D 投影，反推相机位姿。cuVSLAM 用 EPnP + RANSAC 剔除外点。

• Bundle Adjustment（BA）：调整所有相机位姿和路标点，让"路标投影回图像"和"实际观测"的误差最小。本质是非线性最小二乘，cuVSLAM 用自家的 cuNLS（CUDA 非线性最小二乘库）加速 5~10 倍。

• 异步 BA：默认 async_sba=True，BA 在后台线程跑不阻塞前端，前端能保持 30 FPS。

3.2 加入 IMU 形成 VIO

纯视觉在快速运动、弱纹理、运动模糊时会丢。加 IMU 形成 VIO（Visual-Inertial Odometry）：

IMU 高频（200~1000 Hz vs 相机 30 Hz），填补帧间空白

不受光照影响，视觉丢失时撑一会

提供绝对尺度（单目 VO 算不出尺度）

估计重力方向

cuVSLAM 用 IMU 预积分 把两帧之间的 IMU 测量预先积分成一个"汇总观测"，避免后端优化时反复重算积分——这是 VINS 系算法的工程基石，具体数学不展开。

IMU 预积分为什么这样设计

最朴素的思路是把 IMU 加速度二次积分得到两帧间的位姿变化。但问题在于：后端优化会调整相机位姿，每次调整后所有中间积分都要重算，计算量爆炸。

把两帧之间的 IMU 测量**预先积分成一个"汇总观测"**，这个汇总观测只依赖于两帧的位姿和 bias，与绝对参考系无关。这样后端优化时无论位姿怎么变，预积分量都不用重算（只需用 bias 的一阶近似修正）。cuVSLAM 的 libs/imu/imu_preintegration.h 实现了预积分 + bias 在线估计 + 重力向量优化。

调用时序：IMU 和图像交替喂入

VIO 有个硬约束：

Track() 和 RegisterImuMeasurement() 必须按时间戳非递减顺序从同一线程调用，不允许并发。

EuRoC 数据集图像 20 Hz、IMU 200 Hz，每两帧图像之间约 10 个 IMU 样本要穿插喂入。时序图：

图像 t0  IMU t0+5ms  IMU t0+10ms  ...  IMU t0+45ms  图像 t1  │         │           │                  │           │  ▼         ▼           ▼                  ▼           ▼ Track    RegImu      RegImu             RegImu      Track

cuVSLAM 内部收到 IMU 时做预积分累积，收到图像时用预积分结果作为位姿先验，再叠加视觉约束做联合优化。

3.3 SLAM 层：回环检测 + 地图持久化

Odometry 类只做"前端 + 局部后端"，会有累积漂移。真正消除漂移要靠 Slam 类：

3.3.1 回环检测：发现"我来过这里"

走 1 公里回到原点，SLAM 应该认出"我来过这里"，然后把累积漂移拉回零。

回环检测的核心是"图像相似度判断"

• 早期方案：词袋模型（Bag of Words, DBoW2）—— 把特征描述子聚类成"视觉单词"，图像表示成单词直方图，比较直方图相似度

深度学习方案：NetVLAD、AP-GeM 等学到的全局描述子

cuVSLAM 内部用的是基于特征匹配的几何验证（具体实现没完全开源，但论文里有）

发现回环后做什么

在位姿图（Pose Graph）里加一条"回环边"，约束两个关键帧位姿的相对变换

1. 触发 **Pose Graph Optimization (PGO)**：优化所有关键帧位姿让所有约束（相邻边 + 回环边）误差最小

路标点根据相机位姿变化相应调整

位姿图优化的规模比 BA 小得多（只优化位姿不优化路标），适合在回环时跑一次。

cuVSLAM 的 libs/slam 类有 GetLoopClosurePoses() 接口，能拿到最近 10 次回环位姿——调试时很有用，能看到回环触发的频率和位置。

3.3.2 地图持久化：LMDB 后端

Slam::Config::map_cache_path 非空时，cuVSLAM 会用 LMDB（Lightning Memory-Mapped Database）做地图持久化。LMDB 是个嵌入式键值存储，特点是：

内存映射文件，读写极快

支持多进程并发读

不需要独立服务进程

这意味着：

大规模地图可以放磁盘，不挤占内存

关机重启后能加载之前的地图继续跑

多个机器人可以共享同一张地图（只读）

3.3.3 重定位：在已知地图里找自己

机器狗关机再开机要能定位。cuVSLAM 的 LocalizeInMap 接口：

tracker.localize_in_map(    folder_name="/tmp/my_map",     # 已保存的地图路径    timestamp_ns=ts_ns,             # 当前时间戳    guess_pose=initial_guess,       # 初始位姿猜测（粗略即可）    images=current_images,          # 当前观测    settings=localization_settings, # 搜索半径、步长    start_cb=lambda: print("locating..."),    finish_cb=lambda pose, err: print(f"found: {pose}"if pose else f"failed: {err}"))

内部做的事：

在地图里以 guess_pose 为中心，按 horizontal_search_radius、vertical_search_radius、angular_step_rads 搜索

每个候选位姿做一次 PnP 匹配

找到内点足够多的位姿就认为定位成功

把当前地图替换为已保存的地图，继续 SLAM

max_map_size = 300 是 cuVSLAM 的默认位姿图大小——官方注释说"300 适合实时建图"。把位姿图限定成滑动窗口，PGO 不会随时间爆炸，但代价是回环检测的范围也受限。

四、cuVSLAM 的五种 Odometry 模式汇总

把前面散落的信息整合一下，cuVSLAM 的 Odometry 类支持五种工作模式：

选型决策树：

你的 rig 是什么？├── 单 RGB 相机│   └── → Mono（注意尺度不确定）├── 单 RGB-D 相机（RealSense / Kinect / Azure Kinect）│   └── → RGBD├── 立体对（已校正）│   └── → Multicamera + rectified_stereo_camera=True├── 立体对 + IMU│   └── → Inertial├── 多立体 rig（≥3 相机，纯视觉）│   └── → Multicamera├── 混合 rig（部分 RGB + 部分 RGB-D，可能加 IMU）│   └── → Multisensor└── 不确定    └── 先 Mono 验证标定，再升级模式

五、上手：从零跑通第一个示例

5.1 环境准备

cuVSLAM 是 CUDA 库，所以第一步是确认 CUDA 环境。它支持 CUDA 12 和 13，Jetson Orin 默认带的 CUDA 版本就能用，x86 + NVIDIA 显卡也行。

# 必备：CUDA Toolkit 12 或 13nvcc --version

NVIDIA 官方没有把 cuVSLAM 发到 PyPI，要去 GitHub Releases 下 wheel。一定要选匹配自己 CUDA 版本、Python 版本和 CPU 架构的 wheel——比如 Jetson 是 aarch64，桌面显卡是 x86_64，选错装不上。

# 推荐 venv，避免污染系统 Pythonpython3 -m venv .venvsource .venv/bin/activate# 装下载好的 wheelpip install cuvslam-*-cu12-*.whl# 可视化依赖：rerun 是官方示例用的可视化工具，numpy 是必须的pip install rerun-sdk numpy pillow

装完后用下面这条最小命令验证：

python -c "import cuvslam; print(cuvslam.__version__)"

如果输出没报错、有版本号，说明装好了。常见的报错原因：CUDA 版本不匹配、wheel 架构选错、系统缺 libcuda.so（Jetson 上偶尔出现，装 nvidia-jetpack 能修复）。

5.2 最小单目 VO：验证安装是否成功

上手第一件事不是搞立体相机 + IMU + ROS2 那一整套，而是用最简单的单目 VO 验证 cuVSLAM 能跑通。这个示例用随机噪声当图像，不需要任何真实数据，专门用来验证安装。

import cuvslamimport numpy as np# 1. 配置单目相机#    内参用 VGA 分辨率的典型值，principal 是图像中心点，focal 是焦距像素camera = cuvslam.Camera()camera.size = (640, 480)camera.principal = [320, 240]   # cx, cy（图像中心）camera.focal = [400, 400]       # fx, fy（像素单位）camera.distortion = cuvslam.Distortion()  # 默认 Pinhole 无畸变# 2. 配置 Mono 模式的 trackercfg = cuvslam.Tracker.OdometryConfig(    odometry_mode=cuvslam.Tracker.OdometryMode.Mono,)tracker = cuvslam.Tracker(cuvslam.Rig([camera]), cfg)# 3. 喂 100 帧假图像，看 tracker 是否能正常返回位姿for i in range(100):    img = np.random.randint(0, 255, (480, 640), dtype=np.uint8)  # 随机噪声    ts_ns = int(i * 1e9 / 30)  # 30 FPS 的时间戳，单位纳秒    pose_est, _ = tracker.track(ts_ns, [img])if pose_est.world_from_rig is None:print(f"Frame {i}: lost")else:        t = pose_est.world_from_rig.pose.translationprint(f"Frame {i}: pos = [{t[0]:.2f}, {t[1]:.2f}, {t[2]:.2f}]")

预期输出：因为喂的是随机噪声图像，cuVSLAM 找不到任何特征点来跟踪，****几乎每一帧都会输出 lost****。这其实是正确的行为——说明 cuVSLAM 没有崩溃、API 调用流程没问题、安装是成功的。如果你看到的是 Segmentation fault 或 ImportError，那是装的有问题；如果看到 Frame 0: lost、Frame 1: lost ... 一路 lost 下去，恭喜，安装验证通过。

关于单目模式的尺度问题：单目 VO 算出的 translation 是相对尺度——你不知道平移是 1 米还是 10 米，因为 3D 路标本身的尺度也是估出来的。这是单目 SLAM 的固有局限，不是 cuVSLAM 的 bug。要绝对尺度得加 IMU、depth 或立体对（见后面几节）。

5.3 立体 VO：最常用的纯视觉方案

立体 VO 用两个有重叠视场的相机，通过 baseline（基线距离）直接三角化出真实尺度的 3D 点，是纯视觉里最精确的方案。机器狗上常见的 RealSense D435i 的左右红外对就是一组天然立体对。

import cuvslam# 1. 左相机：和单目配置一样left = cuvslam.Camera()left.size = (640, 480)left.principal = [320, 240]left.focal = [400, 400]# 2. 右相机：内参与左相机相同，但 rig_from_camera 多了一个 baseline 平移right = cuvslam.Camera()right.size = (640, 480)right.principal = [320, 240]right.focal = [400, 400]right.rig_from_camera.translation = [-0.12, 0, 0]  # baseline 12cm，沿 X 轴# 3. 配置 Multicamera 模式cfg = cuvslam.Tracker.OdometryConfig(    odometry_mode=cuvslam.Tracker.OdometryMode.Multicamera,    rectified_stereo_camera=True,   # 已校正立体走快速路径    async_sba=True,                 # 异步 BA 提速)tracker = cuvslam.Tracker(cuvslam.Rig([left, right]), cfg)# 4. 跟踪循环for i in range(num_frames):    ts_ns, left_img, right_img = get_stereo_pair(i)    pose_est, _ = tracker.track(ts_ns, [left_img, right_img])if pose_est.world_from_rig is not None:        t = pose_est.world_from_rig.pose.translationprint(f"Frame {i}: pos = [{t[0]:.2f}, {t[1]:.2f}, {t[2]:.2f}]")

几个关键配置讲解：

rig_from_camera.translation = [-0.12, 0, 0]：右相机相对左相机沿 X 轴平移 12cm。取负号是因为 cuVSLAM 约定 rig_from_camera 表示"从相机坐标系到 rig 坐标系"的变换——右相机原点在 rig 原点（通常取左相机）的负 X 方向。baseline 越大，深度估计越准，但重叠视场越小。

rectified_stereo_camera=True：如果你的相机已经做了立体校正（左右极线水平对齐，RealSense 出厂就校正过），开这个开关走快速路径——省掉极线搜索，三角化直接 （ 是视差），能省约 30% 前端计算量。前提是相机真的校正过，没校正开这个开关会跑飞。

async_sba=True：BA 在后台线程跑，不阻塞前端跟踪。实时场景一定要开。

输出怎么用：pose_est.world_from_rig.pose.translation 就是 rig（机器人）在世界坐标系下的位置，单位是米。世界坐标系原点是第一帧启动时的位置，朝向也是第一帧的朝向。这是真实尺度的，不像单目那样是相对的。

5.4 立体惯性 VIO：机器狗场景的标配

立体 VO 在快速运动、运动模糊、弱纹理时会丢。加 IMU 形成 VIO，让 IMU 在视觉丢失时撑一会。这是机器狗场景的标配方案。

# 1. IMU 标定参数（相机部分同 4.3，省略）imu_calib = cuvslam.ImuCalibration()imu_calib.rig_from_imu = cuvslam.Pose()  # 假设 IMU 和 rig 原点重合（实际要标定！）# 噪声参数：用 Kalibr 标定，没标过用 IMU 数据手册典型值imu_calib.gyroscope_noise_density = 0.00016       # rad/(s*sqrt(Hz)) 陀螺仪白噪声imu_calib.gyroscope_random_walk = 2.2e-06         # rad/(s^2*sqrt(Hz)) 陀螺仪零偏游走imu_calib.accelerometer_noise_density = 0.0028    # m/(s^2*sqrt(Hz)) 加速度计白噪声imu_calib.accelerometer_random_walk = 8.6e-05     # m/(s^3*sqrt(Hz)) 加速度计零偏游走imu_calib.frequency = 200.0  # IMU 采样频率，Hz# 2. 把 IMU 挂到 rig 上rig = cuvslam.Rig([left, right])rig.imus = [imu_calib]# 3. 配置 Inertial 模式cfg = cuvslam.Tracker.OdometryConfig(    odometry_mode=cuvslam.Tracker.OdometryMode.Inertial,    async_sba=True,)tracker = cuvslam.Tracker(rig, cfg)# 4. 跟踪循环：IMU 和图像交替喂入（关键！）while True:# 4a. 有 IMU 样本就先喂 IMUif has_imu_sample():        imu_data = get_imu_sample()        m = cuvslam.ImuMeasurement()        m.timestamp_ns = imu_data.t        m.linear_accelerations = imu_data.accel   # [ax, ay, az]，单位 m/s^2        m.angular_velocities = imu_data.gyro      # [wx, wy, wz]，单位 rad/s        tracker.register_imu_measurement(0, m)    # 0 是 IMU 的 sensor_index# 4b. 有图像对就喂图像if has_image_pair():        ts, left_img, right_img = get_stereo_pair()        pose_est, _ = tracker.track(ts, [left_img, right_img])

关键点讲解：

rig_from_imu 必须标定：示例里用 cuvslam.Pose()（单位变换）只是偷懒，实际部署必须标定。IMU 和相机的相对位姿如果错了，预积分给的先验就是错的，整个 VIO 会跑飞。用 Kalibr 标定是标准做法。

噪声参数从哪来：

noise_density 是白噪声密度，单位带 ，是 IMU 数据手册会给出的指标

random_walk 是零偏游走噪声，描述 IMU 零偏随时间缓慢变化的程度

这两个参数直接影响后端优化时 IMU 约束的权重，给错了会导致系统过信或欠信 IMU

IMU 和图像必须交替喂入：这是 VIO 的硬约束。Track() 和 RegisterImuMeasurement() 必须按时间戳非递减顺序从同一线程调用，不能并发。EuRoC 数据集图像 20 Hz、IMU 200 Hz，每两帧图像之间约 10 个 IMU 样本要穿插喂入。

怎么验证 VIO 跑对了：

gravity = tracker.get_last_gravity()# 应该返回 [0, +9.81, 0]（cuVSLAM 用 OpenCV 约定 +Y 朝下）

如果重力向量不朝下，说明 rig_from_imu 外参标错了，这是最直接的排查指标。

5.5 启用 SLAM：地图保存与重定位

前面三种模式都是"里程计"——只做位姿估计，不保存地图，关机后所有数据丢失。要实现"建图一次，多次复用"，得启用 Slam 类。

# 1. 配置 SLAM（在 OdometryConfig 基础上加 SlamConfig）slam_cfg = cuvslam.Tracker.SlamConfig(    map_cache_path="/tmp/cuvslam_map",  # LMDB 持久化路径    max_map_size=300,                    # 位姿图最大节点数    planar_constraints=False,            # 平面约束)tracker = cuvslam.Tracker(rig, cfg, slam_cfg)

三个配置字段讲解：

map_cache_path：非空时启用 LMDB 地图持久化。机器狗跑一圈建好地图后，地图会存到这个目录，重启能加载继续跑。如果为空，地图只在内存里，关机即丢。

max_map_size：位姿图最大节点数。默认 300 是个稳妥的工程取舍——把位姿图限定成滑动窗口，PGO 不会随时间爆炸，但回环检测的范围也受限（更早的回环发现不了）。大场景建图需要调大，但要权衡 PGO 计算开销。

planar_constraints：机器人是轮式或腿式（在地面运动）时设 True，约束位姿在水平面内，能提升稳定性。无人机设 False。

保存地图的时机：

# 建图完成后保存（不是每帧都保存！）tracker.save_map("/tmp/my_map", lambda ok: print("saved"if ok else"failed"))

save_map 是个比较重的操作（写整个位姿图 + 路标到 LMDB），建议在建图结束后调用一次，不要在跟踪循环里频繁调。lambda 是保存完成的回调，因为这是异步操作。

重定位：在新会话里加载地图找自己：

# 配置搜索范围settings = cuvslam.Tracker.SlamLocalizationSettings(    horizontal_search_radius=1.0,    # 水平搜索半径，单位 m    vertical_search_radius=0.5,      # 垂直搜索半径    horizontal_step=0.2,             # 水平搜索步长    vertical_step=0.1,               # 垂直搜索步长    angular_step_rads=0.2,           # 角度搜索步长，约 11.5°)# 触发重定位tracker.localize_in_map("/tmp/my_map",                   # 已保存的地图路径    ts_ns,                           # 当前时间戳    guess_pose,                      # 初始位姿猜测（粗略即可）    current_images,                  # 当前观测    settings,                        # 搜索参数    start_cb=lambda: print("locating..."),    finish_cb=lambda pose, err: print(f"found: {pose}"if pose else f"failed: {err}"))

重定位参数怎么调：

guess_pose 越准，搜索越快。机器狗场景下，可以从上次关机时的位姿作为猜测，或者从腿足里程计给一个粗略估计。

搜索范围（*_radius）和步长（*_step）决定搜索空间大小。范围太大 + 步长太小 = 搜索时间长；范围太小 = 可能找不到。建议先用大步长快速扫描，找到大致区域再用小步长精化。

angular_step_rads=0.2（约 11.5°）是默认值，对室内机器人够用。机器狗朝向变化大时可以调小。

重定位内部做的事：

在地图里以 guess_pose 为中心，按搜索半径和步长撒一堆候选位姿

每个候选位姿做一次 PnP 匹配当前观测

找到内点足够多的位姿就认为定位成功

把当前地图替换为已保存的地图，切换到 SLAM 模式继续跑

定位成功后，cuVSLAM 会用已保存的地图作为参考，继续做回环检测和位姿图优化，相当于"接着上次跑"。

六、源码走读

讲清楚基础用法后，看 cuVSLAM 官方仓库里的两个核心示例：KITTI 立体 VO（最简单）和 EuRoC 立体惯性（VIO 标杆）。这两个示例能帮你理解前面几节那些配置在实际项目里是怎么用的。

6.1 KITTI 立体 VO 示例

[examples/kitti/track_kitti.py](file:///～/cuVSLAM/examples/kitti/track_kitti.py) 只有 155 行，但涵盖了 cuVSLAM 使用的所有核心步骤。这是上手 cuVSLAM 的最佳起点。

import cuvslam# 1. 从 KITTI calib.txt 读取相机参数cameras = [cuvslam.Camera(), cuvslam.Camera()]cameras[0].size = sizecameras[0].principal = [cx, cy]cameras[0].focal = [fx, fy]cameras[1].rig_from_camera.translation[0] = -baseline# 2. 配置 trackercfg = cuvslam.Tracker.OdometryConfig(    async_sba=False,                    # KITTI 离线跑，不用异步    enable_final_landmarks_export=True, # 导出最终路标点做可视化    rectified_stereo_camera=True,       # KITTI 已校正，走快速路径)tracker = cuvslam.Tracker(cuvslam.Rig(cameras), cfg)# 3. 逐帧跟踪for frame in range(len(timestamps)):    images = [load_left(frame), load_right(frame)]    odom_pose, _ = tracker.track(timestamps[frame], images)if odom_pose.world_from_rig is None:continue# 跟踪失败# 4. 拿数据可视化    observations = tracker.get_last_observations(0)  # 左相机观测    landmarks = tracker.get_last_landmarks()         # 当前帧 3D 路标    final_landmarks = tracker.get_final_landmarks()  # 所有历史路标（BA 优化后）

逐段讲解：

第 1 步：标定传递

KITTI 的 calib.txt 每行是一个 3×4 投影矩阵 ：

所以从 calib.txt 解析参数的代码长这样：

intrinsics = loadtxt('calib.txt', usecols=range(1, 13))[:4].reshape(4, 3, 4)cameras[i].principal = [intrinsics[i][0][2], intrinsics[i][1][2]]   # cx, cycameras[i].focal = [intrinsics[i].diagonal()[0], intrinsics[i].diagonal()[1]]  # fx, fycameras[1].rig_from_camera.translation[0] = -intrinsics[1][0][3] / intrinsics[1][0][0]  # -B

右相机相对左相机的 baseline 通过 算出来。代码里取负号是因为 cuVSLAM 约定 rig_from_camera 表示"从相机坐标到 rig 坐标"的变换方向。

第 2 步：tracker 配置

async_sba=False：KITTI 是离线跑（不是实时机器人），不需要异步 BA，关掉反而更简单（结果立即可见）。

• enable_final_landmarks_export=True：导出最终路标点用于可视化。这是最贵的导出开关，实时场景慎开。

• rectified_stereo_camera=True：KITTI 图像已做立体校正（左右极线水平对齐），走快速路径省掉极线搜索。前提是相机真的校正过——没校正开这个开关会跑飞。

第 3 步：跟踪循环

tracker.track() 返回两个值：odom_pose（里程计位姿）和 slam_pose（SLAM 优化后的位姿，没用 SLAM 时为 None）。odom_pose.world_from_rig 是 rig 在世界坐标系的位姿，失败时为 None。世界坐标系原点是第一帧启动时的位置，朝向也是第一帧的朝向。

第 4 步：三种数据导出

get_last_observations(camera_index)：当前帧某一相机的 2D 观测，包含路标 ID 和像素坐标

get_last_landmarks()：当前帧看到的 3D 路标点（世界坐标系）

• get_final_landmarks()：所有历史帧累计的最终路标点，经过 BA 优化后的版本，这是最准的地图

前两个适合实时可视化（看当前帧在跟什么），第三个适合建图结束后导出完整地图。

6.2 EuRoC 立体惯性示例：IMU 怎么喂

[examples/euroc/track_euroc.py](file:///~/cuVSLAM/examples/euroc/track_euroc.py) 展示了 VIO 的标准用法。和 KITTI 的核心区别在于 IMU 数据要穿插喂入。

for frame_metadata in frames_metadata:# 1. 如果是 IMU 样本，先喂 IMUif frame_metadata['type'] == 'imu':        imu = cuvslam.ImuMeasurement()        imu.timestamp_ns = int(frame_metadata['timestamp'])        imu.linear_accelerations = np.asarray(frame_metadata['accel'])        imu.angular_velocities = np.asarray(frame_metadata['gyro'])        tracker.register_imu_measurement(0, imu)  # sensor_index=0continue# 2. 如果是图像帧，喂图像    images = [load_frame(p) for p in frame_metadata['images_paths']]    odom_pose, _ = tracker.track(timestamp, images)

-为什么 IMU 要穿插喂入：cuVSLAM 内部维护一个预积分缓冲区，IMU 样本到来时累积预积分，图像帧到来时用累积的预积分作为位姿先验做联合优化。如果先喂完所有 IMU 再喂图像，预积分就用不上了——必须按时间戳顺序穿插。

-EuRoC 的数据组织方式：EuRoC 把 IMU 和图像的 metadata 混在一个 CSV 里按时间戳排序。代码用一个循环按顺序读，遇到 IMU 行就 register_imu_measurement，遇到图像行就 track。实际项目里你的传感器驱动也要保证这种顺序——通常是开一个线程按时间戳合并所有传感器数据，然后单线程喂给 cuVSLAM。

-相机畸变模型选择：EuRoC 数据集有意思的地方是提供了两套标定：

原始数据集用 Brown 模型（5 参数）：[k1, k2, p1, p2, k3]

cuVSLAM 仓库重新标定的版本（sensor_cam0.yaml、sensor_cam1.yaml）用 Fisheye 模型（4 参数）：[k1, k2, k3, k4]

cuvslam.Distortion(    model=cuvslam.Distortion.Model.Fisheye,    parameters=[k1, k2, k3, k4])

四种畸变模型对应不同相机：

七、和机器狗导航实战的结合

7.1 典型集成架构

+---------------------------------------------------+|                                                   ||    +----------+  +----------+  +----------+       ||    | RealSense|  | RealSense|  |    IMU   |       ||    |  前相机   |  |  后相机   |   |  (板载)  |       ||    +-----+----+  +-----+----+  +-----+----+       ||          |             |             |            ||          +-------------+-------------+            ||                        |                          ||                        v                          ||              +--------------------+               ||              |      cuVSLAM       |   <- 这一层    ||              |    (VIO + SLAM)    |               ||              +---------+----------+               ||                        v                          ||              +--------------------+               ||              |  /odometry topic   |   <- ROS2 接口 ||              +---------+----------+               ||          +-------------+-------------+            ||          v             v             v            ||    +---------+   +---------+   +-----------+      ||    |  Nav2   |   | Nvblox  |   | 楼梯逻辑    |      ||    +---------+   +---------+   +-----------+      ||                        |                          ||                        v                          ||              +--------------------+               ||              |    腿足控制器        |               ||              +--------------------+               ||                                                   |+---------------------------------------------------+

cuVSLAM 输出 ROS2 的 /odometry 话题和 TF 变换。NVIDIA 的 Isaac ROS Visual SLAM 包做了这个封装。

7.2 实战注意点

时间戳同步：机器狗上的相机、IMU、关节编码器各有各的时戳源。必须用硬件触发或软件 PTP 同步，cuVSLAM 要求多相机时戳差 < 1ms。

振动问题：机器狗走路相机抖动严重容易产生运动模糊。对策：

提高帧率到 60 FPS（cuVSLAM 在 Jetson Orin 上能扛住）

用全局快门相机（卷帘快门在振动下会扭曲图像）

楼梯场景：上下楼梯 pitch 变化大，纯视觉容易丢特征。用 VIO 模式（IMU 在视觉丢失时撑一会），配合腿足里程计外层 EKF 融合。

重定位：机器狗关机再开机要能定位。localize_in_map 接口把之前建的地图加载进来在地图里找当前位姿。

7.3 cuVSLAM 的局限

诚实地说几个局限：

1. 不支持纯激光 SLAM：要激光惯性融合得用 LIO-SAM、FAST-LIO2

2. 没有腿足里程计融合：得自己在外层 EKF/UKF 里融合

3. 闭源部分较多：回环检测、关键帧选择的具体算法没完全开源

4. License 限制：NVIDIA Community License，限定 NVIDIA 硬件

5. ROS1 不支持：只有 ROS2 包

如果项目是 ROS1 + 激光主导，cuVSLAM 不是最佳选择。如果是 ROS2 + 视觉主导 + Jetson 平台，cuVSLAM 是目前最省心的方案之一。

八、延伸阅读

• cuVSLAM 论文：arXiv 2506.04359

• 视觉 SLAM 十四讲（高翔）：视觉 SLAM 入门圣经，第二版覆盖到 VIO

• VINS-Mono 论文：arXiv 1709.06510 — IMU 预积分工程实现参考

• LIO-SAM 论文：arXiv 2007.00258 — 激光惯性 SLAM 标杆

• Isaac ROS Visual SLAM：GitHub

• Rerun 文档：rerun.io — cuVSLAM 用的可视化工具

• Kalibr：GitHub — 相机-IMU 外参标定工具