From e5715e17da4faaf3fec70ec964eac32094548cee Mon Sep 17 00:00:00 2001 From: russian_proger Date: Sat, 4 Oct 2025 23:08:26 +0300 Subject: [PATCH] feat: motion graphic on gomography matrix --- autopilot.py | 59 +++--------------- main.py | 3 +- todo.md | 8 ++- visualization.py | 156 +++++++++++++++++++++++++++++++++++------------ 4 files changed, 135 insertions(+), 91 deletions(-) diff --git a/autopilot.py b/autopilot.py index 46f63aa..0a4c8ef 100644 --- a/autopilot.py +++ b/autopilot.py @@ -272,11 +272,6 @@ class AutoPilot(Pilot): height, width = self.prev_image.shape[:2] self.image_center = (width // 2, height // 2) - # Обновляем визуализацию детекции - if self.vis_manager is not None: - kp, _ = self.orb_detector.detectAndCompute(self.prev_image, None) - self.vis_manager.update_detection(self.prev_image, kp) - return PilotCommand() # Конвертируем текущее изображение @@ -314,8 +309,6 @@ class AutoPilot(Pilot): # Обновляем визуализацию if self.vis_manager: - # Обновляем детекцию ключевых точек - self.vis_manager.update_detection(current_image, kp2) # Обновляем сопоставление точек self.vis_manager.update_matches( @@ -323,6 +316,16 @@ class AutoPilot(Pilot): current_image, kp1, kp2, matches, transformation_info) + + self.vis_manager.update_motion_vectors( + current_image, + kp1, kp2, matches) + + mask = transformation_info['mask'] + self.vis_manager.update_motion_gomography( + current_image, + kp1, kp2, + np.array(matches)[mask.ravel().astype(bool)]) # Пытаемся найти ориентир на картинке: landmark_image = cv2.imread(Path('chunks') / f'chunk_{self.target_idx}.png', cv2.IMREAD_COLOR_RGB) @@ -389,45 +392,3 @@ class AutoPilot(Pilot): self.y = y self.angle = angle self.frame_count = 0 - - -class RandomPilot(Pilot): - counter: int - - def __init__(self, velocity: float = 1): - self.counter = 0 - - def act(self) -> tuple[float, float] | None: - self.counter += 1 - if self.counter > 300: - return None - - return 1 / (self.counter + 20), 10.0 - -# def _test(): -# randomPilot = RandomPilot() -# point = [0, 0] -# iter_count = 100 -# points = [point.copy()] - -# for i in range(iter_count): -# dx, dy = randomPilot.step() -# prev_point = point.copy() -# point[0] += dx -# point[1] += dy -# points.append(point.copy()) - -# coords = list(zip(*points)) -# padding = 5 -# plt.axis([ -# min(coords[0]) - padding, max(coords[0]) + padding, -# min(coords[1]) - padding, max(coords[1]) + padding]) - -# for i in range(iter_count): -# plt.plot(coords[0][i:i+2], coords[1][i:i+2], color='#5e5') -# plt.pause(0.05) - -# sleep(1) - -# if __name__ == '__main__': -# _test() \ No newline at end of file diff --git a/main.py b/main.py index 0c03ad5..6a646b1 100644 --- a/main.py +++ b/main.py @@ -1,4 +1,3 @@ -from autopilot import AutoPilot, RandomPilot from simulator import Simulator from visualization import VisualizationManager from trajectory_drawer import TrajectoryDrawer @@ -112,7 +111,7 @@ def main(): photo.save(Path('images') / f"photo_{i}.png") vis_manager.update_display() - vis_manager.pause(1) + vis_manager.pause(0.2) if command.stop: break diff --git a/todo.md b/todo.md index f19db0f..eb2df9b 100644 --- a/todo.md +++ b/todo.md @@ -1,4 +1,8 @@ -[-] График межкадрового смещения +[+] График межкадрового смещения +| [+] График межкадровых смещениях по версии матрицы гомографии +| [+] Уменьшение погрешность за счёт удаления выбросов +| [-] Исследовать причину погрешности при развороте + [-] Отображение bounding box на графике сопоставления точек кадра с ориентиром [-] Проверка корректности выявления ориентира на кадре [-] Исправление коррекции координат на основе сопоставления с ориентиром @@ -10,4 +14,4 @@ [?] Изменение масштаба во время полёта, обработка этой трансформации [?] Поворот ориентиров -[?] Ограничение выбора точек, чтобы ориентиры полностью попадали в кадр +[?] Ограничение выбора точек при построении маршрута, чтобы ориентиры полностью попадали в кадр diff --git a/visualization.py b/visualization.py index f4a5757..c7ad133 100644 --- a/visualization.py +++ b/visualization.py @@ -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()