用Intel RealSense T265+Python玩转视觉惯性里程计：一个简易的轨迹记录与可视化脚本

用Intel RealSense T265+Python实现视觉惯性里程计：从数据采集到3D轨迹可视化

视觉惯性里程计（VIO）技术正在重塑移动机器人和AR设备的定位方式。Intel RealSense T265作为一款集双目鱼眼摄像头与6轴IMU于一体的追踪模组，其开箱即用的VIO功能让开发者无需从头实现复杂的传感器融合算法。本文将展示如何用Python在15分钟内构建一个完整的轨迹记录系统——从传感器数据获取到3D轨迹可视化，全部代码不超过50行。

1. 环境配置与传感器初探

在开始编码前，我们需要搭建Python开发环境。推荐使用Anaconda创建独立环境以避免依赖冲突：

conda create -n realsense_env python=3.8 conda activate realsense_env pip install pyrealsense2 matplotlib numpy opencv-python

连接T265时需注意：

• 必须使用USB 3.0及以上接口（蓝色接口）
• 避免强光直射摄像头模组
• 初次使用建议先运行realsense-viewer验证设备状态

传感器数据特性对比：

提示：T265的IMU数据已与视觉数据进行出厂标定，这是实现高精度VIO的关键

2. 实时位姿数据采集

通过pyrealsense2库获取数据仅需几行代码。新建t265_tracker.py文件，写入以下核心逻辑：

import pyrealsense2 as rs pipeline = rs.pipeline() config = rs.config() config.enable_stream(rs.stream.pose) pipeline.start(config) try: while True: frames = pipeline.wait_for_frames() pose_frame = frames.get_pose_frame() if pose_frame: pose_data = pose_frame.get_pose_data() print(f"Position: {pose_data.translation}") finally: pipeline.stop()

这段代码会持续输出传感器的三维坐标（单位：米）。关键对象解析：

• pipeline: 数据流管理中枢
• config: 指定需要启用的数据流
• pose_data: 包含以下关键字段：
  • translation: (x,y,z)位置坐标
  • rotation: 四元数表示的姿态
  • velocity: 线速度矢量
  • acceleration: 线加速度矢量

3. 轨迹可视化实现

3.1 2D平面轨迹绘制

使用Matplotlib实现实时轨迹显示：

import matplotlib.pyplot as plt from collections import deque # 初始化轨迹容器 trajectory = deque(maxlen=1000) fig, ax = plt.subplots() ax.set_xlim(-5, 5) ax.set_ylim(-5, 5) line, = ax.plot([], [], 'b-') while True: # 获取pose数据代码同上... if pose_frame: pos = pose_data.translation trajectory.append((pos.x, pos.z)) # 使用XZ平面 # 更新绘图 line.set_data(*zip(*trajectory)) plt.pause(0.001)

这种实现方式适合地面机器人等主要在二维平面运动的场景。若需要更流畅的显示效果，可以考虑使用PyQtGraph库。

3.2 3D轨迹可视化进阶

对于无人机、AR眼镜等三维运动场景，我们需要升级到3D可视化：

from mpl_toolkits.mplot3d import Axes3D fig = plt.figure() ax = fig.add_subplot(111, projection='3d') ax.set_xlim3d(-3, 3) ax.set_ylim3d(-3, 3) ax.set_zlim3d(-3, 3) traj = [] while True: # 获取数据... traj.append((pos.x, pos.y, pos.z)) ax.cla() ax.plot3D(*zip(*traj), c='blue') plt.pause(0.01)

4. 数据持久化与回放分析

将轨迹数据保存为CSV便于后续分析：

import csv from datetime import datetime with open(f'trajectory_{datetime.now().strftime("%Y%m%d_%H%M")}.csv', 'w') as f: writer = csv.writer(f) writer.writerow(['timestamp', 'x', 'y', 'z', 'qx', 'qy', 'qz', 'qw']) while True: # 获取数据... writer.writerow([ pose_frame.timestamp, pos.x, pos.y, pos.z, pose_data.rotation.x, pose_data.rotation.y, pose_data.rotation.z, pose_data.rotation.w ]) f.flush() # 确保实时写入

回放分析时可结合Pandas进行数据处理：

import pandas as pd df = pd.read_csv('trajectory_20230815_1430.csv') df['velocity'] = (df[['x','y','z']].diff()**2).sum(axis=1)**0.5 / 0.01 # 估算瞬时速度

5. 实际应用中的性能调优

5.1 坐标系处理技巧

T265使用右手坐标系：

• +X：传感器右侧方向
• +Y：传感器下方方向
• +Z：传感器前方方向

转换到ROS等常用坐标系系的变换矩阵：

import numpy as np def t265_to_ros_transform(position, rotation): """ 将T265坐标系转换到ROS标准坐标系 """ transform = np.eye(4) transform[:3, 3] = [position.z, -position.x, -position.y] q = [rotation.w, rotation.z, -rotation.x, -rotation.y] # 四元数转旋转矩阵... return transform

5.2 漂移补偿策略

虽然T265内置VIO算法已相当稳健，但长时间运行仍可能产生漂移。可通过以下方式改善：

• 闭环检测：当传感器回到起始位置时，重置坐标系原点
• 外部参考：结合AprilTag等视觉标记进行校正
• 运动约束：对于地面机器人，可假设Z轴不变

if (np.linalg.norm([pos.x, pos.y, pos.z]) < 0.1 and len(trajectory) > 100): print("检测到可能闭环，重置原点") trajectory = [(p[0]-pos.x, p[1]-pos.y, p[2]-pos.z) for p in trajectory]

6. 扩展应用场景

这个基础框架可以轻松扩展到更多有趣的应用：

AR物体固定：将虚拟物体坐标与T265坐标系绑定

virtual_obj_pos = np.array([1, 0, 0]) # 初始位置 current_pose = get_current_pose() # 获取当前传感器位姿 # 计算物体在当前视角下的坐标 obj_in_camera = current_pose.inverse() @ virtual_obj_pos

多设备同步：使用硬件触发同步多个T265

config.enable_device('012345678901') # 指定设备序列号 config.enable_stream(rs.stream.pose, rs.format.six_dof, 30) config.set_option(rs.option.inter_cam_sync_mode, 1) # 主模式

在最近的一个室内无人机项目中，我们使用T265配合这个Python脚本快速验证了飞行控制算法的可行性。相比复杂的SLAM系统，这种轻量级方案让迭代速度提升了3倍以上。当需要部署到生产环境时，只需将Python原型用C++重写关键性能模块即可。