在 Jetson 上部署 PyCuVSLAM 全流程实战：从安装到实时视觉里程计！
·
介绍
PyCuVSLAM 是 NVIDIA 的 GPU 加速视觉里程计与 SLAM 库 cuVSLAM 的 Python 封装，支持单目、双目、RGB‑D、多相机以及视觉‑惯性（IMU）模式。它提供简洁的 Python API，可直接对接相机数据流并输出实时相机位姿、地图点与回环信息。得益于底层 CUDA 优化，PyCuVSLAM 能在 PC 与 Jetson 设备上实现高精度、低时延的 SLAM 推理，适用于机器人导航、无人机定位与 3D 感知等应用。本文将演示如何在 Seeed Studio reComputer 上部署与使用 PyCuVSLAM。
预先准备
- 预装 JetPack 6.2 的 Jetson（本文使用Seeed Studio的Robotics J4012开发板 ）
- RGB 相机
- RGB‑D 相机（推荐 Orbbec Gemini2）
- 已安装 ROS2 Humble 环境
安装
步骤 1. 克隆 PyCuVSLAM 仓库
git clone https://github.com/NVlabs/pycuvslam.git
cd pycuvslam
步骤 2. 安装 Git LFS
sudo apt-get install git-lfs
步骤 3. 安装 PyCuVSLAM 包
Jetson 设备（ARM64 架构）：
# 适用于 Jetson 设备
pip install -e bin/aarch64
Ubuntu PC（x86_64 架构）：
# 适用于 Ubuntu PC
pip install -e bin/x86_64
步骤 4. 安装依赖
# 安装示例所需依赖
pip install -r examples/requirements.txt
单目视觉里程计（Monocular VO）
数据集准备
步骤 1. 下载 EuRoC MH_01_easy 数据集：
mkdir -p examples/euroc/dataset
wget http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset/machine_hall/MH_01_easy/MH_01_easy.zip -O examples/euroc/dataset/MH_01_easy.zip
unzip examples/euroc/dataset/MH_01_easy.zip -d examples/euroc/dataset
rm examples/euroc/dataset/MH_01_easy.zip
步骤 2. 拷贝标定文件：
cp examples/euroc/sensor_cam0.yaml examples/euroc/dataset/mav0/cam0/sensor_recalibrated.yaml
cp examples/euroc/sensor_cam1.yaml examples/euroc/dataset/mav0/cam1/sensor_recalibrated.yaml
cp examples/euroc/sensor_imu0.yaml examples/euroc/dataset/mav0/imu0/sensor_recalibrated.yaml
相机标定（USB 相机）
v4l2_camera 是 ROS2 官方维护的节点，可直接发布 USB 摄像头图像，用于标定流程。
步骤 1. 安装相机标定与采集：
sudo apt install ros-humble-camera-calibration
sudo apt install ros-${ROS_DISTRO}-v4l2-camera
步骤 2. 启动相机节点：
ros2 run v4l2_camera v4l2_camera_node
默认话题：
/image_raw- 原始图像/camera- 相机信息
步骤 3. 运行相机标定：
ros2 run camera_calibration cameracalibrator \
--size 8x6 --square 0.025 \
--ros-args --remap image:=/image_raw --remap camera:=/camera
步骤 3. 运行相机标定：
# 在另一个终端中
ros2 run camera_calibration cameracalibrator \
--size 8x6 --square 0.025 \
--ros-args --remap image:=/image_raw --remap camera:=/camera
--size 8x6指内角点数量（8×6 = 48个角点，对应9×7网格）--square 0.025指方块大小（米），此处为25mm- 移动相机从不同角度采集图像，直到
CALIBRATE按钮亮起
标定成功后，您将在终端中获得类似以下的相机参数：
运行示例
mono_slam.py#
# Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
#
# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
# property and proprietary rights in and to this material, related
# documentation and any modifications thereto. Any use, reproduction,
# disclosure or distribution of this material and related documentation
# without an express license agreement from NVIDIA CORPORATION or
# its affiliates is strictly prohibited.
#
"""
使用网络摄像头/USB相机的实时单目视觉SLAM
此脚本演示如何使用cuVSLAM进行实时单目SLAM，配合实时相机数据流。
"""
import argparse
import time
from typing import List, Optional
import os
import cv2
import numpy as np
import rerun as rr
import rerun.blueprint as rrb
import yaml
import cuvslam
def color_from_id(identifier):
"""从整数标识符生成伪随机颜色用于可视化。"""
return [
(identifier * 17) % 256,
(identifier * 31) % 256,
(identifier * 47) % 256
]
def load_camera_config(config_path: str) -> dict:
"""
从YAML文件加载相机配置。
Args:
config_path: YAML配置文件路径
Returns:
包含相机参数的字典
"""
if not os.path.exists(config_path):
raise FileNotFoundError(f"配置文件未找到: {config_path}")
with open(config_path, 'r') as f:
config = yaml.safe_load(f)
return config
def create_camera_rig(
width: int,
height: int,
fx: Optional[float] = None,
fy: Optional[float] = None,
cx: Optional[float] = None,
cy: Optional[float] = None,
distortion_coeffs: Optional[List[float]] = None
) -> cuvslam.Rig:
"""
创建具有指定参数的单目相机设备。
Args:
width: 图像宽度（像素）
height: 图像高度（像素）
fx: X方向焦距（像素）。如果为None，则根据图像宽度估算
fy: Y方向焦距（像素）。如果为None，则根据图像宽度估算
cx: 主点X坐标。如果为None，设置为width/2
cy: 主点Y坐标。如果为None，设置为height/2
distortion_coeffs: 畸变系数 [k1, k2, p1, p2, k3]。如果为None，假设为针孔相机
Returns:
配置用于单目跟踪的cuvslam.Rig对象
"""
# 如果未提供，使用默认相机参数
# 根据典型网络摄像头FOV（~60-70度）估算焦距
if fx is None:
fx = width * 0.9 # 近似焦距
if fy is None:
fy = width * 0.9
if cx is None:
cx = width / 2.0
if cy is None:
cy = height / 2.0
# 创建相机对象
cam = cuvslam.Camera()
cam.focal = (fx, fy)
cam.principal = (cx, cy)
cam.size = (width, height)
# 设置畸变模型
if distortion_coeffs is not None:
# Brown-Conrady畸变模型
cam.distortion = cuvslam.Distortion(
cuvslam.Distortion.Model.Brown,
distortion_coeffs
)
else:
# 针孔相机（无畸变）
cam.distortion = cuvslam.Distortion(cuvslam.Distortion.Model.Pinhole)
# 对于单目相机，设置恒等变换（相机位于设备原点）
cam.rig_from_camera = cuvslam.Pose(
rotation=[0, 0, 0, 1], # 恒等四元数 (w, x, y, z)
translation=[0, 0, 0] # 零平移
)
# 创建包含单个相机的设备
rig = cuvslam.Rig()
rig.cameras = [cam]
return rig
def setup_visualizer():
"""初始化单目SLAM布局的rerun可视化器。"""
rr.init("单目SLAM可视化器", spawn=True)
# 设置坐标基 - cuVSLAM使用右手坐标系
# X-右，Y-下，Z-前
rr.log("world", rr.ViewCoordinates.RIGHT_HAND_Y_DOWN, static=True)
# 设置可视化布局
rr.send_blueprint(
rrb.Blueprint(
rrb.TimePanel(state="collapsed"),
rrb.Horizontal(
column_shares=[0.5, 0.5],
contents=[
rrb.Vertical(contents=[
rrb.Spatial2DView(
origin='world/camera',
name="带特征点的相机画面"
),
]),
rrb.Spatial3DView(
origin='world',
name="3D轨迹"
)
]
)
)
)
def main():
"""实时单目SLAM的主函数。"""
parser = argparse.ArgumentParser(description='实时单目视觉SLAM')
parser.add_argument(
'--config',
type=str,
default=None,
help='相机配置YAML文件路径（覆盖单独参数）'
)
parser.add_argument(
'--camera',
type=int,
default=0,
help='相机设备ID（默认：0，对应/dev/video0）'
)
parser.add_argument(
'--width',
type=int,
default=640,
help='相机帧宽度（默认：640）'
)
parser.add_argument(
'--height',
type=int,
default=480,
help='相机帧高度（默认：480）'
)
parser.add_argument(
'--fps',
type=int,
default=30,
help='相机FPS（默认：30）'
)
parser.add_argument(
'--fx',
type=float,
default=None,
help='X方向焦距（像素）。如果未提供，将进行估算'
)
parser.add_argument(
'--fy',
type=float,
default=None,
help='Y方向焦距（像素）。如果未提供，将进行估算'
)
parser.add_argument(
'--cx',
type=float,
default=None,
help='主点X坐标。如果未提供，将设置为image_width/2'
)
parser.add_argument(
'--cy',
type=float,
default=None,
help='主点Y坐标。如果未提供，将设置为image_height/2'
)
parser.add_argument(
'--distortion',
type=float,
nargs=5,
default=None,
metavar=('k1', 'k2', 'p1', 'p2', 'k3'),
help='畸变系数：k1 k2 p1 p2 k3（Brown-Conrady模型）'
)
parser.add_argument(
'--grayscale',
action='store_true',
help='将帧转换为灰度（推荐以获得更好性能）'
)
parser.add_argument(
'--undistort',
action='store_true',
help='跟踪前校正图像（如果您有畸变系数，推荐使用）'
)
parser.add_argument(
'--show-debug',
action='store_true',
help='显示调试信息和特征点数量'
)
parser.add_argument(
'--detect-stationary',
action='store_true',
help='启用静止检测以抑制相机静止时的漂移'
)
parser.add_argument(
'--min-features',
type=int,
default=50,
help='跟踪所需的最小特征点数量（默认：50）'
)
parser.add_argument(
'--quality-threshold',
type=float,
default=0.01,
help='特征检测的质量阈值（默认：0.01）'
)
parser.add_argument(
'--show-opencv',
action='store_true',
help='显示带特征点的OpenCV窗口（除了Rerun外）'
)
args = parser.parse_args()
# 如果提供了配置文件，从文件加载相机配置
fx, fy, cx, cy, distortion_coeffs = None, None, None, None, None
if args.config:
print(f"从以下路径加载相机配置: {args.config}")
config = load_camera_config(args.config)
# 从配置中提取参数
if 'image' in config:
args.width = config['image'].get('width', args.width)
args.height = config['image'].get('height', args.height)
if 'camera_matrix' in config:
fx = config['camera_matrix'].get('fx')
fy = config['camera_matrix'].get('fy')
cx = config['camera_matrix'].get('cx')
cy = config['camera_matrix'].get('cy')
if 'distortion_coefficients' in config:
dist = config['distortion_coefficients']
distortion_coeffs = [
dist.get('k1', 0.0),
dist.get('k2', 0.0),
dist.get('p1', 0.0),
dist.get('p2', 0.0),
dist.get('k3', 0.0)
]
print(f"配置已加载: {args.width}x{args.height}, fx={fx}, fy={fy}, cx={cx}, cy={cy}")
else:
# 使用命令行参数
fx = args.fx
fy = args.fy
cx = args.cx
cy = args.cy
distortion_coeffs = args.distortion
# 打开相机
print(f"正在打开相机 {args.camera}...")
cap = cv2.VideoCapture(args.camera)
if not cap.isOpened():
print(f"错误：无法打开相机 {args.camera}")
return
# 设置相机属性
cap.set(cv2.CAP_PROP_FRAME_WIDTH, args.width)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, args.height)
cap.set(cv2.CAP_PROP_FPS, args.fps)
# 获取实际相机属性（可能与请求的不同）
actual_width = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
actual_height = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
actual_fps = cap.get(cv2.CAP_PROP_FPS)
print(f"相机已初始化: {actual_width}x{actual_height} @ {actual_fps} FPS")
# 创建相机设备
rig = create_camera_rig(
width=actual_width,
height=actual_height,
fx=fx,
fy=fy,
cx=cx,
cy=cy,
distortion_coeffs=distortion_coeffs
)
print(f"相机内参:")
print(f" 焦距: ({rig.cameras[0].focal[0]:.2f}, {rig.cameras[0].focal[1]:.2f})")
print(f" 主点: ({rig.cameras[0].principal[0]:.2f}, {rig.cameras[0].principal[1]:.2f})")
print(f" 分辨率: {rig.cameras[0].size}")
if distortion_coeffs:
print(f" 畸变: {distortion_coeffs}")
# 为单目模式配置跟踪器
# 注意：单目SLAM无法估计绝对尺度，只能估计相对运动
# 为更好的精度和减少漂移优化的参数
cfg = cuvslam.Tracker.OdometryConfig(
async_sba=True, # 启用异步束调整以获得更好的优化
enable_observations_export=True,
enable_final_landmarks_export=False, # 为性能禁用
horizontal_stereo_camera=False,
odometry_mode=cuvslam.Tracker.OdometryMode.Mono,
use_gpu=True, # 使用GPU加速
use_motion_model=True, # 使用运动模型进行预测
enable_landmarks_export=False # 为性能禁用
)
# 初始化跟踪器
tracker = cuvslam.Tracker(rig, cfg)
print(f"cuVSLAM跟踪器已初始化，里程计模式：单目")
print("注意：单目SLAM提供旋转和相对平移（尺度是任意的）")
# 设置可视化器
setup_visualizer()
# 跟踪变量
frame_id = 0
trajectory = []
start_time = time.time()
failed_frames = 0
# 单目SLAM初始化状态
is_initialized = False
initialization_frames = 0
min_init_frames = 30 # 需要至少30帧具有良好特征才能初始化
# 静止检测
stationary_threshold = 0.001 # 单目的非常小阈值（尺度是任意的）
stationary_count = 0
last_position = None
is_stationary = False
# 运动质量跟踪
low_feature_count = 0
motion_warnings = []
print("\n开始实时SLAM...")
print("=" * 60)
print("📌 单目SLAM使用技巧:")
print(" 1. 在初始化期间（前30帧）缓慢移动相机")
print(" 2. 避免快速旋转或平移")
print(" 3. 确保场景具有丰富的纹理特征")
print(" 4. 保持光照稳定")
print(" 5. 注意：单目SLAM无法估计真实尺度")
print("=" * 60)
print("\n按'q'退出，按's'保存轨迹\n")
try:
while True:
# 捕获帧
ret, frame = cap.read()
if not ret:
print("错误：无法捕获帧")
break
# 如果请求，转换为灰度
if args.grayscale:
if len(frame.shape) == 3:
frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
else:
# cuVSLAM期望BGR格式，OpenCV默认使用BGR
pass
# 生成时间戳（纳秒）
# 使用单调时间避免系统时钟变化的问题
timestamp_ns = int((time.time() - start_time) * 1e9)
# 跟踪帧
# 对于单目，在列表中传递单个图像
odom_pose_estimate, _ = tracker.track(timestamp_ns, [frame])
# 检查跟踪是否成功
if odom_pose_estimate.world_from_rig is None:
failed_frames += 1
if frame_id % 10 == 0 or frame_id < 50:
print(f"⚠️ 警告：帧 {frame_id} 跟踪失败")
frame_id += 1
continue
# 获取当前位姿和观测
odom_pose = odom_pose_estimate.world_from_rig.pose
current_observations = tracker.get_last_observations(0)
num_features = len(current_observations)
# 检查特征点数量质量
if num_features < args.min_features:
low_feature_count += 1
if low_feature_count % 10 == 1:
print(f"⚠️ 特征点不足: {num_features} < {args.min_features}（建议改善光照或场景纹理）")
else:
low_feature_count = 0
# 单目初始化阶段
if not is_initialized:
initialization_frames += 1
if initialization_frames >= min_init_frames and num_features >= args.min_features:
is_initialized = True
print(f"✅ 单目SLAM初始化完成！已处理 {initialization_frames} 帧")
print(f" 开始正常跟踪，当前特征点: {num_features}")
elif initialization_frames % 10 == 0:
print(f"⏳ 正在初始化... {initialization_frames}/{min_init_frames} 帧，特征点: {num_features}")
# 获取位置用于漂移检测
current_position = np.array(odom_pose.translation)
# 静止检测（单目，初始化后）
if args.detect_stationary and is_initialized and last_position is not None:
position_change = np.linalg.norm(current_position - last_position)
if position_change < stationary_threshold:
stationary_count += 1
if stationary_count > 30: # 30帧静止
is_stationary = True
else:
stationary_count = 0
is_stationary = False
# 静止时抑制漂移
if is_stationary and len(trajectory) > 0:
current_position = last_position
last_position = current_position.copy()
trajectory.append(current_position)
# 使用rerun可视化
rr.set_time_sequence("frame", frame_id)
# 记录轨迹（仅在初始化后以避免噪声初始轨迹）
if is_initialized and len(trajectory) > 1:
# 平滑轨迹用于可视化（减少抖动）
if len(trajectory) > 5:
# 使用最后N个点进行更平滑的可视化
smoothed_traj = trajectory[-min(len(trajectory), 500):]
rr.log("world/trajectory", rr.LineStrips3D(smoothed_traj), static=True)
else:
rr.log("world/trajectory", rr.LineStrips3D(trajectory), static=True)
# 记录相机位姿
rr.log(
"world/camera",
rr.Transform3D(
translation=odom_pose.translation,
quaternion=odom_pose.rotation
),
rr.Arrows3D(
vectors=np.eye(3) * 0.2,
colors=[[255, 0, 0], [0, 255, 0], [0, 0, 255]] # RGB对应XYZ
)
)
# 在相机图像上记录观测
points = np.array([[obs.u, obs.v] for obs in current_observations])
colors = np.array([color_from_id(obs.id) for obs in current_observations])
# 准备用于可视化的图像
if args.grayscale and len(frame.shape) == 2:
# 将灰度转换为RGB用于可视化
vis_frame = cv2.cvtColor(frame, cv2.COLOR_GRAY2RGB)
elif not args.grayscale and len(frame.shape) == 3:
# 将BGR转换为RGB用于可视化
vis_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
else:
vis_frame = frame
rr.log(
"world/camera/observations",
rr.Points2D(positions=points, colors=colors, radii=5.0),
rr.Image(vis_frame).compress(jpeg_quality=80)
)
# 显示FPS和特征点数量
if frame_id % 30 == 0 and is_initialized:
elapsed = time.time() - start_time
fps = frame_id / elapsed if elapsed > 0 else 0
# 状态指示器
feature_status = "🔴 低" if num_features < args.min_features else "🟡 正常" if num_features < 100 else "🟢 良好"
motion_status = "🛑 静止" if is_stationary else "🚀 移动"
status_line = f"📊 帧 {frame_id}: {num_features} 特征点 {feature_status}, {fps:.1f} FPS"
if args.detect_stationary:
status_line += f" | {motion_status}"
print(status_line)
print(f" 📍 相对位置: [{current_position[0]:.3f}, {current_position[1]:.3f}, {current_position[2]:.3f}]（任意尺度）")
if failed_frames > 0:
print(f" ⚠️ 累计失败帧数: {failed_frames}")
frame_id += 1
# 可选：在OpenCV窗口中显示帧用于调试
if args.show_opencv:
# 在帧上绘制特征点
display_frame = vis_frame.copy()
for obs in current_observations:
pt = (int(obs.u), int(obs.v))
cv2.circle(display_frame, pt, 2, (0, 255, 0), -1)
# 添加状态文本
status_text = f"帧: {frame_id}"
if is_initialized:
status_text += f" | 特征点: {num_features}"
if args.detect_stationary and is_stationary:
status_text += " | 静止"
else:
status_text += f" | 初始化: {initialization_frames}/{min_init_frames}"
cv2.putText(display_frame, status_text, (10, 30),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
cv2.imshow('单目SLAM', display_frame)
key = cv2.waitKey(1)
if key == ord('q'):
break
elif key == ord('s'):
# 保存轨迹
if len(trajectory) > 0:
np.savetxt('trajectory_mono.txt', np.array(trajectory),
fmt='%.6f', delimiter=',')
print(f"✅ 轨迹已保存: trajectory_mono.txt ({len(trajectory)} 个位姿)")
except KeyboardInterrupt:
print("\n用户中断")
finally:
# 清理
print("\n" + "="*60)
print("单目SLAM会话总结")
print("="*60)
print(f"总处理帧数: {frame_id}")
print(f"成功跟踪: {len(trajectory)} 个位姿")
print(f"失败帧数: {failed_frames}")
print(f"初始化状态: {'✅ 完成' if is_initialized else '❌ 未完成'}")
if frame_id > 0:
success_rate = (len(trajectory) / frame_id) * 100
print(f"成功率: {success_rate:.1f}%")
# 运动质量统计
if low_feature_count > 0:
print(f"\n⚠️ 跟踪质量警告:")
print(f" 低特征点帧数: {low_feature_count}")
# 保存轨迹
if len(trajectory) > 0:
trajectory_array = np.array(trajectory)
np.savetxt('trajectory_mono.txt', trajectory_array,
fmt='%.6f', delimiter=',',
header='x,y,z (任意尺度)')
print(f"\n✅ 轨迹已保存到 trajectory_mono.txt ({len(trajectory)} 个位姿)")
# 计算轨迹统计
if len(trajectory) > 1:
distances = np.diff(trajectory_array, axis=0)
total_distance = np.sum(np.linalg.norm(distances, axis=1))
print(f" 总行驶距离: {total_distance:.2f}（任意单位）")
else:
print("\n⚠️ 无轨迹数据可保存")
cap.release()
cv2.destroyAllWindows()
print("\n相机已释放，正在退出...")
print("="*60)
if __name__ == "__main__":
main()
运行示例脚本：
python mono_slam.py --config /path/to/your/Camera parameters yaml file
您需要将 /path/to/your/Camera parameters yaml file 修改为您保存相机参数的yaml文件。yaml文件内容如下：
# 单目相机标定配置文件
# 图像参数
image:
width: 640
height: 480
# 相机内参矩阵 (3x3)
# 格式: [fx, 0, cx; 0, fy, cy; 0, 0, 1]
camera_matrix:
fx: 503.404437 # X方向焦距
fy: 633.115563 # Y方向焦距
cx: 414.895624 # 主点X坐标
cy: 200.895636 # 主点Y坐标
# 畸变系数 (Brown-Conrady模型)
# 格式: [k1, k2, p1, p2, k3]
distortion_coefficients:
k1: 0.185506 # 径向畸变系数1
k2: -0.107727 # 径向畸变系数2
p1: -0.006414 # 切向畸变系数1
p2: 0.060255 # 切向畸变系数2
k3: 0.000000 # 径向畸变系数3
# 单目SLAM优化建议
slam_optimization:
# 推荐最小特征点数量
min_features: 80
# 推荐相机移动速度（相对）
max_motion_per_frame: 0.1
# 初始化建议
initialization:
min_frames: 30
slow_motion: true
rich_texture: true
单目-深度视觉里程计
相机标定
单目-深度视觉里程计需要相机和深度图像之间的像素级对应关系。Orbbec Gemini 2 是一个立体结构光/主动立体红外3D相机，提供深度和RGB（彩色）输出。其关键特性之一是硬件加速的深度到彩色对齐（D2C，深度→彩色），这意味着深度图和RGB图像在数据到达您的主机计算机之前已经在像素级别进行了空间对齐。这减少了您主机处理器的计算负载，并简化了深度+彩色的融合，适用于3D重建、SLAM、带深度的目标检测等应用。
步骤 1. 安装Orbbec ROS2驱动：
mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src
git clone https://github.com/orbbec/OrbbecSDK_ROS2.git
# 安装依赖
sudo apt install libgflags-dev nlohmann-json3-dev \
ros-$ROS_DISTRO-image-transport ros-${ROS_DISTRO}-image-transport-plugins \
ros-${ROS_DISTRO}-compressed-image-transport ros-$ROS_DISTRO-image-publisher \
ros-$ROS_DISTRO-camera-info-manager ros-$ROS_DISTRO-diagnostic-updater \
ros-$ROS_DISTRO-diagnostic-msgs ros-$ROS_DISTRO-statistics-msgs \
ros-$ROS_DISTRO-backward-ros libdw-dev
pip install catkin_pkg empy==3.3.4 lark-parser
# 安装udev规则
cd ~/ros2_ws/src/OrbbecSDK_ROS2/orbbec_camera/scripts
sudo bash install_udev_rules.sh
sudo udevadm control --reload-rules && sudo udevadm trigger
# 构建
cd ~/ros2_ws/
colcon build --event-handlers console_direct+ --cmake-args -DCMAKE_BUILD_TYPE=Release
colcon build --packages-select orbbec_camera_msgs
# 源化并启动
source ./install/setup.bash
ros2 launch orbbec_camera gemini2.launch.py
您可以通过观察相机数据话题是否正常发布来检查相机节点是否能正常启动。
步骤 2. 运行RGB-D相机标定：
ros2 run camera_calibration cameracalibrator --size 8x6 --square 0.025 \
--ros-args --remap image:=/camera/color/image_raw --remap camera:=/camera/color
对于RGB-D相机，您将获得相机的参数：
[image]
width: 1280
height: 720
[narrow_stereo]
camera matrix:
690.546721 0.000000 684.064868
0.000000 683.586452 370.939099
0.000000 0.000000 1.000000
distortion:
-0.010482 -0.019797 0.001294 0.021572 0.000000
rectification:
1.000000 0.000000 0.000000
0.000000 1.000000 0.000000
0.000000 0.000000 1.000000
projection:
654.569214 0.000000 734.393949 0.000000
0.000000 690.282776 371.706832 0.000000
0.000000 0.000000 1.000000 0.000000
步骤 3. 获取深度到彩色的外参
ros2 launch orbbec_camera gemini2.launch.py
ros2 topic echo /camera/depth_to_color
从ROS2话题中读取从深度相机坐标系到彩色相机坐标系的旋转矩阵和平移向量：
# 深度相机到彩色相机坐标系变换
rotation:
- 0.9999980330467224
- 0.0005175529513508081
- 0.0019138390198349953
- -0.0005151802906766534
- 0.9999991059303284
- -0.0012400292325764894
- -0.0019144790712743998
- 0.001239040750078857
- 0.9999973773956299
translation:
- -0.013858354568481446
- 0.0001548745185136795
- -0.00187313711643219
运行示例
rgbd_slam.py#
# Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
#
# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
# property and proprietary rights in and to this material, related
# documentation and any modifications thereto. Any use, reproduction,
# disclosure or distribution of this material and related documentation
# without an express license agreement from NVIDIA CORPORATION or
# its affiliates is strictly prohibited.
#
"""
Orbbec Gemini 2 深度相机RGBD视觉SLAM
此脚本演示如何使用cuVSLAM与Orbbec Gemini 2相机进行RGBD SLAM
"""
# 导入系统库
import sys
import time
from typing import List, Optional, Tuple
import argparse
# 导入计算机视觉和数值计算库
import cv2 # OpenCV - 图像处理
import numpy as np # NumPy - 数值计算
import yaml # YAML - 配置文件解析
# 导入Orbbec相机SDK
from pyorbbecsdk import *
# 导入NVIDIA cuVSLAM库
import cuvslam as vslam
# 添加realsense文件夹到系统路径以导入可视化器
import os
sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), 'realsense')))
from visualizer import RerunVisualizer
from opencv_visualizer import OpenCVVisualizer
# ==================== 常量定义 ====================
WARMUP_FRAMES = 60 # 预热帧数 - SLAM系统需要一些帧来初始化
IMAGE_JITTER_THRESHOLD_MS = 100 * 1e6 # 图像抖动阈值（纳秒）- 超过此间隔的帧被视为丢失
NUM_VIZ_CAMERAS = 2 # 可视化相机数量 - 用于显示彩色和深度图像
DEPTH_SCALE_FACTOR = 1000.0 # 深度缩放因子 - 将毫米转换为米（Gemini 2深度单位为毫米）
FRAME_WAIT_TIMEOUT_MS = 200 # 帧等待超时（毫秒）- 增加超时以减少帧丢失
def simple_frame_to_bgr(frame) -> Optional[np.ndarray]:
"""
将Orbbec帧转换为BGR格式numpy数组
Args:
frame: Orbbec相机帧对象
Returns:
Optional[np.ndarray]: BGR格式图像数组，如果转换失败则返回None
"""
# 获取帧宽度和高度
width = frame.get_width()
height = frame.get_height()
# 从帧数据创建numpy数组
data = np.frombuffer(frame.get_data(), dtype=np.uint8)
# 检查数据大小是否正确（应该是width * height * 3字节）
if data.size != width * height * 3:
return None
# 将1D数组重塑为3D图像数组（高度，宽度，3）
image = data.reshape((height, width, 3))
# 将RGB格式转换为BGR格式（OpenCV使用BGR）
return cv2.cvtColor(image, cv2.COLOR_RGB2BGR)
def create_depth_visualization(depth_data: np.ndarray) -> np.ndarray:
"""
创建深度图像可视化效果（类似于test_camera.py）
Args:
depth_data: 深度数据数组（单位：毫米）
Returns:
np.ndarray: 彩色深度可视化图像
"""
# 设置深度范围（毫米）- 过滤掉太近和太远的点
min_depth = 150 # 最小深度150mm
max_depth = 2000 # 最大深度2000mm
# 将深度值限制在指定范围内
depth_clipped = np.clip(depth_data, min_depth, max_depth)
# 反转深度值（近点显示为亮，远点显示为暗）
depth_inverted = max_depth - depth_clipped
# 将深度值归一化到0-255范围
depth_normalized = cv2.normalize(depth_inverted, None, 0, 255, cv2.NORM_MINMAX).astype(np.uint8)
# 应用MAGMA颜色映射（红-黄-白渐变）
depth_vis = cv2.applyColorMap(depth_normalized, cv2.COLORMAP_MAGMA)
return depth_vis
def get_gemini2_camera_intrinsics(color_profile) -> dict:
"""
从Orbbec Gemini 2彩色流配置中提取相机内参
Args:
color_profile: Orbbec彩色流配置对象
Returns:
dict: 包含相机内参的字典
- fx, fy: 焦距（像素）
- cx, cy: 主点坐标（像素）
- width, height: 图像分辨率
"""
# 获取相机内参
intrinsics = color_profile.get_intrinsic()
# 返回内参字典
return {
'fx': intrinsics.fx, # X方向焦距
'fy': intrinsics.fy, # Y方向焦距
'cx': intrinsics.cx, # X方向主点
'cy': intrinsics.cy, # Y方向主点
'width': intrinsics.width, # 图像宽度
'height': intrinsics.height # 图像高度
}
def create_gemini2_rig(intrinsics: dict, distortion_coeffs: Optional[List[float]] = None) -> vslam.Rig:
"""
为Orbbec Gemini 2相机创建cuVSLAM Rig对象
Args:
intrinsics: 相机内参字典
distortion_coeffs: 畸变系数列表 [k1, k2, p1, p2, k3]
Returns:
vslam.Rig: 包含相机配置的cuVSLAM Rig对象
"""
# 创建相机对象
cam = vslam.Camera()
# 设置相机内参
cam.focal = (intrinsics['fx'], intrinsics['fy']) # 焦距
cam.principal = (intrinsics['cx'], intrinsics['cy']) # 主点
cam.size = (intrinsics['width'], intrinsics['height']) # 图像大小
# 设置畸变模型
if distortion_coeffs is not None and any(abs(coeff) > 1e-6 for coeff in distortion_coeffs):
# 使用RadialTangential畸变模型（cuVSLAM支持的畸变模型）
try:
cam.distortion = vslam.Distortion(vslam.Distortion.Model.RadialTangential)
cam.distortion.coeffs = distortion_coeffs
print(f"使用畸变校正: {distortion_coeffs}")
except AttributeError:
# 如果RadialTangential不可用，尝试其他模型
try:
cam.distortion = vslam.Distortion(vslam.Distortion.Model.Radial)
cam.distortion.coeffs = distortion_coeffs[:2] # 只使用前两个径向畸变系数
print(f"使用径向畸变校正: {distortion_coeffs[:2]}")
except AttributeError:
# 如果都不支持，使用针孔模型
cam.distortion = vslam.Distortion(vslam.Distortion.Model.Pinhole)
print("畸变模型不支持，使用针孔模型（无畸变）")
else:
# 使用针孔模型（无畸变）
cam.distortion = vslam.Distortion(vslam.Distortion.Model.Pinhole)
print("使用针孔模型（无畸变）")
# Rig坐标系中的相机位姿（相机位于Rig原点）
cam.rig_from_camera = vslam.Pose(
rotation=[0, 0, 0, 1], # 单位四元数 (w, x, y, z)
translation=[0, 0, 0] # 零平移
)
# 创建包含单个相机的Rig
rig = vslam.Rig()
rig.cameras = [cam]
return rig
def setup_gemini2_pipeline(target_width: Optional[int] = None, target_height: Optional[int] = None) -> Tuple[Pipeline, Config, dict]:
"""
设置Orbbec Gemini 2相机管道并获取相机内参
Args:
target_width: 目标图像宽度（None表示使用默认/最高分辨率）
target_height: 目标图像高度（None表示使用默认/最高分辨率）
Returns:
Tuple[Pipeline, Config, dict]:
- Pipeline: Orbbec相机管道对象
- Config: 相机配置对象
- dict: 相机内参字典
"""
# 创建相机配置和管道对象
config = Config()
pipeline = Pipeline()
# 获取彩色流配置 - 使用与test_camera.py相同的方法
color_profile_list = pipeline.get_stream_profile_list(OBSensorType.COLOR_SENSOR)
color_profile = None
# 如果指定了分辨率，查找匹配的配置
if target_width is not None and target_height is not None:
print(f"寻找分辨率 {target_width}x{target_height} RGB配置...")
for cp in color_profile_list:
if cp.get_format() == OBFormat.RGB:
if cp.get_width() == target_width and cp.get_height() == target_height:
color_profile = cp
print(f"✅ 找到匹配的分辨率配置: {target_width}x{target_height}")
break
if color_profile is None:
print(f"⚠️ 未找到精确匹配的分辨率 {target_width}x{target_height}")
print("可用的RGB分辨率:")
for cp in color_profile_list:
if cp.get_format() == OBFormat.RGB:
print(f" - {cp.get_width()}x{cp.get_height()}")
print("将使用默认分辨率...")
# 如果未找到指定分辨率，使用默认RGB配置
if color_profile is None:
for cp in color_profile_list:
if cp.get_format() == OBFormat.RGB:
color_profile = cp
break
if color_profile is None:
print("错误：未找到RGB格式的彩色流配置")
sys.exit(-1)
# 获取与彩色流对齐的深度流配置 - 使用硬件D2C对齐
hw_d2c_profile_list = pipeline.get_d2c_depth_profile_list(color_profile, OBAlignMode.HW_MODE)
if len(hw_d2c_profile_list) == 0:
print("错误：未找到D2C对齐的深度流配置")
sys.exit(-1)
hw_d2c_profile = hw_d2c_profile_list[0]
# 启用流配置
config.enable_stream(hw_d2c_profile) # 启用深度流
config.enable_stream(color_profile) # 启用彩色流
config.set_align_mode(OBAlignMode.HW_MODE) # 设置硬件对齐模式
pipeline.enable_frame_sync() # 启用帧同步
# 启动管道
pipeline.start(config)
# 获取初始帧以提取内参 - 带重试机制
print("获取初始帧以提取内参...")
frames = None
for attempt in range(10): # 最多尝试10次
frames = pipeline.wait_for_frames(100)
if frames is not None:
color_frame = frames.get_color_frame()
if color_frame is not None:
print(f"尝试{attempt + 1}成功获取初始帧")
break
print(f"尝试{attempt + 1}：未获取到有效帧，重试...")
if frames is None:
print("错误：10次尝试后仍无法从相机获取帧")
sys.exit(-1)
color_frame = frames.get_color_frame()
if color_frame is None:
print("错误：无法获取彩色帧")
sys.exit(-1)
# 提取相机内参
intrinsics = get_gemini2_camera_intrinsics(color_profile)
# 打印相机内参信息
print(f"Gemini 2相机内参:")
print(f" 分辨率: {intrinsics['width']}x{intrinsics['height']}")
print(f" 焦距: ({intrinsics['fx']:.2f}, {intrinsics['fy']:.2f})")
print(f" 主点: ({intrinsics['cx']:.2f}, {intrinsics['cy']:.2f})")
return pipeline, config, intrinsics
def load_camera_config(config_path: str) -> dict:
"""
从YAML文件加载相机配置
Args:
config_path: 配置文件路径
Returns:
dict: 相机配置字典
Exceptions:
FileNotFoundError: 当配置文件不存在时抛出
"""
# 检查配置文件是否存在
if not os.path.exists(config_path):
raise FileNotFoundError(f"配置文件未找到: {config_path}")
# 读取并解析YAML配置文件
with open(config_path, 'r') as f:
config = yaml.safe_load(f)
return config
def apply_depth_to_color_transform(depth_data: np.ndarray, transform: dict) -> np.ndarray:
"""
应用深度相机到彩色相机变换（如果需要）
Args:
depth_data: 原始深度数据
transform: 深度到彩色变换参数
Returns:
np.ndarray: 变换后的深度数据
"""
# 目前Orbbec SDK已经处理了D2C对齐，所以这里主要是为了完整性
# 如果将来需要额外的变换处理，可以在这里实现
return depth_data
def validate_depth_color_alignment(color_image: np.ndarray, depth_data: np.ndarray) -> bool:
"""
验证深度和彩色图像对齐质量
Args:
color_image: 彩色图像
depth_data: 深度数据
Returns:
bool: 对齐质量是否良好
"""
# 检查图像大小是否匹配
if color_image.shape[:2] != depth_data.shape:
print(f"警告：深度和彩色图像大小不匹配 - 彩色: {color_image.shape[:2]}, 深度: {depth_data.shape}")
return False
# 检查深度数据的有效性
valid_depth_ratio = np.sum(depth_data > 0) / depth_data.size
if valid_depth_ratio < 0.3: # 如果有效深度点少于30%
print(f"警告：有效深度点比例过低: {valid_depth_ratio:.2%}")
return False
return True
def enhance_depth_quality(depth_data: np.ndarray, fast_mode: bool = True) -> np.ndarray:
"""
增强深度数据质量
Args:
depth_data: 原始深度数据
fast_mode: 快速模式（使用更小的滤波核以提高性能）
Returns:
np.ndarray: 增强后的深度数据
"""
if fast_mode:
# 快速模式：只使用3x3中值滤波，显著提高性能
depth_enhanced = cv2.medianBlur(depth_data.astype(np.uint16), 3)
else:
# 完整模式：使用更大的滤波核，质量更好但速度较慢
# 应用中值滤波去除噪声
depth_enhanced = cv2.medianBlur(depth_data.astype(np.uint16), 5)
# 对于深度数据，使用高斯滤波而不是双边滤波（因为双边滤波不支持uint16）
# 首先转换为float32进行滤波，然后转换回uint16
depth_float = depth_enhanced.astype(np.float32)
depth_filtered = cv2.GaussianBlur(depth_float, (5, 5), 1.0)
depth_enhanced = depth_filtered.astype(np.uint16)
return depth_enhanced
def main() -> None:
"""
功能：
1. 解析命令行参数
2. 设置相机管道
3. 初始化SLAM跟踪器
4. 运行实时跟踪循环
5. 保存轨迹数据
"""
# 创建命令行参数解析器
parser = argparse.ArgumentParser(description='Orbbec Gemini 2 RGBD视觉SLAM')
# 添加命令行参数
parser.add_argument(
'--config',
type=str,
default=None,
help='相机配置YAML文件路径（如果提供则使用标定参数）'
)
parser.add_argument(
'--undistort',
action='store_true',
help='使用标定参数进行畸变校正'
)
parser.add_argument(
'--no-viz',
action='store_true',
help='禁用可视化（当Rerun服务器不可用时有用）'
)
parser.add_argument(
'--enable-distortion',
action='store_true',
help='启用畸变校正（使用标定文件中的畸变系数）'
)
parser.add_argument(
'--enhance-depth',
action='store_true',
help='启用深度数据质量增强（滤波和去噪）'
)
parser.add_argument(
'--opencv-viz',
action='store_true',
help='使用OpenCV可视化器（适用于嵌入式GPU）'
)
parser.add_argument(
'--viz-skip-frames',
type=int,
default=1,
help='可视化跳帧（例如：2表示每隔一帧可视化以提高性能）'
)
parser.add_argument(
'--fast-depth',
action='store_true',
help='使用快速深度增强模式（3x3滤波而不是5x5+高斯，提高性能）'
)
parser.add_argument(
'--disable-observations',
action='store_true',
help='禁用观测导出（最大化性能，但可视化不会显示特征点）'
)
parser.add_argument(
'--resolution',
type=str,
default=None,
help='相机分辨率（格式：WIDTHxHEIGHT，例如：640x480, 1280x720）。常用：640x480（最快）, 1280x720（平衡）, 1920x1080（默认）'
)
parser.add_argument(
'--list-resolutions',
action='store_true',
help='列出所有支持的分辨率并退出'
)
parser.add_argument(
'--use-hardware-timestamp',
action='store_true',
help='使用相机硬件时间戳而不是系统时间戳（提高时间精度）'
)
parser.add_argument(
'--diagnose-timestamps',
action='store_true',
help='启用时间戳诊断模式（显示详细的帧间隔统计）'
)
parser.add_argument(
'--camera-timeout',
type=int,
default=FRAME_WAIT_TIMEOUT_MS,
help=f'相机帧等待超时（毫秒），默认: {FRAME_WAIT_TIMEOUT_MS}ms'
)
parser.add_argument(
'--single-window',
action='store_true',
help='使用单窗口模式（将3个视图合并为1个窗口，减少窗口数量）'
)
parser.add_argument(
'--detect-stationary',
action='store_true',
help='启用静止检测（当相机静止时抑制位姿更新，减少漂移）'
)
# 解析命令行参数
args = parser.parse_args()
# 如果用户想要列出所有支持的分辨率
if args.list_resolutions:
print("查询支持的分辨率...")
try:
from pyorbbecsdk import Pipeline, OBSensorType, OBFormat
pipeline = Pipeline()
color_profile_list = pipeline.get_stream_profile_list(OBSensorType.COLOR_SENSOR)
print("\n支持的RGB分辨率:")
print("-" * 40)
resolutions = []
for cp in color_profile_list:
if cp.get_format() == OBFormat.RGB:
width = cp.get_width()
height = cp.get_height()
res_str = f"{width}x{height}"
if res_str not in resolutions:
resolutions.append(res_str)
# 添加性能建议
if width <= 640:
perf = "🚀 最快"
elif width <= 1280:
perf = "⚡ 快速"
elif width <= 1920:
perf = "⚖️ 平衡"
else:
perf = "🐢 较慢"
print(f" {res_str:15s} {perf}")
print("-" * 40)
print(f"\n用法: --resolution WIDTHxHEIGHT")
print(f"示例: python {sys.argv[0]} --resolution 640x480")
except Exception as e:
print(f"错误：无法查询分辨率 - {e}")
sys.exit(0)
# 解析分辨率参数
target_width = None
target_height = None
if args.resolution:
try:
width_str, height_str = args.resolution.split('x')
target_width = int(width_str)
target_height = int(height_str)
print(f"将使用分辨率: {target_width}x{target_height}")
except ValueError:
print(f"错误：无效的分辨率格式 '{args.resolution}'")
print(f"正确格式: WIDTHxHEIGHT（例如: 640x480）")
sys.exit(-1)
# 打印程序标题
print("="*60)
print("Orbbec Gemini 2 RGBD视觉SLAM")
print("="*60)
# 尝试设置相机管道（带重试机制）
pipeline = None
config = None
intrinsics = None
# 最多尝试3次设置相机管道
for attempt in range(3):
try:
print(f"尝试设置相机管道（尝试{attempt + 1}/3）...")
pipeline, config, intrinsics = setup_gemini2_pipeline(target_width, target_height)
print("✅ 相机管道设置成功！")
break
except Exception as e:
print(f"❌ 尝试{attempt + 1}失败: {e}")
if attempt < 2:
print("2秒后重试...")
time.sleep(2)
else:
print("所有尝试都失败了。请检查：")
print("1. 相机已正确连接")
print("2. 没有其他应用程序正在使用相机")
print("3. USB权限正确")
sys.exit(-1)
# 如果提供了配置文件，加载标定配置
calibrated_intrinsics = None
distortion_coeffs = None
depth_to_color_transform = None
if args.config:
print(f"从路径加载标定配置: {args.config}")
calib_config = load_camera_config(args.config)
# 使用标定值覆盖内参
calibrated_intrinsics = {
'fx': calib_config['camera_matrix']['fx'], # 标定的X方向焦距
'fy': calib_config['camera_matrix']['fy'], # 标定的Y方向焦距
'cx': calib_config['camera_matrix']['cx'], # 标定的X方向主点
'cy': calib_config['camera_matrix']['cy'], # 标定的Y方向主点
'width': calib_config['image']['width'], # 标定时的图像宽度
'height': calib_config['image']['height'] # 标定时的图像高度
}
# 加载畸变系数（仅在启用畸变校正时）
if args.enable_distortion and 'distortion_coefficients' in calib_config:
distortion_coeffs = [
calib_config['distortion_coefficients']['k1'],
calib_config['distortion_coefficients']['k2'],
calib_config['distortion_coefficients']['p1'],
calib_config['distortion_coefficients']['p2'],
calib_config['distortion_coefficients']['k3']
]
print(f"畸变校正已启用")
else:
distortion_coeffs = None
if not args.enable_distortion:
print(f"畸变校正已禁用（使用 --enable-distortion 启用）")
# 加载深度相机到彩色相机变换参数
if 'depth_to_color_transform' in calib_config:
depth_to_color_transform = calib_config['depth_to_color_transform']
print(f"使用标定内参:")
print(f" 分辨率: {calibrated_intrinsics['width']}x{calibrated_intrinsics['height']}")
print(f" 焦距: ({calibrated_intrinsics['fx']:.2f}, {calibrated_intrinsics['fy']:.2f})")
print(f" 主点: ({calibrated_intrinsics['cx']:.2f}, {calibrated_intrinsics['cy']:.2f})")
if distortion_coeffs:
print(f" 畸变系数: {distortion_coeffs}")
if depth_to_color_transform:
print(f" 深度-彩色变换: 已加载")
# 创建相机Rig（如果有标定内参则使用标定值）
# 但保持实际相机分辨率用于图像处理
if calibrated_intrinsics:
# 使用标定内参但保持实际相机分辨率
final_intrinsics = calibrated_intrinsics.copy()
final_intrinsics['width'] = intrinsics['width'] # 使用实际相机宽度
final_intrinsics['height'] = intrinsics['height'] # 使用实际相机高度
# 按比例缩放焦距和主点
scale_x = intrinsics['width'] / calibrated_intrinsics['width']
scale_y = intrinsics['height'] / calibrated_intrinsics['height']
final_intrinsics['fx'] *= scale_x # 缩放X方向焦距
final_intrinsics['fy'] *= scale_y # 缩放Y方向焦距
final_intrinsics['cx'] *= scale_x # 缩放X方向主点
final_intrinsics['cy'] *= scale_y # 缩放Y方向主点
print(f"内参已缩放到实际分辨率:")
print(f" 分辨率: {final_intrinsics['width']}x{final_intrinsics['height']}")
print(f" 焦距: ({final_intrinsics['fx']:.2f}, {final_intrinsics['fy']:.2f})")
print(f" 主点: ({final_intrinsics['cx']:.2f}, {final_intrinsics['cy']:.2f})")
# 创建Rig，使用标定内参和畸变系数
rig = create_gemini2_rig(final_intrinsics, distortion_coeffs)
else:
# 如果没有标定文件，使用相机默认内参
rig = create_gemini2_rig(intrinsics)
# 配置RGBD设置
rgbd_settings = vslam.Tracker.OdometryRGBDSettings()
rgbd_settings.depth_scale_factor = DEPTH_SCALE_FACTOR # 将毫米转换为米
rgbd_settings.depth_camera_id = 0 # 深度相机ID（第一个相机）
rgbd_settings.enable_depth_stereo_tracking = False # 禁用深度立体跟踪
# 如果存在深度到彩色变换参数，可以在这里应用
if depth_to_color_transform:
print("深度-彩色变换参数已加载，将用于优化RGBD对齐")
# 配置跟踪器 - 使用支持的参数（优化性能）
cfg = vslam.Tracker.OdometryConfig(
async_sba=True, # 启用异步束调整（提高性能）
enable_final_landmarks_export=False, # 禁用最终地标导出（提高性能）
odometry_mode=vslam.Tracker.OdometryMode.RGBD, # 设置为RGBD里程计模式
rgbd_settings=rgbd_settings, # 应用RGBD设置
use_gpu=True, # 使用GPU加速
use_motion_model=True, # 使用运动模型（提高跟踪稳定性）
use_denoising=False, # 禁用去噪（我们有自己的深度增强，节省计算）
enable_observations_export=not args.disable_observations, # 观测导出（用于可视化特征点）
enable_landmarks_export=False # 禁用地标导出（提高性能）
)
# 初始化跟踪器和可视化器
tracker = vslam.Tracker(rig, cfg) # 创建SLAM跟踪器
# 创建可视化器（可选，支持多种可视化方案）
visualizer = None
if not args.no_viz:
if args.opencv_viz:
# 使用OpenCV可视化器（适用于嵌入式GPU）
try:
visualizer = OpenCVVisualizer("Gemini 2 RGBD SLAM", single_window=args.single_window)
print("✅ OpenCV可视化器初始化成功")
except Exception as e:
print(f"⚠️ OpenCV可视化器初始化失败: {e}")
print("继续运行，但无可视化界面...")
visualizer = None
else:
# 尝试使用RerunVisualizer
try:
visualizer = RerunVisualizer(num_viz_cameras=NUM_VIZ_CAMERAS)
print("✅ Rerun可视化器初始化成功")
except Exception as e:
print(f"⚠️ Rerun可视化器初始化失败: {e}")
print("尝试使用OpenCV可视化器...")
try:
visualizer = OpenCVVisualizer("Orbbec Gemini 2 RGBD SLAM")
print("✅ OpenCV可视化器初始化成功（自动回退）")
except Exception as e2:
print(f"⚠️ OpenCV可视化器也失败了: {e2}")
print("继续运行，但无可视化界面...")
visualizer = None
# 打印跟踪器初始化信息
print(f"\ncuVSLAM跟踪器已初始化，里程计模式: RGBD")
print(f"深度缩放因子: {DEPTH_SCALE_FACTOR}（毫米到米）")
# 打印性能优化配置
print(f"\n⚡ 性能配置:")
print(f" 分辨率: {intrinsics['width']}x{intrinsics['height']}")
print(f" 相机超时: {args.camera_timeout}ms")
print(f" 时间戳模式: {'硬件时间戳' if args.use_hardware_timestamp else '系统时间戳'}")
print(f" 时间戳诊断: {'启用（详细模式）' if args.diagnose_timestamps else '禁用'}")
print(f" 可视化跳帧: 每{args.viz_skip_frames}帧")
if args.enhance_depth:
depth_mode = "快速模式（3x3）" if args.fast_depth else "完整模式（5x5+高斯）"
print(f" 深度增强: 启用 [{depth_mode}]")
else:
print(f" 深度增强: 禁用")
print(f" 畸变校正: {'启用' if (args.enable_distortion and distortion_coeffs) else '禁用'}")
if args.opencv_viz:
viz_mode = "单窗口" if args.single_window else "3窗口"
print(f" 可视化器: OpenCV ({viz_mode})")
else:
print(f" 可视化器: Rerun（默认）")
print(f" 观测导出: {'禁用（最大性能）' if args.disable_observations else '启用'}")
print(f" 静止检测: {'启用（抑制漂移）' if args.detect_stationary else '禁用'}")
if args.viz_skip_frames == 1 and not args.disable_observations and args.resolution is None:
print(f"\n💡 性能优化提示:")
print(f" 如果帧率较低，请尝试以下选项（从低到高影响）:")
print(f" --resolution 640x480 # 降低分辨率（最大改进！）")
print(f" --viz-skip-frames 3 # 每3帧可视化一次（轻微改进）")
print(f" --fast-depth # 使用快速深度增强（中等改进）")
print(f" --opencv-viz # 使用OpenCV可视化（中等改进）")
print(f" --disable-observations # 禁用特征点导出（显著改进）")
print(f" --no-viz # 完全禁用可视化（最大改进）")
print(f"\n 💡 使用 --list-resolutions 查看所有支持的分辨率")
# 跟踪变量初始化
frame_id = 0 # 帧计数器
prev_timestamp: Optional[int] = None # 前一帧时间戳
trajectory: List[np.ndarray] = [] # 轨迹点列表（仅位置）
pose_data: List[dict] = [] # 完整位姿数据列表（位置+旋转）
start_time = time.time() # 开始时间
frame_drop_warnings = 0 # 帧丢失警告计数器
# 时间戳诊断统计
timestamp_intervals = [] # 时间戳间隔列表（用于统计）
hardware_timestamp_base = None # 硬件时间戳基线
last_frame_time = time.time() # 前一帧系统时间（用于计算实际FPS）
# 静止检测
stationary_threshold = 0.001 # 1mm位置变化阈值
stationary_count = 0 # 连续静止帧数
last_position = None # 前一帧位置
is_stationary = False # 当前静止
# 打印开始信息和使用技巧
print("\n" + "="*60)
print("开始RGBD SLAM...")
print("="*60)
print("\n💡 更好的跟踪效果技巧:")
print(" 1. 确保彩色相机有良好的光照条件")
print(" 2. 避免反射表面影响深度感知")
print(" 3. 缓慢平稳地移动相机")
print(" 4. 将物体保持在0.5-5米范围内以获得最佳深度质量")
print("\n按Ctrl+C停止并保存轨迹\n")
try:
# 主跟踪循环
while True:
# 记录帧获取开始时间
frame_acquire_start = time.time()
# 等待帧数据（使用配置的超时时间）
frames = pipeline.wait_for_frames(args.camera_timeout)
if frames is None:
continue # 如果未获取到帧，继续下一循环
# 获取彩色帧和深度帧
color_frame = frames.get_color_frame()
depth_frame = frames.get_depth_frame()
# 如果任何帧为空，跳过此帧（静默跳过，类似于test_camera.py）
if color_frame is None or depth_frame is None:
continue
# 将帧转换为numpy数组（与test_camera.py相同的方法）
color_image = simple_frame_to_bgr(color_frame)
if color_image is None:
continue # 如果转换失败，静默跳过此帧
# 获取深度数据（与test_camera.py相同的方法）
depth_height = depth_frame.get_height() # 深度图像高度
depth_width = depth_frame.get_width() # 深度图像宽度
depth_data = np.frombuffer(depth_frame.get_data(), dtype=np.uint16).reshape((depth_height, depth_width))
# 验证深度-彩色对齐质量
if not validate_depth_color_alignment(color_image, depth_data):
continue # 如果对齐质量差，跳过此帧
# 增强深度数据质量（如果启用）
if args.enhance_depth:
depth_data = enhance_depth_quality(depth_data, fast_mode=args.fast_depth)
# 应用深度到彩色变换（如果配置）
if depth_to_color_transform:
depth_data = apply_depth_to_color_transform(depth_data, depth_to_color_transform)
# 生成时间戳（纳秒）
if args.use_hardware_timestamp:
# 尝试使用相机硬件时间戳
try:
# 获取彩色帧硬件时间戳（微秒）
hw_timestamp_us = color_frame.get_timestamp()
# 初始化硬件时间戳基线
if hardware_timestamp_base is None:
hardware_timestamp_base = hw_timestamp_us
# 转换为相对时间戳（纳秒）
timestamp_ns = int((hw_timestamp_us - hardware_timestamp_base) * 1000)
except Exception as e:
# 如果硬件时间戳不可用，回退到系统时间
if frame_id == 0:
print(f"⚠️ 硬件时间戳不可用，使用系统时间: {e}")
timestamp_ns = int((time.time() - start_time) * 1e9)
else:
# 使用系统时间戳
timestamp_ns = int((time.time() - start_time) * 1e9)
# 计算实际帧间隔（用于FPS统计）
current_frame_time = time.time()
actual_frame_interval_ms = (current_frame_time - last_frame_time) * 1000
last_frame_time = current_frame_time
# 检查与前一帧的时间戳差异
if prev_timestamp is not None:
timestamp_diff = timestamp_ns - prev_timestamp
timestamp_intervals.append(timestamp_diff / 1e6) # 保存间隔（毫秒）
# 诊断模式：显示详细信息
if args.diagnose_timestamps and frame_id > WARMUP_FRAMES:
print(f"[帧 {frame_id}] 时间戳间隔: {timestamp_diff/1e6:.2f}ms, "
f"实际间隔: {actual_frame_interval_ms:.2f}ms, "
f"实时FPS: {1000/actual_frame_interval_ms:.1f}")
# 正常模式：仅在超过阈值时警告
if timestamp_diff > IMAGE_JITTER_THRESHOLD_MS:
frame_drop_warnings += 1
# 仅每10次警告显示一次以减少信息冗余
if frame_drop_warnings % 10 == 1:
print(
f"⚠️ 时间戳间隔过大: {timestamp_diff/1e6:.2f} ms "
f"(阈值: {IMAGE_JITTER_THRESHOLD_MS/1e6:.2f} ms) "
f"[实际FPS: {1000/actual_frame_interval_ms:.1f}] "
f"(#{frame_drop_warnings} 次)"
)
frame_id += 1 # 增加帧计数器
# 预热指定数量的帧
if frame_id > WARMUP_FRAMES:
# 为跟踪准备图像
images = [color_image] # 彩色图像列表
depths = [depth_data] # 深度图像列表
# 跟踪当前帧
odom_pose_estimate, _ = tracker.track(
timestamp_ns, images=images, depths=depths
)
# 检查跟踪是否成功
if odom_pose_estimate.world_from_rig is None:
print(f"警告：跟踪帧 {frame_id} 失败")
continue
# 获取当前位姿和观测数据
odom_pose = odom_pose_estimate.world_from_rig.pose
current_position = np.array(odom_pose.translation)
# 静止检测
if args.detect_stationary and last_position is not None:
position_change = np.linalg.norm(current_position - last_position)
if position_change < stationary_threshold:
stationary_count += 1
if stationary_count > 30: # 连续30帧静止
is_stationary = True
else:
stationary_count = 0
is_stationary = False
# 如果检测到静止，使用第一个位置（抑制漂移）
if is_stationary and len(trajectory) > 0:
# 使用最近的稳定位置而不是漂移位置
current_position = last_position
last_position = current_position.copy()
trajectory.append(current_position) # 将位置添加到轨迹
# 存储完整位姿数据
pose_data.append({
'frame_id': frame_id, # 帧ID
'timestamp': timestamp_ns, # 时间戳
'position': current_position, # 位置 [x, y, z]
'rotation_quat': odom_pose.rotation, # 旋转四元数 [w, x, y, z]
'stationary': is_stationary if args.detect_stationary else False # 静止标志
})
# 提取位置和旋转信息
position = odom_pose.translation # 位置向量 [x, y, z]
rotation_quat = odom_pose.rotation # 四元数 [w, x, y, z]
# 将四元数转换为欧拉角（Roll, pitch, yaw）
import math
w, x, y, z = rotation_quat # 四元数分量
# Roll（绕X轴旋转）
sinr_cosp = 2 * (w * x + y * z)
cosr_cosp = 1 - 2 * (x * x + y * y)
roll = math.atan2(sinr_cosp, cosr_cosp)
# Pitch（绕Y轴旋转）
sinp = 2 * (w * y - z * x)
if abs(sinp) >= 1:
pitch = math.copysign(math.pi / 2, sinp) # 如果超出范围，使用90度
else:
pitch = math.asin(sinp)
# Yaw（绕Z轴旋转）
siny_cosp = 2 * (w * z + x * y)
cosy_cosp = 1 - 2 * (y * y + z * z)
yaw = math.atan2(siny_cosp, cosy_cosp)
# 转换为度
roll_deg = math.degrees(roll) # Roll角（度）
pitch_deg = math.degrees(pitch) # Pitch角（度）
yaw_deg = math.degrees(yaw) # Yaw角（度）
# 获取用于可视化的观测数据（如果启用了观测导出）
observations = [] if args.disable_observations else tracker.get_last_observations(0)
# 存储当前时间戳用于下一次迭代
prev_timestamp = timestamp_ns
# 可视化结果（如果启用，支持多种可视化器）
# 使用帧跳过减少可视化开销以提高性能
if visualizer is not None and frame_id % args.viz_skip_frames == 0:
try:
if isinstance(visualizer, OpenCVVisualizer):
# OpenCV可视化器调用
visualizer.visualize_frame(
frame_id=frame_id,
color_image=images[0],
depth_image=depth_data,
pose=odom_pose,
observations=observations,
trajectory=trajectory
)
else:
# Rerun可视化器调用（与run_rgbd.py一致）
# 对于RGBD，我们只有一个相机，所以复制图像和观测数据
# 为第二个视图创建深度可视化
depth_vis = create_depth_visualization(depth_data)
visualizer.visualize_frame(
frame_id=frame_id, # 帧ID
images=[images[0], depth_vis], # 彩色图像和深度可视化
pose=odom_pose, # 当前位姿
observations_main_cam=[observations, observations], # 主相机观测数据
trajectory=trajectory, # 轨迹
timestamp=timestamp_ns # 时间戳
)
except Exception as e:
# 如果可视化失败，静默继续运行
if frame_id % 100 == 0: # 每100帧打印一次警告
print(f"⚠️ 可视化错误: {e}")
# 每60帧显示状态（减少打印频率以提高性能）
if frame_id % 60 == 0:
elapsed = time.time() - start_time # 经过时间
fps = frame_id / elapsed if elapsed > 0 else 0 # 计算FPS
num_features = len(observations) # 特征点数量
# 特征质量指示器
feature_status = "🔴 低" if num_features < 30 else "🟡 正常" if num_features < 80 else "🟢 良好"
# 静止状态指示器
motion_status = "🛑 静止" if is_stationary else "🚀 移动"
# 打印详细状态信息
status_line = f"📊 帧 {frame_id}: {num_features} 特征点 {feature_status}, {fps:.1f} FPS"
if args.detect_stationary:
status_line += f" | {motion_status}"
print(status_line)
print(f" 📍 位置 (XYZ): [{position[0]:.3f}, {position[1]:.3f}, {position[2]:.3f}] 米")
print(f" 🔄 旋转 (RPY): Roll={roll_deg:.1f}°, Pitch={pitch_deg:.1f}°, Yaw={yaw_deg:.1f}°")
print(f" 🧭 四元数: w={w:.3f}, x={x:.3f}, y={y:.3f}, z={z:.3f}")
print()
else:
# 预热期间，只显示进度
if frame_id % 10 == 0:
print(f"⏳ 预热中... 帧 {frame_id}/{WARMUP_FRAMES}")
except KeyboardInterrupt:
print("\n用户中断程序")
finally:
# 清理和总结
print("\n" + "="*60)
print("RGBD SLAM会话总结")
print("="*60)
print(f"总处理帧数: {frame_id}")
print(f"成功跟踪: {len(trajectory)} 个位姿")
print(f"帧丢失警告: {frame_drop_warnings}")
if frame_id > WARMUP_FRAMES:
success_rate = (len(trajectory) / (frame_id - WARMUP_FRAMES)) * 100
print(f"成功率: {success_rate:.1f}%")
# 时间戳统计
if len(timestamp_intervals) > 0:
import statistics
avg_interval = statistics.mean(timestamp_intervals)
min_interval = min(timestamp_intervals)
max_interval = max(timestamp_intervals)
median_interval = statistics.median(timestamp_intervals)
stdev_interval = statistics.stdev(timestamp_intervals) if len(timestamp_intervals) > 1 else 0
print(f"\n📊 时间戳间隔统计:")
print(f" 平均间隔: {avg_interval:.2f} ms ({1000/avg_interval:.1f} FPS)")
print(f" 中位间隔: {median_interval:.2f} ms ({1000/median_interval:.1f} FPS)")
print(f" 最小间隔: {min_interval:.2f} ms ({1000/min_interval:.1f} FPS)")
print(f" 最大间隔: {max_interval:.2f} ms ({1000/max_interval:.1f} FPS)")
print(f" 标准差: {stdev_interval:.2f} ms")
print(f" 间隔抖动: {(stdev_interval/avg_interval*100):.1f}%")
# 分析问题
if avg_interval > 100:
print(f"\n⚠️ 时间戳分析:")
print(f" 平均帧间隔 ({avg_interval:.1f}ms) 较大，可能原因：")
print(f" 1. 处理速度慢（尝试降低分辨率 --resolution 640x480）")
print(f" 2. 相机帧率低（检查相机配置）")
print(f" 3. CPU/GPU负载高（关闭其他程序）")
if stdev_interval / avg_interval > 0.3:
print(f"\n⚠️ 时间戳抖动较大 ({(stdev_interval/avg_interval*100):.1f}%)，可能原因：")
print(f" 1. 系统负载不稳定")
print(f" 2. USB带宽不足")
print(f" 3. 可视化开销大（尝试 --viz-skip-frames 或 --no-viz）")
if args.use_hardware_timestamp:
print(f"\n✅ 使用了硬件时间戳")
else:
print(f"\n💡 提示：使用 --use-hardware-timestamp 可能提高时间精度")
# 保存轨迹和位姿数据
if len(trajectory) > 0:
# 保存简单轨迹（仅位置）
trajectory_array = np.array(trajectory)
np.savetxt('trajectory_gemini2_rgbd.txt', trajectory_array,
fmt='%.6f', delimiter=',',
header='x,y,z (米)')
# 保存完整位姿数据（位置+旋转）
with open('pose_data_gemini2_rgbd.txt', 'w') as f:
f.write('# Frame_ID, Timestamp(ns), X(m), Y(m), Z(m), Qw, Qx, Qy, Qz\n')
for pose in pose_data:
pos = pose['position']
quat = pose['rotation_quat']
f.write(f"{pose['frame_id']}, {pose['timestamp']}, "
f"{pos[0]:.6f}, {pos[1]:.6f}, {pos[2]:.6f}, "
f"{quat[0]:.6f}, {quat[1]:.6f}, {quat[2]:.6f}, {quat[3]:.6f}\n")
print(f"\n✅ 数据已保存:")
print(f" 📍 轨迹: trajectory_gemini2_rgbd.txt ({len(trajectory)} 个位姿)")
print(f" 🎯 完整位姿: pose_data_gemini2_rgbd.txt ({len(pose_data)} 个位姿)")
# 计算轨迹统计
if len(trajectory) > 1:
distances = np.diff(trajectory_array, axis=0)
total_distance = np.sum(np.linalg.norm(distances, axis=1))
print(f" 📏 总行驶距离: {total_distance:.2f} 米")
else:
print("\n⚠️ 无轨迹数据可保存")
# 停止相机管道和可视化器
try:
pipeline.stop()
print("\n相机已释放，程序退出...")
except Exception as e:
print(f"\n警告：停止相机管道时出错: {e}")
finally:
# 关闭可视化器
if visualizer is not None and hasattr(visualizer, 'close'):
try:
visualizer.close()
except Exception as e:
print(f"关闭可视化器时出错: {e}")
print("="*60)
# 程序入口点
if __name__ == "__main__":
main()
python rgbd_slam.py --config examples/gemini2_calibrated_config.yaml --resolution 1280x720 --enable-distortion --enhance-depth
yaml文件如下：
gemini2_calibrated_config.yaml# Gemini 2 相机标定配置文件
image:
width: 1280
height: 720
# 相机内参矩阵 (更新的标定参数)
# [fx 0 cx]
# [ 0 fy cy]
# [ 0 0 1]
camera_matrix:
fx: 690.546721
fy: 683.586452
cx: 684.064868
cy: 370.939099
# 畸变系数 (Brown-Conrady模型) - 更新的标定参数
# [k1, k2, p1, p2, k3]
distortion_coefficients:
k1: -0.010482
k2: -0.019797
p1: 0.001294
p2: 0.021572
k3: 0.000000
# 投影矩阵 (仅供参考)
projection_matrix:
- [675.994629, 0.000000, 740.107685, 0.000000]
- [0.000000, 698.293884, 396.362314, 0.000000]
- [0.000000, 0.000000, 1.000000, 0.000000]
# 深度相机到彩色相机变换参数 (来自ROS2话题)
# 用于RGBD SLAM中的深度-彩色对齐
depth_to_color_transform:
# 旋转矩阵 (3x3)
rotation:
- [0.9999980330467224, 0.0005175529513508081, 0.0019138390198349953]
- [-0.0005151802906766534, 0.9999991059303284, -0.0012400292325764894]
- [-0.0019144790712743998, 0.001239040750078857, 0.9999973773956299]
# 平移向量 (3x1)
translation:
- -0.013858354568481446
- 0.0001548745185136795
- -0.00187313711643219
相关资源
立足具身智能前沿赛道，致力于搭建全球化、开源化、全栈式技术交流与实践共创平台。
更多推荐
4
0- 0
已为社区贡献1条内容
所有评论(0)