feat: motion graphic on gomography matrix

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(
@@ -324,6 +317,16 @@ class AutoPilot(Pilot):
                         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)
         src_pts, dst_pts, matches, kp1, kp2 = self.detect_and_match_keypoints(current_image, landmark_image)
@@ -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()

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

todo.md

@@ -1,4 +1,8 @@
-[-] График межкадрового смещения
+[+] График межкадрового смещения
+| [+] График межкадровых смещениях по версии матрицы гомографии
+| [+] Уменьшение погрешность за счёт удаления выбросов
+| [-] Исследовать причину погрешности при развороте
+
 [-] Отображение bounding box на графике сопоставления точек кадра с ориентиром
 [-] Проверка корректности выявления ориентира на кадре
 [-] Исправление коррекции координат на основе сопоставления с ориентиром
@@ -10,4 +14,4 @@
 
 [?] Изменение масштаба во время полёта, обработка этой трансформации
 [?] Поворот ориентиров
-[?] Ограничение выбора точек, чтобы ориентиры полностью попадали в кадр
+[?] Ограничение выбора точек при построении маршрута, чтобы ориентиры полностью попадали в кадр

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 = []
@@ -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_matches = self.fig.add_subplot(gs[0, 2])
-        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.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)
         
@@ -212,31 +219,6 @@ class VisualizationManager:
                 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()