#!/usr/bin/env python3 """ Модуль для управления общим окном визуализации """ from PIL import Image from enum import Enum from scipy.interpolate import make_interp_spline import cv2 import matplotlib import matplotlib.axes import matplotlib.pyplot as plt import matplotlib.patches as patches import numpy as np # Настройки matplotlib matplotlib.use('TkAgg') plt.rcParams['figure.raise_window'] = False class SimMode(Enum): OPERATOR = 1 AUTONOME = 2 class VisualizationManager: """ Менеджер для управления общим окном визуализации """ def __init__(self, window_title="Drone Autopilot Visualization"): self.window_title = window_title self.fig = None self.ax_error_plot = None # График погрешности позиции self.ax_global_map = None self.ax_detection = None self.ax_matches = None self.ax_chunk_matches = None self.ax_motion_vectors = None # Данные для глобальной карты self.trajectory_x = [] self.trajectory_y = [] self.current_x = 0.0 self.current_y = 0.0 self.target_idx = 0 self.target_pts = [] # Данные для траектории БПЛА (его собственное видение) self.drone_trajectory_x = [] self.drone_trajectory_y = [] # Данные для графика погрешности self.error_times = [] self.position_errors = [] # Данные для детекции self.current_frame = None self.keypoints = [] self.matches = [] self._setup_window() def _setup_window(self): """Настраивает общее окно с несколькими областями""" plt.ion() self.fig = plt.figure(figsize=(16, 10)) self.fig.canvas.manager.window.title(self.window_title) # Открываем окно на полный экран self.fig.canvas.manager.window.state('zoomed') # Создаем сетку 3x3 с разными размерами колонок gs = self.fig.add_gridspec(2, 3, hspace=0.3, wspace=0.3, width_ratios=[1, 0.7, 1]) # График погрешности позиции (левый верхний угол) 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]) self.ax_global_map.set_title('Global Map - Траектория полета беспилотника') self.ax_global_map.set_xlabel('X координата') self.ax_global_map.set_ylabel('Y координата') self.ax_global_map.grid(True, alpha=0.3) self.ax_global_map.axhline(y=0, color='k', linestyle='-', alpha=0.3) self.ax_global_map.axvline(x=0, color='k', linestyle='-', alpha=0.3) # Сопоставление точек (правый верхний угол) self.ax_matches = self.fig.add_subplot(gs[0, 2]) self.ax_matches.set_title('Feature Matching') self.ax_matches.axis('off') # Сопоставление точек (средний средний угол) self.ax_chunk_matches = self.fig.add_subplot(gs[1, 1:3]) self.ax_chunk_matches.set_title('Chunk Matching') self.ax_chunk_matches.axis('off') # Визуализация движения ключевых точек (левый нижний угол) # self.ax_motion_vectors = self.fig.add_subplot(gs[1, 1]) # self.ax_motion_vectors.set_title('Motion Vectors - Движение ключевых точек') # self.ax_motion_vectors.axis('off') # Визуализация движения ключевых точек на основе матрицы гомографии self.ax_motion_gomography = self.fig.add_subplot(gs[0, 1]) self.ax_motion_gomography.set_title('Keypoint Detection') self.ax_motion_gomography.axis('off') # Настройки окна self.fig.canvas.manager.window.attributes('-topmost', False) plt.tight_layout() plt.show(block=False) def set_target_points(self, target_pts): """ Обновление списка координат целевых точек """ self.target_pts = target_pts def set_target_index(self, target_idx): """ Обновление номера целевой точки """ self.target_idx = target_idx def update_global_map(self, x: float, y: float): """Обновляет глобальную карту""" self.current_x = x self.current_y = y self.trajectory_x.append(x) self.trajectory_y.append(y) self.ax_global_map.clear() self.ax_global_map.set_title('Global Map - Траектория полета беспилотника') self.ax_global_map.set_xlabel('X координата') self.ax_global_map.set_ylabel('Y координата') self.ax_global_map.grid(True, alpha=0.3) self.ax_global_map.axhline(y=0, color='k', linestyle='-', alpha=0.3) self.ax_global_map.axvline(x=0, color='k', linestyle='-', alpha=0.3) if len(self.trajectory_x) > 1: # Рисуем траекторию оператора (синий цвет) self.ax_global_map.plot(self.trajectory_x, self.trajectory_y, 'b-', 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.current_x, self.current_y, 'ko', markersize=6, label='Текущая позиция') # Рисуем ориентиры for i in range(len(self.target_pts)): if i != self.target_idx: pt = self.target_pts[i] self.ax_global_map.plot(pt[0], pt[1], 'go', markersize=8) # Рисуем текущую целевую точку if self.target_idx < len(self.target_pts): pt = self.target_pts[self.target_idx] self.ax_global_map.plot(pt[0], pt[1], 'yo', markersize=8, label='Цель') self.ax_global_map.legend() # Автоматически масштабируем оси if len(self.trajectory_x) > 0: margin = 50 x_min, x_max = min(self.trajectory_x), max(self.trajectory_x) y_min, y_max = min(self.trajectory_y), max(self.trajectory_y) for pt in self.target_pts: x_min = min(x_min, pt[0]) x_max = max(x_max, pt[0]) y_min = min(y_min, pt[1]) y_max = max(y_max, pt[1]) # Учитываем также траекторию БПЛА при масштабировании 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) y_max = max(y_max, 0) 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_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.error_times.append(frame_count) self.position_errors.append(error) 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=1, alpha=0.4, label='Погрешность данных') if len(self.error_times) > 5: # Сглаженный график smoothed_times = np.linspace(self.error_times[0], self.error_times[-1], 300) spl = make_interp_spline(self.error_times, self.position_errors, k=3) smoothed_errors = spl(smoothed_times) self.ax_error_plot.plot(smoothed_times, smoothed_errors, 'orange', linewidth=2, label='Сглаженный тренд') # if len(self.position_errors) > 5: # Достаточно данных для сглаживания # window_size = min(11, len(self.position_errors) // 3) # Адаптивный размер окна # if window_size % 2 == 0: # Должен быть нечетным # window_size += 1 # # Метод скользящего среднего # smoothed_errors = np.convolve( # self.position_errors, # np.ones(window_size) / window_size, # mode='valid' # ) # # Корректируем временную ось для сглаженных данных # offset = (window_size - 1) // 2 # smoothed_times = self.error_times[offset:offset + len(smoothed_errors)] self.ax_error_plot.legend(loc='upper right') # Автоматически масштабируем оси 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_matches(self, img1: np.ndarray, img2: np.ndarray, kp1, kp2, matches, transformation_info=None): """Обновляет визуализацию сопоставления точек""" self.ax_matches.clear() self.ax_matches.set_title('Feature Matching') if img1 is not None and img2 is not None and matches: # Рисуем сопоставления img_matches = cv2.drawMatches(img1, kp1, img2, kp2, matches, None, flags=cv2.DrawMatchesFlags_NOT_DRAW_SINGLE_POINTS) # Конвертируем BGR в RGB if len(img_matches.shape) == 3 and img_matches.shape[2] == 3: img_matches_rgb = cv2.cvtColor(img_matches, cv2.COLOR_BGR2RGB) else: img_matches_rgb = img_matches self.ax_matches.imshow(img_matches_rgb) # Добавляем информацию о трансформации if transformation_info: tx, ty = transformation_info['translation'] angle = transformation_info['rotation'] info_text = f"Translation: ({tx:.2f}, {ty:.2f})" info_text2 = f"Rotation: {angle:.2f} rad ({np.degrees(angle):.1f}°)" 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.axis('off') def update_chunk_matches(self, img1: np.ndarray, img2: np.ndarray, kp1, kp2, matches, transformation_info=None): """Обновляет визуализацию сопоставления точек""" self.ax_chunk_matches.clear() self.ax_chunk_matches.set_title('Chunk Matching') if img1 is not None and img2 is not None and matches: # Рисуем сопоставления img_matches = cv2.drawMatches(img1, kp1, img2, kp2, matches, None, flags=cv2.DrawMatchesFlags_NOT_DRAW_SINGLE_POINTS) # Конвертируем BGR в RGB if len(img_matches.shape) == 3 and img_matches.shape[2] == 3: img_matches_rgb = cv2.cvtColor(img_matches, cv2.COLOR_BGR2RGB) else: img_matches_rgb = img_matches self.ax_chunk_matches.imshow(img_matches_rgb) # Добавляем информацию о трансформации if transformation_info: tx, ty = transformation_info['translation'] angle = transformation_info['rotation'] info_text = f"Translation: ({tx:.2f}, {ty:.2f})" info_text2 = f"Rotation: {angle:.2f} rad ({np.degrees(angle):.1f}°)" self.ax_chunk_matches.text(10, 30, info_text, fontsize=8, color='green', bbox=dict(boxstyle="round,pad=0.3", facecolor="white", alpha=0.8)) self.ax_chunk_matches.text(10, 90, info_text2, fontsize=8, color='green', bbox=dict(boxstyle="round,pad=0.3", facecolor="white", alpha=0.8)) self.ax_chunk_matches.axis('off') def _update_motion_vectors(self, axes: matplotlib.axes.Axes, current_frame: np.ndarray, prev_keypoints, current_keypoints, matches=None): """Обновляет визуализацию движения ключевых точек между кадрами""" axes.clear() axes.set_title('Motion Vectors - Движение ключевых точек') if current_frame is not None: # Конвертируем BGR в RGB для matplotlib if len(current_frame.shape) == 3 and current_frame.shape[2] == 3: frame_rgb = cv2.cvtColor(current_frame, cv2.COLOR_BGR2RGB) else: frame_rgb = current_frame # Показываем текущий кадр axes.imshow(frame_rgb) # Если есть совпадения, рисуем векторы движения if matches is not None and len(matches) > 0: # Получаем координаты ключевых точек prev_pts = np.array([prev_keypoints[m.queryIdx].pt for m in matches]) curr_pts = np.array([current_keypoints[m.trainIdx].pt for m in matches]) # Вычисляем векторы движения motion_vectors = curr_pts - prev_pts # Вычисляем длину и направление векторов vector_lengths = np.linalg.norm(motion_vectors, axis=1) vector_angles = np.arctan2(motion_vectors[:, 1], motion_vectors[:, 0]) # Нормализуем длины для цветовой карты (0-1) if len(vector_lengths) > 0: max_length = np.max(vector_lengths) if max_length > 0: normalized_lengths = vector_lengths / max_length else: normalized_lengths = np.zeros_like(vector_lengths) else: normalized_lengths = np.array([]) # Рисуем векторы с цветовой индикацией for i, (start_pt, end_pt, length, angle, norm_length) in enumerate( zip(prev_pts, curr_pts, vector_lengths, vector_angles, normalized_lengths)): # Цвет зависит от направления (угол -> HSV) hue = (angle + np.pi) / (2 * np.pi) # Нормализуем угол к 0-1 saturation = 1.0 value = 0.8 + 0.2 * norm_length # Яркость зависит от длины # Конвертируем HSV в RGB import matplotlib.colors as mcolors rgb = mcolors.hsv_to_rgb([hue, saturation, value]) # Толщина линии зависит от длины вектора linewidth = max(1, min(5, 2 + 3 * norm_length)) # Рисуем вектор axes.arrow( start_pt[0], start_pt[1], end_pt[0] - start_pt[0], end_pt[1] - start_pt[1], head_width=3, head_length=5, fc=rgb, ec=rgb, alpha=0.8, linewidth=linewidth ) # Добавляем текст с информацией о движении # if length > 5: # Показываем только для значительных движений # axes.text( # end_pt[0] + 5, end_pt[1] + 5, # f'{length:.1f}px', fontsize=6, color='white', # bbox=dict(boxstyle="round,pad=0.2", facecolor="black", alpha=0.7) # ) # Добавляем легенду с цветовой схемой # legend_text = "Цвет: направление, Яркость: скорость" # axes.text( # 10, 30, legend_text, fontsize=8, color='white', # bbox=dict(boxstyle="round,pad=0.3", facecolor="black", alpha=0.8) # ) # Статистика движения # if len(vector_lengths) > 0: # avg_speed = np.mean(vector_lengths) # max_speed = np.max(vector_lengths) # stats_text = f"Средняя скорость: {avg_speed:.1f}px\nМаксимальная: {max_speed:.1f}px" # axes.text( # 10, 60, stats_text, fontsize=8, color='white', # bbox=dict(boxstyle="round,pad=0.3", facecolor="black", alpha=0.8) # ) axes.axis('off') def update_motion_vectors(self, current_frame: np.ndarray, prev_keypoints, current_keypoints, matches=None): self._update_motion_vectors(self.ax_motion_vectors, current_frame, prev_keypoints, current_keypoints, matches) def update_motion_vectors(self, current_frame: np.ndarray, prev_keypoints, current_keypoints, matches=None): self._update_motion_vectors(self.ax_motion_vectors, current_frame, prev_keypoints, current_keypoints, matches) def update_motion_gomography(self, current_frame: np.ndarray, prev_keypoints, current_keypoints, matches=None): self._update_motion_vectors(self.ax_motion_gomography, current_frame, prev_keypoints, current_keypoints, matches) def update_homography_grid(self, current_frame: np.ndarray, homography_matrix: np.ndarray, grid_step: int = 80): """ Визуализирует движение точек по сетке на основе матрицы гомографии """ self.ax_motion_gomography.clear() self.ax_motion_gomography.set_title('Движение точек по сетке') if current_frame is None or homography_matrix is None: self.ax_motion_gomography.axis('off') return # Конвертируем BGR в RGB для matplotlib if len(current_frame.shape) == 3 and current_frame.shape[2] == 3: frame_rgb = cv2.cvtColor(current_frame, cv2.COLOR_BGR2RGB) else: frame_rgb = current_frame # Показываем текущий кадр self.ax_motion_gomography.imshow(frame_rgb) # Получаем размеры изображения и центр height, width = current_frame.shape[:2] # Создаем сетку точек с заданным шагом grid_points = [] for y in range(grid_step, height, grid_step): for x in range(grid_step, width, grid_step): grid_points.append([x, y]) if len(grid_points) == 0: self.ax_motion_gomography.axis('off') return # Конвертируем в numpy массив и отцентрируем координаты относительно центра изображения grid_points = np.array(grid_points, dtype=np.float32) grid_points_centered = [] for pt in grid_points: # Отцентрируем координаты точно так же, как в detect_and_match_keypoints centered_x = pt[0] centered_y = pt[1] grid_points_centered.append([centered_x, centered_y]) grid_points_centered = np.array(grid_points_centered, dtype=np.float32) grid_points_homogeneous = np.column_stack([grid_points_centered, np.ones(len(grid_points_centered))]) # Применяем матрицу гомографии transformed_points_homogeneous = homography_matrix @ grid_points_homogeneous.T transformed_points_homogeneous = transformed_points_homogeneous.T # Нормализуем по третьей координате (перспективное преобразование) transformed_points_centered = transformed_points_homogeneous[:, :2] / transformed_points_homogeneous[:, 2:3] # Конвертируем обратно в координаты изображения transformed_points = [] for pt in transformed_points_centered: # Обратное преобразование от центрированных координат к координатам изображения img_x = pt[0] img_y = pt[1] # Инвертируем Y обратно transformed_points.append([img_x, img_y]) transformed_points = np.array(transformed_points, dtype=np.float32) # Фильтруем точки, которые остались в пределах изображения valid_indices = ( (transformed_points[:, 0] >= 0) & (transformed_points[:, 0] < width) & (transformed_points[:, 1] >= 0) & (transformed_points[:, 1] < height) ) if np.sum(valid_indices) == 0: self.ax_motion_gomography.axis('off') return # Получаем валидные исходные и трансформированные точки valid_source_points = grid_points[valid_indices] valid_transformed_points = transformed_points[valid_indices] # Вычисляем векторы движения motion_vectors = valid_transformed_points - valid_source_points # Вычисляем длину и направление векторов vector_lengths = np.linalg.norm(motion_vectors, axis=1) if len(vector_lengths) > 0: # Нормализуем длины для цветовой карты (0-1) max_length = np.max(vector_lengths) if max_length > 0: normalized_lengths = vector_lengths / max_length else: normalized_lengths = np.zeros_like(vector_lengths) # Рисуем векторы движения for i, (start_pt, end_pt, length, norm_length) in enumerate( zip(valid_source_points, valid_transformed_points, vector_lengths, normalized_lengths)): # Пропускаем очень маленькие векторы if length < 1.0: continue # Цвет зависит от длины вектора (синий -> красный) if norm_length < 0.5: color = [0, norm_length * 2, 1 - norm_length * 2] # Синий -> Голубой else: color = [(norm_length - 0.5) * 2, 1 - (norm_length - 0.5) * 2, 0] # Голубой -> Красный # Толщина линии зависит от длины вектора linewidth = max(1, min(4, 1 + 3 * norm_length)) # Рисуем вектор self.ax_motion_gomography.arrow( start_pt[0], start_pt[1], end_pt[0] - start_pt[0], end_pt[1] - start_pt[1], head_width=3, head_length=5, fc=color, ec=color, alpha=0.8, linewidth=linewidth ) # Рисуем исходную точку сетки self.ax_motion_gomography.plot(start_pt[0], start_pt[1], 'o', color='green', markersize=3, alpha=0.7) # Рисуем целевую точку self.ax_motion_gomography.plot(end_pt[0], end_pt[1], 's', color='red', markersize=2, alpha=0.7) # Добавляем информацию о статистике avg_length = np.mean(vector_lengths) max_length = np.max(vector_lengths) total_points = len(vector_lengths) # info_text = f"Точек сетки: {total_points}\nСреднее движение: {avg_length:.1f}px\nМакс. движение: {max_length:.1f}px" # self.ax_motion_gomography.text( # 10, 30, info_text, fontsize=8, color='white', # bbox=dict(boxstyle="round,pad=0.3", facecolor="black", alpha=0.8) # ) # Добавляем легенду # legend_text = "Зеленые точки: исходные\nКрасные квадраты: целевые\nЦвет стрелок: скорость" # self.ax_motion_gomography.text( # 10, 90, legend_text, fontsize=7, color='white', # bbox=dict(boxstyle="round,pad=0.3", facecolor="black", alpha=0.8) # ) self.ax_motion_gomography.axis('off') def update_display(self): """Обновляет отображение всех областей""" self.fig.canvas.draw() self.fig.canvas.flush_events() plt.pause(0.2) def close(self): """Закрывает окно""" plt.close(self.fig) def show_final(self): """Показывает финальное состояние окна""" plt.ioff() print("Симуляция завершена. Окно визуализации остается открытым для анализа.") plt.pause(100000) def pause(self, duration: float): plt.pause(duration)