diff --git a/autopilot.py b/autopilot.py index 9395ef2..f4577b9 100644 --- a/autopilot.py +++ b/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 diff --git a/simulator.py b/simulator.py index 7612c64..71aab45 100644 --- a/simulator.py +++ b/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.viz_manager.update_display() - - self.viz_manager.pause(0.25) + self.update_map() + self.viz_manager.update_display() # Финальное обновление карты self.update_map() diff --git a/visualization.py b/visualization.py index 90e86d9..410e76b 100644 --- a/visualization.py +++ b/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_drone_view.set_title('Вид БПЛА') - self.ax_drone_view.axis('off') + # График погрешности позиции (левый верхний угол) + self.ax_error_plot = self.fig.add_subplot(gs[0, 0]) + self.ax_error_plot.set_title('Погрешность позиции от времени') + 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): - """Обновляет вид БПЛА""" - self.drone_view_image = image.copy() + def update_drone_trajectory(self, drone_x: float, drone_y: float): + """Обновляет траекторию БПЛА (его собственное видение позиции)""" + 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.ax_drone_view.set_title('Вид БПЛА') + self.error_times.append(frame_count) + self.position_errors.append(error) - if image is not None: - # Конвертируем BGR в RGB для matplotlib - if len(image.shape) == 3 and image.shape[2] == 3: - image_rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB) - else: - image_rgb = image + self.ax_error_plot.clear() + self.ax_error_plot.set_title('Погрешность позиции от времени') + self.ax_error_plot.set_xlabel('Время (кадры)') + self.ax_error_plot.set_ylabel('Погрешность (метры)') + self.ax_error_plot.grid(True, alpha=0.3) + + 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) - - self.ax_drone_view.axis('off') + # Автоматически масштабируем оси + if len(self.position_errors) > 0: + 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) \ No newline at end of file + plt.pause(duration) \ No newline at end of file