fix: reduce position error

visualization.py

@@ -75,7 +75,7 @@ class VisualizationManager:
         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.set_ylabel('Погрешность (пиксели)')
         self.ax_error_plot.grid(True, alpha=0.3)
         
         # Глобальная карта (левый средний угол)
@@ -384,6 +384,152 @@ class VisualizationManager:
 
     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_homography_grid(self, current_frame: np.ndarray, homography_matrix: np.ndarray, grid_step: int = 80):
+        """
+        Визуализирует движение точек по сетке на основе матрицы гомографии
+        """
+        self.ax_motion_gomography.clear()
+        self.ax_motion_gomography.set_title('Движение точек по сетке')
+        
+        if current_frame is None or homography_matrix is None:
+            self.ax_motion_gomography.axis('off')
+            return
+            
+        # Конвертируем 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
+        
+        # Показываем текущий кадр
+        self.ax_motion_gomography.imshow(frame_rgb)
+        
+        # Получаем размеры изображения и центр
+        height, width = current_frame.shape[:2]
+        center_x, center_y = width // 2, height // 2
+        
+        # Создаем сетку точек с заданным шагом
+        grid_points = []
+        for y in range(grid_step, height, grid_step):
+            for x in range(grid_step, width, grid_step):
+                grid_points.append([x, y])
+        
+        if len(grid_points) == 0:
+            self.ax_motion_gomography.axis('off')
+            return
+            
+        # Конвертируем в numpy массив и отцентрируем координаты относительно центра изображения
+        grid_points = np.array(grid_points, dtype=np.float32)
+        grid_points_centered = []
+        for pt in grid_points:
+            # Отцентрируем координаты точно так же, как в detect_and_match_keypoints
+            centered_x = pt[0] - center_x
+            centered_y = center_y - pt[1]  # Инвертируем Y (изображение Y направлен вниз)
+            grid_points_centered.append([centered_x, centered_y])
+        
+        grid_points_centered = np.array(grid_points_centered, dtype=np.float32)
+        grid_points_homogeneous = np.column_stack([grid_points_centered, np.ones(len(grid_points_centered))])
+        
+        # Применяем матрицу гомографии
+        transformed_points_homogeneous = homography_matrix @ grid_points_homogeneous.T
+        transformed_points_homogeneous = transformed_points_homogeneous.T
+        
+        # Нормализуем по третьей координате (перспективное преобразование)
+        transformed_points_centered = transformed_points_homogeneous[:, :2] / transformed_points_homogeneous[:, 2:3]
+        
+        # Конвертируем обратно в координаты изображения
+        transformed_points = []
+        for pt in transformed_points_centered:
+            # Обратное преобразование от центрированных координат к координатам изображения
+            img_x = pt[0] + center_x
+            img_y = center_y - pt[1]  # Инвертируем Y обратно
+            transformed_points.append([img_x, img_y])
+        
+        transformed_points = np.array(transformed_points, dtype=np.float32)
+        
+        # Фильтруем точки, которые остались в пределах изображения
+        valid_indices = (
+            (transformed_points[:, 0] >= 0) & 
+            (transformed_points[:, 0] < width) & 
+            (transformed_points[:, 1] >= 0) & 
+            (transformed_points[:, 1] < height)
+        )
+        
+        if np.sum(valid_indices) == 0:
+            self.ax_motion_gomography.axis('off')
+            return
+            
+        # Получаем валидные исходные и трансформированные точки
+        valid_source_points = grid_points[valid_indices]
+        valid_transformed_points = transformed_points[valid_indices]
+        
+        # Вычисляем векторы движения
+        motion_vectors = valid_transformed_points - valid_source_points
+        
+        # Вычисляем длину и направление векторов
+        vector_lengths = np.linalg.norm(motion_vectors, axis=1)
+        
+        if len(vector_lengths) > 0:
+            # Нормализуем длины для цветовой карты (0-1)
+            max_length = np.max(vector_lengths)
+            if max_length > 0:
+                normalized_lengths = vector_lengths / max_length
+            else:
+                normalized_lengths = np.zeros_like(vector_lengths)
+                
+            # Рисуем векторы движения
+            for i, (start_pt, end_pt, length, norm_length) in enumerate(
+                zip(valid_source_points, valid_transformed_points, vector_lengths, normalized_lengths)):
+                
+                # Пропускаем очень маленькие векторы
+                if length < 1.0:
+                    continue
+                    
+                # Цвет зависит от длины вектора (синий -> красный)
+                if norm_length < 0.5:
+                    color = [0, norm_length * 2, 1 - norm_length * 2]  # Синий -> Голубой
+                else:
+                    color = [(norm_length - 0.5) * 2, 1 - (norm_length - 0.5) * 2, 0]  # Голубой -> Красный
+                
+                # Толщина линии зависит от длины вектора
+                linewidth = max(1, min(4, 1 + 3 * norm_length))
+                
+                # Рисуем вектор
+                self.ax_motion_gomography.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=color, ec=color, alpha=0.8, linewidth=linewidth
+                )
+                
+                # Рисуем исходную точку сетки
+                self.ax_motion_gomography.plot(start_pt[0], start_pt[1], 'o', 
+                                            color='green', markersize=3, alpha=0.7)
+                
+                # Рисуем целевую точку
+                self.ax_motion_gomography.plot(end_pt[0], end_pt[1], 's', 
+                                            color='red', markersize=2, alpha=0.7)
+            
+            # Добавляем информацию о статистике
+            avg_length = np.mean(vector_lengths)
+            max_length = np.max(vector_lengths)
+            total_points = len(vector_lengths)
+            
+            # info_text = f"Точек сетки: {total_points}\nСреднее движение: {avg_length:.1f}px\nМакс. движение: {max_length:.1f}px"
+            # self.ax_motion_gomography.text(
+            #     10, 30, info_text, fontsize=8, color='white',
+            #     bbox=dict(boxstyle="round,pad=0.3", facecolor="black", alpha=0.8)
+            # )
+            
+            # Добавляем легенду
+            # legend_text = "Зеленые точки: исходные\nКрасные квадраты: целевые\nЦвет стрелок: скорость"
+            # self.ax_motion_gomography.text(
+            #     10, 90, legend_text, fontsize=7, color='white',
+            #     bbox=dict(boxstyle="round,pad=0.3", facecolor="black", alpha=0.8)
+            # )
+        
+        self.ax_motion_gomography.axis('off')
 
     def update_display(self):
         """Обновляет отображение всех областей"""