fast imu

visualize_3d.py

@@ -2,7 +2,7 @@
 IMU 3D 轨迹实时可视化
 
 matplotlib 3D 窗口, 30Hz 刷新, 显示:
-  - 蓝色轨迹线
+  - 蓝色轨迹线 (自动降采样, 最多 800 点)
   - 当前点红点
   - 原点坐标系指示
   - 等比例坐标轴
@@ -10,19 +10,19 @@ 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):
+    def __init__(self, title="IMU 3D Odometry", max_trail_points=800):
         """
         Args:
-            title:           窗口标题
-            refresh_interval: 刷新间隔 (ms), 默认 33ms ≈ 30Hz
+            title:            窗口标题
+            max_trail_points: 轨迹线最多显示点数 (降采样, 避免渲染卡顿)
         """
-        self.refresh_interval = refresh_interval
+        self.max_trail_points = max_trail_points
+        self._full_count = 0
 
         self.fig = plt.figure(figsize=(8, 7))
         self.fig.canvas.manager.set_window_title(title)
@@ -44,9 +44,9 @@ class TrajectoryViewer:
 
         self._setup_axes()
 
-        # 动画
-        self.anim = FuncAnimation(self.fig, self._animate, interval=self.refresh_interval,
-                                   cache_frame_data=False, blit=False)
+        # 显示非阻塞窗口
+        plt.show(block=False)
+        plt.pause(0.1)
 
     def _setup_axes(self):
         """初始化坐标轴"""
@@ -56,7 +56,6 @@ class TrajectoryViewer:
         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])
@@ -68,34 +67,41 @@ class TrajectoryViewer:
 
         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:
+        n = len(history_array)
+        if n < 1:
             return
 
-        x, y, z = history_array[:, 0], history_array[:, 1], history_array[:, 2]
+        # 降采样: 超过 max_trail_points 时取等间隔子集
+        if n > self.max_trail_points:
+            step = n // self.max_trail_points
+            indices = np.arange(0, n, step)
+            sampled = history_array[indices]
+        else:
+            sampled = history_array
+
+        x, y, z = sampled[:, 0], sampled[:, 1], sampled[:, 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]])
+        # 当前点
+        last = history_array[-1]
+        self.current_point.set_data([last[0]], [last[1]])
+        self.current_point.set_3d_properties([last[2]])
 
-        # 自适应坐标轴范围
-        self._auto_scale(x, y, z)
+        # 自适应坐标轴 (每 20 次完整更新做一次, 减少开销)
+        self._full_count += 1
+        if self._full_count % 5 == 0:
+            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
@@ -105,10 +111,6 @@ class TrajectoryViewer:
         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)