update imu

visualize_3d.py

@@ -2,9 +2,9 @@
 IMU 3D 轨迹实时可视化
 
 matplotlib 3D 窗口, 30Hz 刷新, 显示:
-  - 蓝色轨迹线 (自动降采样, 最多 800 点)
-  - 当前点红点
-  - 原点坐标系指示
+  - 蓝色轨迹线 (最近 800 点, Tracker 端已限长)
+  - 当前点红点 + 朝向指示线 (RGB = body XYZ)
+  - 原点世界坐标系指示
   - 等比例坐标轴
 """
 
@@ -15,15 +15,7 @@ import matplotlib.pyplot as plt
 class TrajectoryViewer:
     """3D 轨迹实时显示窗口"""
 
-    def __init__(self, title="IMU 3D Odometry", max_trail_points=800):
-        """
-        Args:
-            title:            窗口标题
-            max_trail_points: 轨迹线最多显示点数 (降采样, 避免渲染卡顿)
-        """
-        self.max_trail_points = max_trail_points
-        self._full_count = 0
-
+    def __init__(self, title="IMU 3D Odometry"):
         self.fig = plt.figure(figsize=(8, 7))
         self.fig.canvas.manager.set_window_title(title)
         self.ax = self.fig.add_subplot(111, projection='3d')
@@ -34,27 +26,28 @@ class TrajectoryViewer:
         # 当前点 (红色)
         self.current_point, = self.ax.plot([], [], [], 'ro', markersize=6, label='Current')
 
-        # 坐标系指示 (原点处)
+        # 朝向指示线 (body XYZ, 用 plot 可原地更新)
+        self.body_x_line, = self.ax.plot([], [], [], 'r-', linewidth=2.0, alpha=0.9)
+        self.body_y_line, = self.ax.plot([], [], [], 'g-', linewidth=2.0, alpha=0.9)
+        self.body_z_line, = self.ax.plot([], [], [], 'b-', linewidth=2.0, alpha=0.9)
+
+        # 世界坐标系指示 (固定在原点)
         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.ax.quiver(0, 0, 0, axis_len, 0, 0, color='r', arrow_length_ratio=0.15)
+        self.ax.quiver(0, 0, 0, 0, axis_len, 0, color='g', arrow_length_ratio=0.15)
+        self.ax.quiver(0, 0, 0, 0, 0, axis_len, color='b', arrow_length_ratio=0.15)
 
         self._setup_axes()
+        self._frame = 0
 
-        # 显示非阻塞窗口
         plt.show(block=False)
         plt.pause(0.1)
 
     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])
@@ -67,25 +60,18 @@ class TrajectoryViewer:
 
         self.ax.grid(True)
 
-    def update(self, history_array):
-        """更新显示的轨迹数据 (自动降采样)
+    def update(self, history_array, rotation_matrix=None):
+        """更新显示
 
         Args:
-            history_array: Nx3 numpy array, 位置历史
+            history_array:   Nx3 位置历史 (已由 Tracker 限长为 800)
+            rotation_matrix: 3x3 body→world 旋转矩阵 (None 则不显示朝向)
         """
         n = len(history_array)
         if n < 1:
             return
 
-        # 降采样: 超过 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]
+        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)
@@ -95,13 +81,40 @@ class TrajectoryViewer:
         self.current_point.set_data([last[0]], [last[1]])
         self.current_point.set_3d_properties([last[2]])
 
-        # 自适应坐标轴 (每 20 次完整更新做一次, 减少开销)
-        self._full_count += 1
-        if self._full_count % 5 == 0:
+        # 朝向 (原地更新, 不重建对象)
+        if rotation_matrix is not None:
+            self._update_orientation(last, rotation_matrix)
+        else:
+            self.body_x_line.set_data([], [])
+            self.body_x_line.set_3d_properties([])
+            self.body_y_line.set_data([], [])
+            self.body_y_line.set_3d_properties([])
+            self.body_z_line.set_data([], [])
+            self.body_z_line.set_3d_properties([])
+
+        # 坐标轴范围 (每 5 帧更新一次)
+        self._frame += 1
+        if self._frame % 8 == 0:
             self._auto_scale(x, y, z)
 
+    def _update_orientation(self, position, R):
+        """更新 body 系朝向指示线 (不重建对象)
+
+        Args:
+            position: (3,) 当前位置
+            R: 3x3 body→world 旋转矩阵
+        """
+        px, py, pz = position
+        length = 0.15
+
+        for line, col in [(self.body_x_line, 0),
+                          (self.body_y_line, 1),
+                          (self.body_z_line, 2)]:
+            d = R[:, col] * length
+            line.set_data([px, px + d[0]], [py, py + d[1]])
+            line.set_3d_properties([pz, pz + d[2]])
+
     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
@@ -112,5 +125,4 @@ class TrajectoryViewer:
         self.ax.set_zlim([mid - half, mid + half])
 
     def close(self):
-        """关闭窗口"""
         plt.close(self.fig)