zenloger/autopilot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: drawing points

Browse Source
This commit is contained in:
russian_proger
2025-09-16 21:29:26 +03:00
parent b47e3978e1
commit 040299ca4c
8 changed files with 606 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

277
visualization.py Normal file
View File

@@ -0,0 +1,277 @@
#!/usr/bin/env python3
"""
Модуль для управления общим окном визуализации
"""
import matplotlib.pyplot as plt
import matplotlib.patches as patches
import numpy as np
from enum import Enum
import cv2
from PIL import Image
import matplotlib
# Настройки 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.trajectory_x = []
self.trajectory_y = []
self.trajectory_modes = []
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._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')
# Создаем сетку 2x2 с разными размерами колонок
gs = self.fig.add_gridspec(2, 2, hspace=0.3, wspace=0.3, width_ratios=[1, 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_detection = self.fig.add_subplot(gs[0, 1])
self.ax_detection.set_title('Keypoint Detection')
self.ax_detection.axis('off')
# Сопоставление точек (правый нижний угол)
self.ax_matches = self.fig.add_subplot(gs[1, 1])
self.ax_matches.set_title('Feature Matching')
self.ax_matches.axis('off')
# Настройки окна
self.fig.canvas.manager.window.attributes('-topmost', False)
plt.tight_layout()
def update_global_map(self, x: float, y: float, mode: SimMode):
"""Обновляет глобальную карту"""
self.current_x = x
self.current_y = y
self.trajectory_x.append(x)
self.trajectory_y.append(y)
self.trajectory_modes.append(mode)
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:
# Разделяем траекторию по режимам
operator_indices = [i for i, m in enumerate(self.trajectory_modes) if m == SimMode.OPERATOR]
autonome_indices = operator_indices[-1:] + [i for i, m in enumerate(self.trajectory_modes) if m == SimMode.AUTONOME]
# Рисуем траекторию оператора (синий цвет)
if len(operator_indices) > 1:
operator_x = [self.trajectory_x[i] for i in operator_indices]
operator_y = [self.trajectory_y[i] for i in operator_indices]
self.ax_global_map.plot(operator_x, operator_y, 'b-', linewidth=2, label='Режим оператора')
# Рисуем траекторию автономного режима (красный цвет)
if len(autonome_indices) > 1:
autonome_x = [self.trajectory_x[i] for i in autonome_indices]
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='Начальная точка')
# Рисуем текущую позицию (черная)
self.ax_global_map.plot(self.current_x, self.current_y, 'ko', markersize=6, label='Текущая позиция')
# Рисуем целевую точку (0, 0) - желтая
self.ax_global_map.plot(0, 0, 'yo', markersize=8, label='Цель (0, 0)')
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)
# Учитываем также траекторию БПЛА при масштабировании
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=2)
# Автоматически масштабируем оси
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):
"""Обновляет визуализацию детекции ключевых точек"""
self.current_frame = image.copy()
self.keypoints = keypoints
self.ax_detection.clear()
self.ax_detection.set_title('Keypoint Detection')
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_detection.imshow(image_rgb)
# Рисуем ключевые точки
if keypoints:
kp_coords = np.array([kp.pt for kp in keypoints])
self.ax_detection.scatter(kp_coords[:, 0], kp_coords[:, 1],
c='red', s=20, alpha=0.7, marker='o')
self.ax_detection.axis('off')
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_display(self):
"""Обновляет отображение всех областей"""
self.fig.canvas.draw()
self.fig.canvas.flush_events()
plt.pause(0.01)
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)