feat: add shared window

This commit is contained in:
2025-06-30 01:33:09 +03:00
parent 8b6bb3e22c
commit 2d85d78def
4 changed files with 316 additions and 118 deletions

View File

@@ -19,13 +19,17 @@ class AutoPilot(Pilot):
orb_detector: cv2.ORB orb_detector: cv2.ORB
bf_matcher: cv2.BFMatcher bf_matcher: cv2.BFMatcher
frame_count: int frame_count: int
image_center: tuple # Центр изображения (x, y)
viz_manager: object # Менеджер визуализации (опционально)
def __init__(self, initial_x: float = 0.0, initial_y: float = 0.0): def __init__(self, viz_manager=None):
self.prev_image = None self.prev_image = None
self.angle = 0.0 self.angle = 0.0
self.x = initial_x self.x = 0.0
self.y = initial_y self.y = 0.0
self.frame_count = 0 self.frame_count = 0
self.image_center = (0, 0) # Будет обновлено при получении первого изображения
self.viz_manager = viz_manager # Менеджер визуализации
# Инициализация ORB детектора # Инициализация ORB детектора
self.orb_detector = cv2.ORB_create( self.orb_detector = cv2.ORB_create(
@@ -72,15 +76,35 @@ class AutoPilot(Pilot):
if len(good_matches) < 4: if len(good_matches) < 4:
return None, None, None, None, None return None, None, None, None, None
# Извлечение координат сопоставленных точек # Получаем центр изображения
src_pts = np.float32([kp1[m.queryIdx].pt for m in good_matches]).reshape(-1, 1, 2) height1, width1 = img1.shape[:2]
dst_pts = np.float32([kp2[m.trainIdx].pt for m in good_matches]).reshape(-1, 1, 2) height2, width2 = img2.shape[:2]
center_x1, center_y1 = width1 // 2, height1 // 2
center_x2, center_y2 = width2 // 2, height2 // 2
# Извлекаем координаты сопоставленных точек и отцентрируем их
src_pts = []
dst_pts = []
for match in good_matches:
# Координаты точки в первом изображении
pt1 = kp1[match.queryIdx].pt
src_pts.append([pt1[0] - center_x1, pt1[1] - center_y1])
# Координаты точки во втором изображении
pt2 = kp2[match.trainIdx].pt
dst_pts.append([pt2[0] - center_x2, pt2[1] - center_y2])
# Конвертируем в numpy массивы
src_pts = np.float32(src_pts).reshape(-1, 1, 2)
dst_pts = np.float32(dst_pts).reshape(-1, 1, 2)
return src_pts, dst_pts, good_matches, kp1, kp2 return src_pts, dst_pts, good_matches, kp1, kp2
def estimate_transformation_matrix(self, src_pts: np.ndarray, dst_pts: np.ndarray): def estimate_transformation_matrix(self, src_pts: np.ndarray, dst_pts: np.ndarray):
""" """
Оценивает матрицу трансформации на основе сопоставленных ключевых точек Оценивает матрицу трансформации на основе сопоставленных ключевых точек
Точки уже отцентрированы относительно центра изображения
""" """
# Используем RANSAC для оценки матрицы гомографии # Используем RANSAC для оценки матрицы гомографии
H, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 5.0) H, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 5.0)
@@ -97,7 +121,7 @@ class AutoPilot(Pilot):
a11, a12 = H[0, 0], H[0, 1] a11, a12 = H[0, 0], H[0, 1]
a21, a22 = H[1, 0], H[1, 1] a21, a22 = H[1, 0], H[1, 1]
# Смещение # Смещение (уже отцентрировано)
tx, ty = H[0, 2], H[1, 2] tx, ty = H[0, 2], H[1, 2]
# Вычисляем угол поворота # Вычисляем угол поворота
@@ -109,7 +133,7 @@ class AutoPilot(Pilot):
scale = (scale_x + scale_y) / 2 scale = (scale_x + scale_y) / 2
return { return {
'translation': (tx, ty), 'translation': (tx, ty), # Уже отцентрировано
'rotation': angle, 'rotation': angle,
'scale': scale, 'scale': scale,
'homography': H, 'homography': H,
@@ -119,12 +143,13 @@ class AutoPilot(Pilot):
def update_drone_position(self, transformation_info: dict): def update_drone_position(self, transformation_info: dict):
""" """
Обновляет позицию и угол БПЛА на основе трансформации изображения Обновляет позицию и угол БПЛА на основе трансформации изображения
Координаты уже отцентрированы относительно центра изображения
""" """
tx, ty = transformation_info['translation'] tx, ty = transformation_info['translation']
rotation = transformation_info['rotation'] rotation = transformation_info['rotation']
scale = transformation_info['scale'] scale = transformation_info['scale']
# Конвертируем смещение в пикселях в метры # Координаты уже отцентрированы, поэтому используем их напрямую
dx_meters = tx dx_meters = tx
dy_meters = ty dy_meters = ty
@@ -133,8 +158,9 @@ class AutoPilot(Pilot):
sin_angle = math.sin(self.angle) sin_angle = math.sin(self.angle)
# Поворачиваем смещение в глобальные координаты # Поворачиваем смещение в глобальные координаты
dx_global = dx_meters * cos_angle - dy_meters * sin_angle # Обратите внимание: dy_meters инвертирован, так как в изображениях Y направлен вниз
dy_global = dx_meters * sin_angle + dy_meters * cos_angle dx_global = dx_meters * cos_angle - (-dy_meters) * sin_angle
dy_global = dx_meters * sin_angle + (-dy_meters) * cos_angle
# Обновляем координаты БПЛА # Обновляем координаты БПЛА
self.x += dx_global self.x += dx_global
@@ -176,18 +202,20 @@ class AutoPilot(Pilot):
info_text = f"Translation: ({tx:.2f}, {ty:.2f})" info_text = f"Translation: ({tx:.2f}, {ty:.2f})"
info_text2 = f"Rotation: {angle:.2f} rad ({np.degrees(angle):.1f}°)" info_text2 = f"Rotation: {angle:.2f} rad ({np.degrees(angle):.1f}°)"
info_text3 = f"Scale: {scale:.2f}" info_text3 = f"Scale: {scale:.2f}"
info_text4 = f"Image Center: {self.image_center}"
cv2.putText(img_matches, info_text, (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2) cv2.putText(img_matches, info_text, (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
cv2.putText(img_matches, info_text2, (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2) cv2.putText(img_matches, info_text2, (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
cv2.putText(img_matches, info_text3, (10, 90), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2) cv2.putText(img_matches, info_text3, (10, 90), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
cv2.putText(img_matches, info_text4, (10, 120), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
# Добавляем информацию о позиции БПЛА # Добавляем информацию о позиции БПЛА
drone_state = self.get_drone_state() drone_state = self.get_drone_state()
pos_text = f"Drone Pos: ({drone_state['x']:.2f}, {drone_state['y']:.2f})" pos_text = f"Drone Pos: ({drone_state['x']:.2f}, {drone_state['y']:.2f})"
angle_text = f"Drone Angle: {drone_state['angle_degrees']:.1f}°" angle_text = f"Drone Angle: {drone_state['angle_degrees']:.1f}°"
cv2.putText(img_matches, pos_text, (10, 120), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 0, 0), 2) cv2.putText(img_matches, pos_text, (10, 150), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 0, 0), 2)
cv2.putText(img_matches, angle_text, (10, 150), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 0, 0), 2) cv2.putText(img_matches, angle_text, (10, 180), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 0, 0), 2)
return img_matches return img_matches
@@ -196,11 +224,24 @@ class AutoPilot(Pilot):
if self.prev_image is None: if self.prev_image is None:
self.prev_image = self.image_to_numpy(image) self.prev_image = self.image_to_numpy(image)
# Вычисляем центр изображения
height, width = self.prev_image.shape[:2]
self.image_center = (width // 2, height // 2)
# Обновляем визуализацию детекции
if self.viz_manager:
kp, _ = self.orb_detector.detectAndCompute(self.prev_image, None)
self.viz_manager.update_detection(self.prev_image, kp)
return return
# Конвертируем текущее изображение # Конвертируем текущее изображение
current_image = self.image_to_numpy(image) current_image = self.image_to_numpy(image)
# Обновляем центр изображения
height, width = current_image.shape[:2]
self.image_center = (width // 2, height // 2)
# Обнаруживаем и сопоставляем ключевые точки # Обнаруживаем и сопоставляем ключевые точки
src_pts, dst_pts, matches, kp1, kp2 = self.detect_and_match_keypoints(self.prev_image, current_image) src_pts, dst_pts, matches, kp1, kp2 = self.detect_and_match_keypoints(self.prev_image, current_image)
@@ -209,11 +250,6 @@ class AutoPilot(Pilot):
transformation_info = self.estimate_transformation_matrix(src_pts, dst_pts) transformation_info = self.estimate_transformation_matrix(src_pts, dst_pts)
if transformation_info: if transformation_info:
# print(f"Frame {self.frame_count}:")
# print(f" Translation: {transformation_info['translation']}")
# print(f" Rotation: {transformation_info['rotation']:.4f} rad")
# print(f" Scale: {transformation_info['scale']:.4f}")
# Обновляем позицию и угол БПЛА # Обновляем позицию и угол БПЛА
self.update_drone_position(transformation_info) self.update_drone_position(transformation_info)
@@ -222,11 +258,20 @@ class AutoPilot(Pilot):
print(f" [Pilot] Drone Position: ({drone_state['x']:.2f}, {drone_state['y']:.2f})") print(f" [Pilot] Drone Position: ({drone_state['x']:.2f}, {drone_state['y']:.2f})")
print(f" [Pilot] Angle: {drone_state['angle_degrees']:.1f}°") print(f" [Pilot] Angle: {drone_state['angle_degrees']:.1f}°")
# Визуализация (опционально) # Обновляем визуализацию
img_matches = self.visualize_matches(self.prev_image, current_image, if self.viz_manager:
# Обновляем детекцию ключевых точек
self.viz_manager.update_detection(current_image, kp2)
# Обновляем сопоставление точек
self.viz_manager.update_matches(self.prev_image, current_image,
kp1, kp2, matches, transformation_info) kp1, kp2, matches, transformation_info)
cv2.imshow('Drone Tracking', img_matches)
cv2.waitKey(1) # Визуализация (опционально, если нет менеджера визуализации)
if not self.viz_manager:
img_matches = self.visualize_matches(self.prev_image, current_image,
kp1, kp2, matches, transformation_info)
cv2.imshow('Drone Tracking', img_matches)
cv2.waitKey(1)
# Обновляем предыдущее изображение # Обновляем предыдущее изображение
self.prev_image = current_image self.prev_image = current_image

View File

@@ -1,6 +1,13 @@
from autopilot import AutoPilot, RandomPilot from autopilot import AutoPilot, RandomPilot
from simulator import Simulator from simulator import Simulator
from visualization import VisualizationManager
# Создаем менеджер визуализации
viz_manager = VisualizationManager("Drone Autopilot - Global Map & Detection")
# Создаем симулятор с AutoPilot для обработки изображений # Создаем симулятор с AutoPilot для обработки изображений
simulator = Simulator(RandomPilot(), AutoPilot()) # Передаем менеджер визуализации в автопилот
simulator = Simulator(RandomPilot(), AutoPilot(viz_manager=viz_manager), viz_manager=viz_manager)
# Запускаем симуляцию
simulator.loop() simulator.loop()

View File

@@ -8,16 +8,10 @@ from selenium.webdriver.common.by import By
from selenium.webdriver.common.action_chains import ActionChains from selenium.webdriver.common.action_chains import ActionChains
from autopilot import Pilot from autopilot import Pilot
from visualization import VisualizationManager, SimMode
from enum import Enum
import matplotlib.pyplot as plt
import numpy as np
import os import os
class SimMode(Enum):
OPERATOR = 1
AUTONOME = 2
class Simulator: class Simulator:
driver: webdriver.Chrome driver: webdriver.Chrome
mode: SimMode mode: SimMode
@@ -27,42 +21,32 @@ class Simulator:
angle: float angle: float
# Переменные для отслеживания траектории # Менеджер визуализации
trajectory_x: list viz_manager: VisualizationManager
trajectory_y: list
trajectory_modes: list # Для отслеживания режимов полета
current_x: float current_x: float
current_y: float current_y: float
fig: plt.Figure
ax: plt.Axes
def __init__(self, operatorPilot: Pilot, autonomePilot: Pilot): def __init__(self, operatorPilot: Pilot, autonomePilot: Pilot, viz_manager: VisualizationManager = None):
self.mode = SimMode.OPERATOR self.mode = SimMode.OPERATOR
self.operatorPilot = operatorPilot self.operatorPilot = operatorPilot
self.autonomePilot = autonomePilot self.autonomePilot = autonomePilot
# Инициализация переменных для траектории # Инициализация переменных для траектории
self.trajectory_x = []
self.trajectory_y = []
self.trajectory_modes = []
self.current_x = 0.0 self.current_x = 0.0
self.current_y = 0.0 self.current_y = 0.0
# Создание окна для карты # Создаем менеджер визуализации
plt.ion() # Включаем интерактивный режим self.viz_manager = viz_manager
self.fig, self.ax = plt.subplots(figsize=(10, 8))
self.ax.set_title('Global Map - Траектория полета беспилотника') # Передаем менеджер визуализации в автопилот, если он поддерживает это
self.ax.set_xlabel('X координата') if hasattr(self.autonomePilot, 'viz_manager'):
self.ax.set_ylabel('Y координата') self.autonomePilot.viz_manager = self.viz_manager
self.ax.grid(True, alpha=0.3)
self.ax.axhline(y=0, color='k', linestyle='-', alpha=0.3)
self.ax.axvline(x=0, color='k', linestyle='-', alpha=0.3)
# Создаем папку для изображений, если её нет # Создаем папку для изображений, если её нет
os.makedirs('./images', exist_ok=True) os.makedirs('./images', exist_ok=True)
options = webdriver.ChromeOptions() options = webdriver.ChromeOptions()
options.add_experimental_option("detach", True) # options.add_experimental_option("detach", True)
self.driver = webdriver.Chrome(options) self.driver = webdriver.Chrome(options)
self.driver.get("https://yandex.ru/maps/43/kazan/?ll=49.103814%2C55.794258&z=14") self.driver.get("https://yandex.ru/maps/43/kazan/?ll=49.103814%2C55.794258&z=14")
sleep(2) sleep(2)
@@ -112,69 +96,12 @@ class Simulator:
# Обновляем текущие координаты # Обновляем текущие координаты
self.current_x += dx self.current_x += dx
self.current_y += dy self.current_y += dy
# Добавляем точку в траекторию
self.trajectory_x.append(self.current_x)
self.trajectory_y.append(self.current_y)
self.trajectory_modes.append(self.mode)
def update_map(self): def update_map(self):
""" """
Обновляет карту траектории полета Обновляет карту траектории полета
""" """
self.ax.clear() self.viz_manager.update_global_map(self.current_x, self.current_y, self.mode)
self.ax.set_title('Global Map - Траектория полета беспилотника')
self.ax.set_xlabel('X координата')
self.ax.set_ylabel('Y координата')
self.ax.grid(True, alpha=0.3)
self.ax.axhline(y=0, color='k', linestyle='-', alpha=0.3)
self.ax.axvline(x=0, color='k', linestyle='-', alpha=0.3)
if len(self.trajectory_x) > 1:
# Разделяем траекторию по режимам
operator_indices = [i for i, mode in enumerate(self.trajectory_modes) if mode == SimMode.OPERATOR]
autonome_indices = [i for i, mode in enumerate(self.trajectory_modes) if mode == 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.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.plot(autonome_x, autonome_y, 'r-', linewidth=2, label='Автономный режим')
# Рисуем начальную точку (зеленая)
self.ax.plot(self.trajectory_x[0], self.trajectory_y[0], 'go', markersize=8, label='Начальная точка')
# Рисуем текущую позицию (черная)
self.ax.plot(self.current_x, self.current_y, 'ko', markersize=6, label='Текущая позиция')
# Рисуем целевую точку (0, 0) - желтая
self.ax.plot(0, 0, 'yo', markersize=8, label='Цель (0, 0)')
self.ax.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)
# Учитываем целевую точку (0, 0)
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.set_xlim(x_min - margin, x_max + margin)
self.ax.set_ylim(y_min - margin, y_max + margin)
plt.draw()
plt.pause(0.25) # Небольшая пауза для обновления окна
def loop(self): def loop(self):
@@ -184,7 +111,7 @@ class Simulator:
# Добавляем начальную точку в траекторию # Добавляем начальную точку в траекторию
self.update_trajectory(0, 0) self.update_trajectory(0, 0)
sleep(2) self.viz_manager.update_global_map(self.current_x, self.current_y, self.mode)
for i in range(1000): for i in range(1000):
dangle = None dangle = None
@@ -200,15 +127,11 @@ class Simulator:
if dangle is None: if dangle is None:
break break
# Сдвиг камеры # Сдвиг камеры
action = ActionChains(self.driver) action = ActionChains(self.driver)
action.move_to_element_with_offset(html, 200, 200) action.move_to_element_with_offset(html, 200, 200)
action.click_and_hold() action.click_and_hold()
print(f" [Simulator] Drone Position: ({self.current_x:.2f}, {self.current_y:.2f})")
print(f" [Simulator] Angle: {self.angle * 180 / math.pi:.1f}°")
self.angle += dangle self.angle += dangle
dx = math.cos(self.angle) * velocity dx = math.cos(self.angle) * velocity
dy = math.sin(self.angle) * velocity dy = math.sin(self.angle) * velocity
@@ -232,17 +155,19 @@ class Simulator:
rotated_im.save(f"./images/{i}.png") rotated_im.save(f"./images/{i}.png")
# Обновляем карту каждые несколько итераций для производительности # Обновляем визуализацию каждые несколько итераций для производительности
if i % 5 == 0: if i % 5 == 0:
self.update_map() self.update_map()
self.viz_manager.update_display()
self.viz_manager.pause(0.25)
# Финальное обновление карты # Финальное обновление карты
self.update_map() self.update_map()
self.viz_manager.update_display()
print("last position: ", self.driver.current_url) print("last position: ", self.driver.current_url)
print(f"Финальная позиция: ({self.current_x:.2f}, {self.current_y:.2f})") print(f"Финальная позиция: ({self.current_x:.2f}, {self.current_y:.2f})")
# Показываем карту до закрытия # Показываем карту до закрытия, но не поднимаем на передний план
plt.ioff() self.viz_manager.show_final()
plt.show() print("Симуляция завершена. Окно визуализации остается открытым для анализа.")

221
visualization.py Normal file
View File

@@ -0,0 +1,221 @@
#!/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_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.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)
# Глобальная карта (левый верхний угол, занимает 2x1)
self.ax_global_map = self.fig.add_subplot(gs[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[1, 0])
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 = [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='Автономный режим')
# Рисуем начальную точку (зеленая)
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)
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_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']
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')
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("Симуляция завершена. Окно визуализации остается открытым для анализа.")
def pause(self, duration: float):
plt.pause(duration)