feat: add error graphic

autopilot.py

@@ -3,6 +3,7 @@ import cv2
 import numpy as np
 from PIL import Image
 import math
+from visualization import VisualizationManager
 
 random.seed(1)
 
@@ -10,6 +11,7 @@ class Pilot:
     def __init__(self): pass
     def handle(self, image: Image): pass
     def act(self) -> tuple[float, float] | None: pass
+    def get_position(self) -> tuple[float, float]: pass
 
 class AutoPilot(Pilot):
     prev_image: np.ndarray | None
@@ -20,7 +22,7 @@ class AutoPilot(Pilot):
     bf_matcher: cv2.BFMatcher
     frame_count: int
     image_center: tuple  # Центр изображения (x, y)
-    viz_manager: object  # Менеджер визуализации (опционально)
+    viz_manager: VisualizationManager  # Менеджер визуализации (опционально)
 
     def __init__(self, viz_manager=None):
         self.prev_image = None
@@ -46,6 +48,9 @@ class AutoPilot(Pilot):
         # Инициализация матчера для сопоставления ключевых точек
         self.bf_matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
 
+    def get_position(self):
+        return self.x, self.y
+
     def image_to_numpy(self, image: Image.Image) -> np.ndarray:
         """Конвертирует PIL Image в numpy array для OpenCV"""
         return np.array(image)
@@ -255,6 +260,7 @@ class AutoPilot(Pilot):
                 
                 # Выводим текущее состояние БПЛА
                 drone_state = self.get_drone_state()
+                self.viz_manager.update_drone_trajectory(drone_state['x'], drone_state['y'])
                 print(f"  [Pilot] Drone Position: ({drone_state['x']:.2f}, {drone_state['y']:.2f})")
                 print(f"  [Pilot] Angle: {drone_state['angle_degrees']:.1f}°")
                 
@@ -285,7 +291,7 @@ class AutoPilot(Pilot):
         distance_to_target = math.sqrt(self.x**2 + self.y**2)
         
         # Если дрон находится рядом с целью, останавливаемся
-        if distance_to_target < 10.0:
+        if distance_to_target < 30.0:
             return None
         
         # Вычисляем угол к цели
@@ -320,7 +326,7 @@ class RandomPilot(Pilot):
 
     def act(self) -> tuple[float, float] | None:
         self.counter += 1
-        if self.counter > 40:
+        if self.counter > 300:
             return None
         
         return 1 / (self.counter + 20), 10.0

simulator.py

@@ -127,6 +127,8 @@ class Simulator:
                 break
 
             dangle, velocity = signal
+            drone_x, drone_y = self.autonomePilot.get_position()
+            self.viz_manager.update_error_plot(i, drone_x, drone_y, self.current_x, self.current_y)
 
             # Сдвиг камеры
             action = ActionChains(self.driver)
@@ -154,14 +156,9 @@ class Simulator:
             # Передаем изображение в AutoPilot для анализа
             self.autonomePilot.handle(rotated_im)
             
-            rotated_im.save(f"./images/{i}.png")
-            
             # Обновляем визуализацию каждые несколько итераций для производительности
-            if i % 5 == 0:
+            self.update_map()
-                self.update_map()
+            self.viz_manager.update_display()
-                self.viz_manager.update_display()
-            
-            self.viz_manager.pause(0.25)
 
         # Финальное обновление карты
         self.update_map()

visualization.py

@@ -27,7 +27,7 @@ class VisualizationManager:
     def __init__(self, window_title="Drone Autopilot Visualization"):
         self.window_title = window_title
         self.fig = None
-        self.ax_drone_view = None
+        self.ax_error_plot = None  # График погрешности позиции
         self.ax_global_map = None
         self.ax_detection = None
         self.ax_matches = None
@@ -39,14 +39,19 @@ class VisualizationManager:
         self.current_x = 0.0
         self.current_y = 0.0
         
+        # Данные для траектории БПЛА (его собственное видение)
+        self.drone_trajectory_x = []
+        self.drone_trajectory_y = []
+        
+        # Данные для графика погрешности
+        self.error_times = []
+        self.position_errors = []
+        
         # Данные для детекции
         self.current_frame = None
         self.keypoints = []
         self.matches = []
         
-        # Данные для вида БПЛА
-        self.drone_view_image = None
-        
         self._setup_window()
     
     def _setup_window(self):
@@ -61,10 +66,12 @@ class VisualizationManager:
         # Создаем сетку 2x2 с разными размерами колонок
         gs = self.fig.add_gridspec(2, 2, hspace=0.3, wspace=0.3, width_ratios=[1, 1])
         
-        # Вид БПЛА (левый верхний угол)
+        # График погрешности позиции (левый верхний угол)
-        self.ax_drone_view = self.fig.add_subplot(gs[0, 0])
+        self.ax_error_plot = self.fig.add_subplot(gs[0, 0])
-        self.ax_drone_view.set_title('Вид БПЛА')
+        self.ax_error_plot.set_title('Погрешность позиции от времени')
-        self.ax_drone_view.axis('off')
+        self.ax_error_plot.set_xlabel('Время (кадры)')
+        self.ax_error_plot.set_ylabel('Погрешность (метры)')
+        self.ax_error_plot.grid(True, alpha=0.3)
         
         # Глобальная карта (левый нижний угол)
         self.ax_global_map = self.fig.add_subplot(gs[1, 0])
@@ -123,6 +130,11 @@ class VisualizationManager:
                 autonome_y = [self.trajectory_y[i] for i in autonome_indices]
                 self.ax_global_map.plot(autonome_x, autonome_y, 'r-', linewidth=2, label='Автономный режим')
             
+            # Рисуем траекторию БПЛА (пунктирная линия, тонкая)
+            if len(self.drone_trajectory_x) > 1:
+                self.ax_global_map.plot(self.drone_trajectory_x, self.drone_trajectory_y, 
+                                      'g--', linewidth=1, alpha=0.7, label='Данные по одометрии')
+            
             # Рисуем начальную точку (зеленая)
             self.ax_global_map.plot(self.trajectory_x[0], self.trajectory_y[0], 'go', markersize=8, label='Начальная точка')
             
@@ -140,6 +152,13 @@ class VisualizationManager:
             x_min, x_max = min(self.trajectory_x), max(self.trajectory_x)
             y_min, y_max = min(self.trajectory_y), max(self.trajectory_y)
             
+            # Учитываем также траекторию БПЛА при масштабировании
+            if len(self.drone_trajectory_x) > 0:
+                x_min = min(x_min, min(self.drone_trajectory_x))
+                x_max = max(x_max, max(self.drone_trajectory_x))
+                y_min = min(y_min, min(self.drone_trajectory_y))
+                y_max = max(y_max, max(self.drone_trajectory_y))
+            
             x_min = min(x_min, 0)
             x_max = max(x_max, 0)
             y_min = min(y_min, 0)
@@ -148,23 +167,36 @@ class VisualizationManager:
             self.ax_global_map.set_xlim(x_min - margin, x_max + margin)
             self.ax_global_map.set_ylim(y_min - margin, y_max + margin)
     
-    def update_drone_view(self, image: np.ndarray):
+    def update_drone_trajectory(self, drone_x: float, drone_y: float):
-        """Обновляет вид БПЛА"""
+        """Обновляет траекторию БПЛА (его собственное видение позиции)"""
-        self.drone_view_image = image.copy()
+        self.drone_trajectory_x.append(drone_x)
+        self.drone_trajectory_y.append(drone_y)
+    
+    def update_error_plot(self, frame_count: int, drone_x: float, drone_y: float, true_x: float, true_y: float):
+        """Обновляет график погрешности позиции"""
+        # Вычисляем погрешность как расстояние между реальной и предполагаемой позицией
+        error = np.sqrt((drone_x - true_x)**2 + (drone_y - true_y)**2)
         
-        self.ax_drone_view.clear()
+        self.error_times.append(frame_count)
-        self.ax_drone_view.set_title('Вид БПЛА')
+        self.position_errors.append(error)
         
-        if image is not None:
+        self.ax_error_plot.clear()
-            # Конвертируем BGR в RGB для matplotlib
+        self.ax_error_plot.set_title('Погрешность позиции от времени')
-            if len(image.shape) == 3 and image.shape[2] == 3:
+        self.ax_error_plot.set_xlabel('Время (кадры)')
-                image_rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
+        self.ax_error_plot.set_ylabel('Погрешность (метры)')
-            else:
+        self.ax_error_plot.grid(True, alpha=0.3)
-                image_rgb = image
+        
+        if len(self.error_times) > 1:
+            self.ax_error_plot.plot(self.error_times, self.position_errors, 'b-', linewidth=2)
             
-            self.ax_drone_view.imshow(image_rgb)
+            # Автоматически масштабируем оси
-        
+            if len(self.position_errors) > 0:
-        self.ax_drone_view.axis('off')
+                margin = 0.1
+                error_min, error_max = min(self.position_errors), max(self.position_errors)
+                if error_max > error_min:
+                    self.ax_error_plot.set_ylim(0, error_max + margin)
+                else:
+                    self.ax_error_plot.set_ylim(0, 1)
     
     def update_detection(self, image: np.ndarray, keypoints):
         """Обновляет визуализацию детекции ключевых точек"""
@@ -214,18 +246,14 @@ class VisualizationManager:
             if transformation_info:
                 tx, ty = transformation_info['translation']
                 angle = transformation_info['rotation']
-                scale = transformation_info['scale']
                 
                 info_text = f"Translation: ({tx:.2f}, {ty:.2f})"
                 info_text2 = f"Rotation: {angle:.2f} rad ({np.degrees(angle):.1f}°)"
-                info_text3 = f"Scale: {scale:.2f}"
                 
                 self.ax_matches.text(10, 30, info_text, fontsize=8, color='green', 
                                    bbox=dict(boxstyle="round,pad=0.3", facecolor="white", alpha=0.8))
                 self.ax_matches.text(10, 90, info_text2, fontsize=8, color='green',
                                    bbox=dict(boxstyle="round,pad=0.3", facecolor="white", alpha=0.8))
-                self.ax_matches.text(10, 150, info_text3, fontsize=8, color='green',
-                                   bbox=dict(boxstyle="round,pad=0.3", facecolor="white", alpha=0.8))
         
         self.ax_matches.axis('off')
     
@@ -243,6 +271,7 @@ class VisualizationManager:
         """Показывает финальное состояние окна"""
         plt.ioff()
         print("Симуляция завершена. Окно визуализации остается открытым для анализа.")
+        plt.pause(100000)
 
     def pause(self, duration: float):
-        plt.pause(duration)
+        plt.pause(duration)