fix: reduce position error

autopilot.py

@@ -67,15 +67,13 @@ class AutoPilot(Pilot):
 
         # Инициализация ORB детектора
         self.orb_detector = cv2.ORB_create(
-            nfeatures=3000,
-            scaleFactor=1.2,
-            nlevels=8,
-            edgeThreshold=15,
-            firstLevel=0,
-            WTA_K=2,
+            nfeatures=300,
+            scaleFactor=1.3,
+            nlevels=4,
+            edgeThreshold=19,
             patchSize=31,
-            fastThreshold=8,
-            scoreType=cv2.ORB_HARRIS_SCORE
+            fastThreshold=15,
+            scoreType=cv2.ORB_FAST_SCORE  # FAST обычно быстрее
         )
 
         # Инициализация матчера для сопоставления ключевых точек (kNN + Lowe ratio)
@@ -113,7 +111,7 @@ class AutoPilot(Pilot):
         
         if des1 is None or des2 is None or len(kp1) < 4 or len(kp2) < 4:
             return None, None, None, None, None
-        
+
         # kNN сопоставление и тест Лоу
         matches_knn = self.bf_matcher.knnMatch(des1, des2, k=2)
         good_matches: list[cv2.DMatch] = []
@@ -196,7 +194,7 @@ class AutoPilot(Pilot):
         a21, a22 = H[1, 0], H[1, 1]
         
         # Смещение (уже отцентрировано)
-        tx, ty = RH[0, 2] * MOVE_KOF, RH[1, 2] * MOVE_KOF
+        tx, ty = -H[0, 2] * MOVE_KOF, -H[1, 2] * MOVE_KOF
         
         print("  [Pilot] translate:", tx, ty)
         
@@ -317,54 +315,58 @@ 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, 
+                    self.vis_manager.update_motion_vectors(
+                        current_image,  
                         kp1, kp2,
                         np.array(matches)[mask.ravel().astype(bool)])
 
+                    
+                    # Используем новую визуализацию движения точек по сетке
+                    homography_matrix = transformation_info['homography']
+                    self.vis_manager.update_homography_grid(
+                        current_image, 
+                        homography_matrix, 
+                        grid_step=85)
+
         # Пытаемся найти ориентир на картинке:
-        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)
+        # 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)
 
-        if src_pts is not None and dst_pts is not None and self.vis_manager:
-            self.vis_manager.update_chunk_matches(current_image, landmark_image, kp1, kp2, matches)
+        # if src_pts is not None and dst_pts is not None and self.vis_manager:
+        #     self.vis_manager.update_chunk_matches(current_image, landmark_image, kp1, kp2, matches)
 
-        if src_pts is not None and dst_pts is not None:
-            # Оцениваем матрицу трансформации
-            landmark_transform = self.estimate_transformation_matrix(src_pts, dst_pts)
-            # Если ориентир достоверно найден — скорректируем глобальные координаты и угол
-            if landmark_transform is not None:
-                ok_scale = (self.min_scale <= landmark_transform['scale'] <= self.max_scale)
-                ok_inliers = (landmark_transform.get('inliers', 0) >= self.min_inliers)
-                ratio = landmark_transform.get('inliers_ratio', 0.0)
-                ok_ratio = (ratio >= self.min_inlier_ratio)
-                rmse = landmark_transform.get('rmse', None)
-                ok_rmse = (rmse is not None and rmse <= self.max_reproj_rmse)
-                if ok_scale and ok_inliers and ok_ratio and ok_rmse:
-                    print("[PILOT]", rmse, ratio, ok_rmse)
-                # if False:
-                    # Считаем абсолютную позу относительно координат ориентира
-                    landmark_world_x, landmark_world_y = self.points[self.target_idx]
-                    tx, ty = landmark_transform['translation']
-                    # Смещение в системе БПЛА: (dx_meters, dy_meters)
-                    dx_meters = ty
-                    dy_meters = tx
-                    # Используем оценённый абсолютный угол из гомографии относительно карты (север-вверх)
-                    absolute_angle = landmark_transform['rotation']
-                    cos_a = math.cos(absolute_angle)
-                    sin_a = math.sin(absolute_angle)
-                    # Переход в глобальные координаты карты
-                    dx_global = dx_meters * cos_a - dy_meters * sin_a
-                    dy_global = dx_meters * sin_a + dy_meters * cos_a
-                    self.x = landmark_world_x + dx_global
-                    self.y = landmark_world_y + dy_global
-                    self.angle = math.atan2(math.sin(absolute_angle), math.cos(absolute_angle))
-                    print(f"  [Pilot] Landmark correction applied (inliers={landmark_transform['inliers']}, ratio={ratio:.2f}, rmse={rmse:.2f})")
+        # if src_pts is not None and dst_pts is not None:
+        #     # Оцениваем матрицу трансформации
+        #     landmark_transform = self.estimate_transformation_matrix(src_pts, dst_pts)
+        #     # Если ориентир достоверно найден — скорректируем глобальные координаты и угол
+        #     if landmark_transform is not None:
+        #         ok_scale = (self.min_scale <= landmark_transform['scale'] <= self.max_scale)
+        #         ok_inliers = (landmark_transform.get('inliers', 0) >= self.min_inliers)
+        #         ratio = landmark_transform.get('inliers_ratio', 0.0)
+        #         ok_ratio = (ratio >= self.min_inlier_ratio)
+        #         rmse = landmark_transform.get('rmse', None)
+        #         ok_rmse = (rmse is not None and rmse <= self.max_reproj_rmse)
+        #         if ok_scale and ok_inliers and ok_ratio and ok_rmse:
+        #             print("[PILOT]", rmse, ratio, ok_rmse)
+        #         # if False:
+        #             # Считаем абсолютную позу относительно координат ориентира
+        #             landmark_world_x, landmark_world_y = self.points[self.target_idx]
+        #             tx, ty = landmark_transform['translation']
+        #             # Смещение в системе БПЛА: (dx_meters, dy_meters)
+        #             dx_meters = ty
+        #             dy_meters = tx
+        #             # Используем оценённый абсолютный угол из гомографии относительно карты (север-вверх)
+        #             absolute_angle = landmark_transform['rotation']
+        #             cos_a = math.cos(absolute_angle)
+        #             sin_a = math.sin(absolute_angle)
+        #             # Переход в глобальные координаты карты
+        #             dx_global = dx_meters * cos_a - dy_meters * sin_a
+        #             dy_global = dx_meters * sin_a + dy_meters * cos_a
+        #             self.x = landmark_world_x + dx_global
+        #             self.y = landmark_world_y + dy_global
+        #             self.angle = math.atan2(math.sin(absolute_angle), math.cos(absolute_angle))
+        #             print(f"  [Pilot] Landmark correction applied (inliers={landmark_transform['inliers']}, ratio={ratio:.2f}, rmse={rmse:.2f})")
 
         if distance_to_target < 75:
             self.target_idx += 1
@@ -372,6 +374,11 @@ class AutoPilot(Pilot):
             if self.target_idx == len(self.points):
                 return PilotCommand(stop=True)
 
+            distance_to_target = math.sqrt(
+                (self.points[self.target_idx][0] - self.x) ** 2 +
+                (self.points[self.target_idx][1] - self.y) ** 2
+            )
+
         # Вычисляем угол к цели
         target_angle = math.atan2(self.points[self.target_idx][1] - self.y, self.points[self.target_idx][0] - self.x)
         

test_autopilot.ipynb

@@ -6,81 +6,11 @@
    "metadata": {},
    "outputs": [
     {
-     "name": "stdout",
+     "name": "stderr",
      "output_type": "stream",
      "text": [
-      "\n",
-      "  [Pilot] translate: 0.05017116751607286 9.687502397756333\n",
-      "  [Pilot] Drone Position: (9.63, 1.05)\n",
-      "  [Pilot] Angle: 5.9°\n",
-      "  [Pilot] Target Index: 0\n",
-      "  [Pilot] Target Position: (1500, 1000)\n",
-      "  [Pilot] Distance: 1802.7756377319947\n",
-      "  [Pilot] translate: 34.49815603085627 29.137861884347547\n",
-      "\n",
-      "  [Pilot] translate: -3.880257616756106 51.463125720098006\n",
-      "  [Pilot] Drone Position: (60.82, 7.61)\n",
-      "  [Pilot] Angle: 11.6°\n",
-      "  [Pilot] Target Index: 0\n",
-      "  [Pilot] Target Position: (1500, 1000)\n",
-      "  [Pilot] Distance: 1794.1861357457751\n",
-      "  [Pilot] translate: -81.10123958005454 -137.34212482489963\n",
-      "\n",
-      "  [Pilot] translate: -4.965415715097187 51.25399608516978\n",
-      "  [Pilot] Drone Position: (111.23, 18.11)\n",
-      "  [Pilot] Angle: 17.3°\n",
-      "  [Pilot] Target Index: 0\n",
-      "  [Pilot] Target Position: (1500, 1000)\n",
-      "  [Pilot] Distance: 1748.165148948221\n",
-      "  [Pilot] translate: 45.12386596220679 -80.30038654495203\n",
-      "\n",
-      "  [Pilot] translate: -4.306576877574656 51.03219412293733\n",
-      "  [Pilot] Drone Position: (159.88, 34.13)\n",
-      "  [Pilot] Angle: 23.1°\n",
-      "  [Pilot] Target Index: 0\n",
-      "  [Pilot] Target Position: (1500, 1000)\n",
-      "  [Pilot] Distance: 1700.8189617850037\n",
-      "  [Pilot] translate: 21.716104890355993 -140.57795924978092\n",
-      "\n",
-      "  [Pilot] translate: -4.698241337005581 50.82696807577958\n",
-      "  [Pilot] Drone Position: (206.68, 54.50)\n",
-      "  [Pilot] Angle: 28.8°\n",
-      "  [Pilot] Target Index: 0\n",
-      "  [Pilot] Target Position: (1500, 1000)\n",
-      "  [Pilot] Distance: 1651.9184793917282\n",
-      "  [Pilot] translate: -105.83895859495036 -240.1764870881905\n",
-      "\n",
-      "  [Pilot] translate: -4.958766997526737 51.41923183115156\n",
-      "  [Pilot] Drone Position: (251.86, 79.55)\n",
-      "  [Pilot] Angle: 34.5°\n",
-      "  [Pilot] Target Index: 0\n",
-      "  [Pilot] Target Position: (1500, 1000)\n",
-      "  [Pilot] Distance: 1602.0752180361662\n",
-      "  [Pilot] translate: -148.22727851217576 277.70124157634604\n",
-      "\n",
-      "  [Pilot] translate: -3.092076978507596 51.453303771043075\n",
-      "  [Pilot] Drone Position: (294.20, 108.95)\n",
-      "  [Pilot] Angle: 38.2°\n",
-      "  [Pilot] Target Index: 0\n",
-      "  [Pilot] Target Position: (1500, 1000)\n",
-      "  [Pilot] Distance: 1550.834822541291\n",
-      "  [Pilot] translate: 84.73919078536481 6.965084966855142\n",
-      "\n",
-      "  [Pilot] translate: 0.6990361118758528 51.266860568260554\n",
-      "  [Pilot] Drone Position: (333.90, 141.39)\n",
-      "  [Pilot] Angle: 38.5°\n",
-      "  [Pilot] Target Index: 0\n",
-      "  [Pilot] Target Position: (1500, 1000)\n",
-      "  [Pilot] Distance: 1499.310186566892\n",
-      "  [Pilot] translate: -169.29950811743842 109.96288683148794\n",
-      "\n",
-      "  [Pilot] translate: 0.06894026606203722 51.71176082802062\n",
-      "  [Pilot] Drone Position: (374.40, 173.54)\n",
-      "  [Pilot] Angle: 38.4°\n",
-      "  [Pilot] Target Index: 0\n",
-      "  [Pilot] Target Position: (1500, 1000)\n",
-      "  [Pilot] Distance: 1448.1010213418751\n",
-      "  [Pilot] translate: -175.2651402920743 -178.56804299442655\n"
+      "c:\\Users\\admin\\Projects\\autopilot\\visualization.py:113: UserWarning: This figure includes Axes that are not compatible with tight_layout, so results might be incorrect.\n",
+      "  plt.tight_layout()\n"
      ]
     }
    ],
@@ -91,27 +21,81 @@
     "from autopilot import AutoPilot\n",
     "from visualization import VisualizationManager\n",
     "\n",
-    "autopilot = AutoPilot([(1500, 1000)], [])\n",
-    "imgs = [Image.open(Path('images') / f'photo_{i}.png') for i in range(10)]\n",
-    "autopilot.handle(imgs[0])\n",
-    "for i in range(1, 10):\n",
-    "    print()\n",
-    "    autopilot.handle(imgs[i])\n"
+    "vzm = VisualizationManager()\n",
+    "\n"
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": 6,
+   "execution_count": 3,
+   "metadata": {},
+   "outputs": [
+    {
+     "data": {
+      "text/plain": [
+       "<autopilot.PilotCommand at 0x2696e2decd0>"
+      ]
+     },
+     "execution_count": 3,
+     "metadata": {},
+     "output_type": "execute_result"
+    }
+   ],
+   "source": [
+    "autopilot = AutoPilot([(0, 0), (1500, 1000)], [], vzm)\n",
+    "imgs = [Image.open(Path('images') / f'photo_{i}.png') for i in range(10)]\n",
+    "autopilot.handle(imgs[0])\n"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 4,
    "metadata": {},
    "outputs": [
     {
      "name": "stdout",
      "output_type": "stream",
      "text": [
-      "None\n"
+      "\n",
+      "  [Pilot] translate: -0.28939364297583925 8.879763334079307\n",
+      "  [Pilot] Drone Position: (8.87, 0.58)\n",
+      "  [Pilot] Angle: 5.6°\n",
+      "  [Pilot] Target Index: 0\n",
+      "  [Pilot] Target Position: (0, 0)\n",
+      "  [Pilot] Distance: 0.0\n",
+      "0.1 50.0\n",
+      "\n",
+      "  [Pilot] translate: -4.194654791266826 51.7432078133843\n",
+      "  [Pilot] Drone Position: (60.43, 6.60)\n",
+      "  [Pilot] Angle: 11.3°\n",
+      "  [Pilot] Target Index: 1\n",
+      "  [Pilot] Target Position: (1500, 1000)\n",
+      "  [Pilot] Distance: 1795.0821692652123\n",
+      "0.1 50.0\n",
+      "\n",
+      "  [Pilot] translate: -4.752188299516379 51.16691704857423\n",
+      "  [Pilot] Drone Position: (110.73, 17.12)\n",
+      "  [Pilot] Angle: 17.1°\n",
+      "  [Pilot] Target Index: 1\n",
+      "  [Pilot] Target Position: (1500, 1000)\n",
+      "  [Pilot] Distance: 1749.0606665752869\n",
+      "0.1 50.0\n"
      ]
     }
    ],
+   "source": [
+    "for i in range(1, 4):\n",
+    "    print()\n",
+    "    command = autopilot.handle(imgs[i])\n",
+    "    print(command.dangle, command.velocity)\n",
+    "    vzm.pause(0.5)\n"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
    "source": []
   }
  ],

todo.md

@@ -1,16 +1,18 @@
 [+] График межкадрового смещения
 | [+] График межкадровых смещениях по версии матрицы гомографии
 | [+] Уменьшение погрешность за счёт удаления выбросов
-| [-] Исследовать причину погрешности при развороте
+| [+] Исследовать причину погрешности при развороте
+[+] Устранение большой погрешности при повороте
 
 [-] Отображение bounding box на графике сопоставления точек кадра с ориентиром
 [-] Проверка корректности выявления ориентира на кадре
 [-] Исправление коррекции координат на основе сопоставления с ориентиром
 [-] FPS счетчик
-[-] Устранение большой погрешности при повороте
+| [-] Оптимизация детекции точек
 [-] Оформление статистики при тестовых запусках
 [-] Проведение тестовых запусков
 [-] Оформление отчета
+[-] Эксперименты с разными детекторами (SIFT, KAZE)
 
 [?] Изменение масштаба во время полёта, обработка этой трансформации
 [?] Поворот ориентиров

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):
         """Обновляет отображение всех областей"""