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

feat: motion graphic on gomography matrix

Browse Source
This commit is contained in:
russian_proger
2025-10-04 23:08:26 +03:00
parent dc8c869bcf
commit e5715e17da
4 changed files with 135 additions and 91 deletions
Download Patch File Download Diff File Expand all files Collapse all files

156
visualization.py
View File

@@ -3,6 +3,7 @@
Модуль для управления общим окном визуализации
"""
import matplotlib.axes
import matplotlib.pyplot as plt
import matplotlib.patches as patches
import numpy as np
@@ -32,6 +33,7 @@ class VisualizationManager:
self.ax_detection = None
self.ax_matches = None
self.ax_chunk_matches = None
self.ax_motion_vectors = None
# Данные для глобальной карты
self.trajectory_x = []
@@ -54,7 +56,7 @@ class VisualizationManager:
self.current_frame = None
self.keypoints = []
self.matches = []
self._setup_window()
def _setup_window(self):
@@ -66,7 +68,7 @@ class VisualizationManager:
# Открываем окно на полный экран
self.fig.canvas.manager.window.state('zoomed')
# Создаем сетку 2x2 с разными размерами колонок
# Создаем сетку 3x3 с разными размерами колонок
gs = self.fig.add_gridspec(2, 3, hspace=0.3, wspace=0.3, width_ratios=[1, 0.7, 1])
# График погрешности позиции (левый верхний угол)
@@ -76,7 +78,7 @@ class VisualizationManager:
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 координата')
@@ -85,21 +87,26 @@ class VisualizationManager:
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, 2])
# Сопоставление точек (правый верхний угол)
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[0, 2])
# Сопоставление точек (средний средний угол)
self.ax_chunk_matches = self.fig.add_subplot(gs[1, 2])
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)
@@ -211,32 +218,7 @@ class VisualizationManager:
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):
"""Обновляет визуализацию сопоставления точек"""
@@ -305,6 +287,104 @@ class VisualizationManager:
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_display(self):
"""Обновляет отображение всех областей"""
self.fig.canvas.draw()