115 lines
3.5 KiB
Python
115 lines
3.5 KiB
Python
"""
|
|
IMU 3D 轨迹实时可视化
|
|
|
|
matplotlib 3D 窗口, 30Hz 刷新, 显示:
|
|
- 蓝色轨迹线
|
|
- 当前点红点
|
|
- 原点坐标系指示
|
|
- 等比例坐标轴
|
|
"""
|
|
|
|
import numpy as np
|
|
import matplotlib.pyplot as plt
|
|
from matplotlib.animation import FuncAnimation
|
|
|
|
|
|
class TrajectoryViewer:
|
|
"""3D 轨迹实时显示窗口"""
|
|
|
|
def __init__(self, title="IMU 3D Odometry", refresh_interval=33):
|
|
"""
|
|
Args:
|
|
title: 窗口标题
|
|
refresh_interval: 刷新间隔 (ms), 默认 33ms ≈ 30Hz
|
|
"""
|
|
self.refresh_interval = refresh_interval
|
|
|
|
self.fig = plt.figure(figsize=(8, 7))
|
|
self.fig.canvas.manager.set_window_title(title)
|
|
self.ax = self.fig.add_subplot(111, projection='3d')
|
|
|
|
# 轨迹线 (蓝色)
|
|
self.traj_line, = self.ax.plot([], [], [], 'b-', linewidth=1.0, label='Trajectory')
|
|
|
|
# 当前点 (红色)
|
|
self.current_point, = self.ax.plot([], [], [], 'ro', markersize=6, label='Current')
|
|
|
|
# 坐标系指示 (原点处)
|
|
axis_len = 0.3
|
|
self.origin_axes = [
|
|
self.ax.quiver(0, 0, 0, axis_len, 0, 0, color='r', arrow_length_ratio=0.15, label='X'),
|
|
self.ax.quiver(0, 0, 0, 0, axis_len, 0, color='g', arrow_length_ratio=0.15, label='Y'),
|
|
self.ax.quiver(0, 0, 0, 0, 0, axis_len, color='b', arrow_length_ratio=0.15, label='Z'),
|
|
]
|
|
|
|
self._setup_axes()
|
|
|
|
# 动画
|
|
self.anim = FuncAnimation(self.fig, self._animate, interval=self.refresh_interval,
|
|
cache_frame_data=False, blit=False)
|
|
|
|
def _setup_axes(self):
|
|
"""初始化坐标轴"""
|
|
self.ax.set_xlabel('X (front)')
|
|
self.ax.set_ylabel('Y (left)')
|
|
self.ax.set_zlabel('Z (up)')
|
|
self.ax.set_title("IMU 3D Trajectory (Z-up)")
|
|
self.ax.legend(loc='upper left')
|
|
|
|
# 初始范围
|
|
self.ax.set_xlim([-1, 1])
|
|
self.ax.set_ylim([-1, 1])
|
|
self.ax.set_zlim([-1, 1])
|
|
|
|
try:
|
|
self.ax.set_box_aspect([1, 1, 1])
|
|
except NotImplementedError:
|
|
pass
|
|
|
|
self.ax.grid(True)
|
|
|
|
def _animate(self, frame):
|
|
"""动画帧回调 (不做任何事, 数据由外部 update 驱动)"""
|
|
pass # 通过 plt.pause 驱动, FuncAnimation 仅用于保持窗口响应
|
|
|
|
def update(self, history_array):
|
|
"""更新显示的轨迹数据
|
|
|
|
Args:
|
|
history_array: Nx3 numpy array, 位置历史
|
|
"""
|
|
if len(history_array) < 1:
|
|
return
|
|
|
|
x, y, z = history_array[:, 0], history_array[:, 1], history_array[:, 2]
|
|
|
|
# 更新轨迹线
|
|
self.traj_line.set_data(x, y)
|
|
self.traj_line.set_3d_properties(z)
|
|
|
|
# 更新当前点
|
|
self.current_point.set_data([x[-1]], [y[-1]])
|
|
self.current_point.set_3d_properties([z[-1]])
|
|
|
|
# 自适应坐标轴范围
|
|
self._auto_scale(x, y, z)
|
|
|
|
def _auto_scale(self, x, y, z):
|
|
"""根据数据自动调整坐标轴范围, 保持等比例"""
|
|
all_coords = np.concatenate([x, y, z])
|
|
margin = max(np.ptp(all_coords) * 0.2, 0.5)
|
|
mid = (all_coords.min() + all_coords.max()) / 2
|
|
half = (all_coords.max() - all_coords.min()) / 2 + margin
|
|
|
|
self.ax.set_xlim([mid - half, mid + half])
|
|
self.ax.set_ylim([mid - half, mid + half])
|
|
self.ax.set_zlim([mid - half, mid + half])
|
|
|
|
def show(self):
|
|
"""阻塞显示窗口"""
|
|
plt.show()
|
|
|
|
def close(self):
|
|
"""关闭窗口"""
|
|
plt.close(self.fig)
|