ref: test version

2025-10-04 14:04:32 +03:00
parent 7610fc6f50
commit dc8c869bcf
7 changed files with 225 additions and 154 deletions
--- a/a.ipynb
+++ b/a.ipynb
--- a/autopilot.py
+++ b/autopilot.py
@@ -58,20 +58,28 @@ class AutoPilot(Pilot):
        self.image_center = (0, 0)  # Будет обновлено при получении первого изображения
        self.vis_manager = viz_manager  # Менеджер визуализации

+        # Пороговые значения качества сопоставления/гомографии
+        self.min_inliers: int = 12
+        self.min_inlier_ratio: float = 0.5
+        self.max_reproj_rmse: float = 3.0
+        self.min_scale: float = 0.7
+        self.max_scale: float = 1.4
+
        # Инициализация ORB детектора
        self.orb_detector = cv2.ORB_create(
-            nfeatures=1000,
+            nfeatures=3000,
            scaleFactor=1.2,
            nlevels=8,
-            edgeThreshold=31,
+            edgeThreshold=15,
            firstLevel=0,
            WTA_K=2,
            patchSize=31,
-            fastThreshold=20
+            fastThreshold=8,
+            scoreType=cv2.ORB_HARRIS_SCORE
        )

-        # Инициализация матчера для сопоставления ключевых точек
-        self.bf_matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
+        # Инициализация матчера для сопоставления ключевых точек (kNN + Lowe ratio)
+        self.bf_matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=False)

        self.points = points
        self.keypoints = keypoints
@@ -88,31 +96,44 @@ class AutoPilot(Pilot):
        """
        Обнаруживает ключевые точки ORB и сопоставляет их между двумя изображениями
        """
-        # Обнаружение ключевых точек и дескрипторов
-        kp1, des1 = self.orb_detector.detectAndCompute(img1, None)
-        kp2, des2 = self.orb_detector.detectAndCompute(img2, None)
+        # Предобработка (повышение контраста и устойчивости)
+        def preprocess(gray_img: np.ndarray) -> np.ndarray:
+            clahe = cv2.createCLAHE(clipLimit=3.0, tileGridSize=(8, 8))
+            return clahe.apply(gray_img)
        
-        if des1 is None or des2 is None:
+        # В градации серого
+        g1 = cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY) if len(img1.shape) == 3 else img1
+        g2 = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY) if len(img2.shape) == 3 else img2
+        g1 = preprocess(g1)
+        g2 = preprocess(g2)
+        
+        # Обнаружение ключевых точек и дескрипторов
+        kp1, des1 = self.orb_detector.detectAndCompute(g1, None)
+        kp2, des2 = self.orb_detector.detectAndCompute(g2, None)
+        
+        if des1 is None or des2 is None or len(kp1) < 4 or len(kp2) < 4:
            return None, None, None, None, None
        
-        # Сопоставление ключевых точек
-        matches = self.bf_matcher.match(des1, des2)
+        # kNN сопоставление и тест Лоу
+        matches_knn = self.bf_matcher.knnMatch(des1, des2, k=2)
+        good_matches: list[cv2.DMatch] = []
+        for m_n in matches_knn:
+            if len(m_n) < 2:
+                continue
+            m, n = m_n
+            if m.distance < 0.75 * n.distance:
+                good_matches.append(m)
        
-        # Сортировка совпадений по расстоянию
-        matches = sorted(matches, key=lambda x: x.distance)
-        
-        # Фильтрация хороших совпадений (расстояние меньше порога)
-        good_matches = []
-        for match in matches:
-            if match.distance < 50:  # Порог расстояния
-                good_matches.append(match)
+        # Дополнительная фильтрация по расстоянию (мягкий порог)
+        good_matches = sorted(good_matches, key=lambda x: x.distance)
+        good_matches = [m for m in good_matches if m.distance < 64]
        
        if len(good_matches) < 4:
            return None, None, None, None, None
        
        # Получаем центр изображения
-        height1, width1 = img1.shape[:2]
-        height2, width2 = img2.shape[:2]
+        height1, width1 = g1.shape[:2]
+        height2, width2 = g2.shape[:2]
        center_x1, center_y1 = width1 // 2, height1 // 2
        center_x2, center_y2 = width2 // 2, height2 // 2
        
@@ -147,6 +168,24 @@ class AutoPilot(Pilot):
        if H is None:
            return None
        
+        # Маска инлайеров
+        inliers = int(mask.sum()) if mask is not None else 0
+        total = int(mask.size) if mask is not None else 0
+        inlier_ratio = (inliers / total) if total > 0 else 0.0
+        
+        # Ошибка репроекции по инлайерам (RMSE)
+        rmse = None
+        try:
+            # Проецируем src через H и сравниваем с dst
+            proj = cv2.perspectiveTransform(src_pts, H)
+            errs = np.linalg.norm((proj - dst_pts).reshape(-1, 2), axis=1)
+            if mask is not None and mask.shape[0] == errs.shape[0]:
+                errs = errs[mask.ravel().astype(bool)]
+            if errs.size > 0:
+                rmse = float(np.sqrt(np.mean(errs ** 2)))
+        except Exception:
+            rmse = None
+        
        # Извлекаем параметры трансформации из матрицы гомографии
        # H = [a11 a12 tx]
        #     [a21 a22 ty]
@@ -174,7 +213,10 @@ class AutoPilot(Pilot):
            'rotation': angle,
            'scale': scale,
            'homography': H,
-            'mask': mask
+            'mask': mask,
+            'inliers': inliers,
+            'inliers_ratio': inlier_ratio,
+            'rmse': rmse
        }

    def update_drone_position(self, transformation_info: dict):
@@ -190,6 +232,9 @@ class AutoPilot(Pilot):
        dx_meters = ty
        dy_meters = tx

+        # Обновляем угол БПЛА
+        self.angle += rotation
+        
        # Применяем поворот к смещению (учитываем текущий угол БПЛА)
        cos_angle = math.cos(self.angle)
        sin_angle = math.sin(self.angle)
@@ -203,9 +248,6 @@ class AutoPilot(Pilot):
        self.x += dx_global
        self.y += dy_global
        
-        # Обновляем угол БПЛА
-        self.angle += rotation
-        
        # Нормализуем угол в диапазоне [-π, π]
        self.angle = math.atan2(math.sin(self.angle), math.cos(self.angle))

@@ -221,41 +263,6 @@ class AutoPilot(Pilot):
            'frame_count': self.frame_count
        }

-    def visualize_matches(self, img1: np.ndarray, img2: np.ndarray, 
-                         kp1, kp2, matches, transformation_info):
-        """
-        Визуализирует сопоставленные ключевые точки и трансформацию
-        """
-        # Рисуем сопоставления
-        img_matches = cv2.drawMatches(img1, kp1, img2, kp2, matches, None,
-                                     flags=cv2.DrawMatchesFlags_NOT_DRAW_SINGLE_POINTS)
-        
-        # Добавляем информацию о трансформации
-        if transformation_info:
-            tx, ty = transformation_info['translation']
-            angle = transformation_info['rotation']
-            scale = transformation_info['scale']
-            
-            info_text = f"Translation: ({tx:.2f}, {ty:.2f})"
-            info_text2 = f"Rotation: {angle:.2f} rad ({np.degrees(angle):.1f}°)"
-            info_text3 = f"Scale: {scale:.2f}"
-            info_text4 = f"Image Center: {self.image_center}"
-            
-            cv2.putText(img_matches, info_text, (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
-            cv2.putText(img_matches, info_text2, (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
-            cv2.putText(img_matches, info_text3, (10, 90), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
-            cv2.putText(img_matches, info_text4, (10, 120), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
-        
-        # Добавляем информацию о позиции БПЛА
-        drone_state = self.get_drone_state()
-        pos_text = f"Drone Pos: ({drone_state['x']:.2f}, {drone_state['y']:.2f})"
-        angle_text = f"Drone Angle: {drone_state['angle_degrees']:.1f}°"
-        
-        cv2.putText(img_matches, pos_text, (10, 150), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 0, 0), 2)
-        cv2.putText(img_matches, angle_text, (10, 180), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 0, 0), 2)
-        
-        return img_matches
-
    def handle(self, image: Image) -> PilotCommand:
        self.frame_count += 1

@@ -294,13 +301,11 @@ class AutoPilot(Pilot):
            transformation_info = self.estimate_transformation_matrix(src_pts, dst_pts)
            
            if transformation_info:
-                # Обновляем позицию и угол БПЛА
+                # Обновляем позицию и угол БПЛА (одометрия по кадрам)
                self.update_drone_position(transformation_info)
                
                # Выводим текущее состояние БПЛА
                drone_state = self.get_drone_state()
-                if self.vis_manager:
-                    self.vis_manager.update_drone_trajectory(drone_state['x'], drone_state['y'])
                print(f"  [Pilot] Drone Position: ({drone_state['x']:.2f}, {drone_state['y']:.2f})")
                print(f"  [Pilot] Angle: {drone_state['angle_degrees']:.1f}°")
                print(f"  [Pilot] Target Index: {self.target_idx}")
@@ -322,13 +327,43 @@ class AutoPilot(Pilot):
        # Пытаемся найти ориентир на картинке:
        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:
-        #     # Оцениваем матрицу трансформации
-        #     transformation_info = self.estimate_transformation_matrix(src_pts, dst_pts)
-        #     if self.vis_manager:
-        #         self.vis_manager.update_chunk_matches(current_image, landmark_image, kp1, kp2, matches, transformation_info)

-        if distance_to_target < 50:
+        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 distance_to_target < 75:
            self.target_idx += 1

            if self.target_idx == len(self.points):
--- a/main.py
+++ b/main.py
@@ -29,27 +29,28 @@ def main():
    # make_global_photo('map.jpg')

    # Получаем траекторию от пользователя
-    points = get_trajectory_points('map.jpg')
-    print(points)
+    # points = get_trajectory_points('map.jpg')
+    # print(points)
 #     points = [np.float64(0.5443937502226799), np.float64(0.4030424838774785)], [np.float64(0.18517133490120316), np.float64(0.4586935604608052)], [np.float64(0.1641887171838272), np.float64(0.5586383510594329)], [np.float64(0.587198290366127), 
 # np.float64(0.5699957136274587)]
+    points = [[np.float64(0.481445897070552), np.float64(0.49958006570569835)], [np.float64(0.5485902737661552), np.float64(0.4325716265543457)], [np.float64(0.5376793125531197), np.float64(0.6052035375883389)], [np.float64(0.34044270600978543), np.float64(0.6052035375883389)], [np.float64(0.34044270600978543), np.float64(0.3689703961734008)], [np.float64(0.6333600493443542), np.float64(0.3224052096444948)], [np.float64(0.6686108471095458), np.float64(0.5518239335186174)]]

    # Для каждой точки сделаем приближенный снимок
    yandexMap = YandexMap()
    keypoints: list[(any, any)] = []

-    plt.ion()
-    for i in range(len(points)):
-        point = points[i]
-        yandexMap.scroll(point[0], point[1], 5, True)
-        sleep(1)
-        img = yandexMap.make_screenshot(point[0], point[1], 0.2, 0.2)
-        Path('chunks').mkdir(exist_ok=True)
-        cv2.imwrite(Path('.') / 'chunks' / f'chunk_{i}.png', img)
-        plt.subplot(1, len(points), i+1)
-        plt.imshow(img)
-        plt.pause(0.25)
-        yandexMap.scroll(point[0], point[1], 5, False)
+    # plt.ion()
+    # for i in range(len(points)):
+    #     point = points[i]
+    #     yandexMap.scroll(point[0], point[1], 5, True)
+    #     sleep(1)
+    #     img = yandexMap.make_screenshot(point[0], point[1], 0.2, 0.2)
+    #     Path('chunks').mkdir(exist_ok=True)
+    #     cv2.imwrite(Path('.') / 'chunks' / f'chunk_{i}.png', img)
+    #     plt.subplot(1, len(points), i+1)
+    #     plt.imshow(img)
+    #     plt.pause(0.25)
+    #     yandexMap.scroll(point[0], point[1], 5, False)

    plt.tight_layout()

@@ -81,9 +82,9 @@ def main():

    # Начнём симуляцию полёта с первой точки
    yandexMap.scroll(points[0][0], points[0][1], 5, True)
-    sleep(1)
+    sleep(0.2)
    yandexMap.make_as_center(*points[0])
-    sleep(5)
+    sleep(1)

    vis_manager = VisualizationManager()
    width, height = yandexMap.get_size()
@@ -97,11 +98,11 @@ def main():

    photo = simulator.get_photo()
    command = pilot.handle(photo)
-    print("DEBUG SIZE:", yandexMap.get_size())
-    print("DEBUG SIZE:", photo.size)

    vis_manager.update_display()
-    vis_manager.pause(5)
+    vis_manager.pause(1)
+
+    vis_manager.set_target_points(points_coords)

    for i in range(10000000000):
        photo = simulator.get_photo()
@@ -116,11 +117,17 @@ def main():
        if command.stop:
            break

+        vis_manager.set_target_index(pilot.target_idx)
+        vis_manager.update_drone_trajectory(pilot.x, pilot.y)
+        vis_manager.update_global_map(simulator.current_x, simulator.current_y)
+        vis_manager.update_error_plot(i, pilot.x, pilot.y, simulator.current_x, simulator.current_y)
+
        simulator.handle(command.dangle, command.velocity)
+
        vis_manager.update_display()
        vis_manager.pause(0.2)
-    vis_manager.pause(50)
-    sleep(30)
+
+    vis_manager.show_final()

 if __name__ == "__main__":
    main()
--- a/map.jpg
+++ b/map.jpg
--- a/test_autopilot.ipynb
+++ b/test_autopilot.ipynb
@@ -10,68 +10,77 @@
     "output_type": "stream",
     "text": [
      "\n",
-      "  [Pilot] translate: -1.1016611435961554 -11.000654444967525\n",
-      "  [Pilot] Drone Position: (11.00, 1.10)\n",
-      "  [Pilot] Angle: 5.7°\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: -1.3733859493278928 -62.101186076671574\n",
-      "  [Pilot] Drone Position: (72.66, 8.66)\n",
-      "  [Pilot] Angle: 7.0°\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: 1793.018946479205\n",
+      "  [Pilot] Distance: 1794.1861357457751\n",
+      "  [Pilot] translate: -81.10123958005454 -137.34212482489963\n",
      "\n",
-      "  [Pilot] translate: -0.286107956133153 -61.700187952027996\n",
-      "  [Pilot] Drone Position: (133.86, 16.44)\n",
-      "  [Pilot] Angle: 7.3°\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: 1737.8358192949638\n",
+      "  [Pilot] Distance: 1748.165148948221\n",
+      "  [Pilot] translate: 45.12386596220679 -80.30038654495203\n",
      "\n",
-      "  [Pilot] translate: -1.217279449147543 -61.59798129394066\n",
-      "  [Pilot] Drone Position: (194.81, 25.43)\n",
-      "  [Pilot] Angle: 8.4°\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: 1683.363020511812\n",
+      "  [Pilot] Distance: 1700.8189617850037\n",
+      "  [Pilot] translate: 21.716104890355993 -140.57795924978092\n",
      "\n",
-      "  [Pilot] translate: -0.5202028232620485 -61.25377666064388\n",
-      "  [Pilot] Drone Position: (255.34, 34.87)\n",
-      "  [Pilot] Angle: 8.9°\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: 1628.8954458349883\n",
+      "  [Pilot] Distance: 1651.9184793917282\n",
+      "  [Pilot] translate: -105.83895859495036 -240.1764870881905\n",
      "\n",
-      "  [Pilot] translate: 0.030834225184026082 -62.02734738952944\n",
-      "  [Pilot] Drone Position: (316.63, 44.41)\n",
-      "  [Pilot] Angle: 8.8°\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: 1575.0091545970413\n",
+      "  [Pilot] Distance: 1602.0752180361662\n",
+      "  [Pilot] translate: -148.22727851217576 277.70124157634604\n",
      "\n",
-      "  [Pilot] translate: -0.4160221189622985 -62.497522722978935\n",
-      "  [Pilot] Drone Position: (378.32, 54.42)\n",
-      "  [Pilot] Angle: 9.2°\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: 1521.0283267047403\n",
+      "  [Pilot] Distance: 1550.834822541291\n",
+      "  [Pilot] translate: 84.73919078536481 6.965084966855142\n",
      "\n",
-      "  [Pilot] translate: 0.5176443048484498 -60.8909129531913\n",
-      "  [Pilot] Drone Position: (438.51, 63.67)\n",
-      "  [Pilot] Angle: 8.7°\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: 1467.065793978559\n",
+      "  [Pilot] Distance: 1499.310186566892\n",
+      "  [Pilot] translate: -169.29950811743842 109.96288683148794\n",
      "\n",
-      "  [Pilot] translate: -0.22161188590699135 -62.49031949249053\n",
-      "  [Pilot] Drone Position: (500.24, 73.38)\n",
-      "  [Pilot] Angle: 8.9°\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: 1415.4431252709192\n"
+      "  [Pilot] Distance: 1448.1010213418751\n",
+      "  [Pilot] translate: -175.2651402920743 -178.56804299442655\n"
     ]
    }
   ],
--- a/todo.md
+++ b/todo.md
@@ -0,0 +1,13 @@
+[-] График межкадрового смещения
+[-] Отображение bounding box на графике сопоставления точек кадра с ориентиром
+[-] Проверка корректности выявления ориентира на кадре
+[-] Исправление коррекции координат на основе сопоставления с ориентиром
+[-] FPS счетчик
+[-] Устранение большой погрешности при повороте
+[-] Оформление статистики при тестовых запусках
+[-] Проведение тестовых запусков
+[-] Оформление отчета
+
+[?] Изменение масштаба во время полёта, обработка этой трансформации
+[?] Поворот ориентиров
+[?] Ограничение выбора точек, чтобы ориентиры полностью попадали в кадр
--- a/visualization.py
+++ b/visualization.py
@@ -36,10 +36,12 @@ class VisualizationManager:
        # Данные для глобальной карты
        self.trajectory_x = []
        self.trajectory_y = []
-        self.trajectory_modes = []
        self.current_x = 0.0
        self.current_y = 0.0
        
+        self.target_idx = 0
+        self.target_pts = []
+        
        # Данные для траектории БПЛА (его собственное видение)
        self.drone_trajectory_x = []
        self.drone_trajectory_y = []
@@ -65,7 +67,7 @@ class VisualizationManager:
        self.fig.canvas.manager.window.state('zoomed')
        
        # Создаем сетку 2x2 с разными размерами колонок
-        gs = self.fig.add_gridspec(2, 3, hspace=0.3, wspace=0.3, width_ratios=[.5, 1, 1])
+        gs = self.fig.add_gridspec(2, 3, hspace=0.3, wspace=0.3, width_ratios=[1, 0.7, 1])
        
        # График погрешности позиции (левый верхний угол)
        self.ax_error_plot = self.fig.add_subplot(gs[0, 0])
@@ -89,7 +91,7 @@ class VisualizationManager:
        self.ax_detection.axis('off')
        
        # Сопоставление точек (правый нижний угол)
-        self.ax_matches = self.fig.add_subplot(gs[1, 1])
+        self.ax_matches = self.fig.add_subplot(gs[1, 2])
        self.ax_matches.set_title('Feature Matching')
        self.ax_matches.axis('off')
        
@@ -104,13 +106,20 @@ class VisualizationManager:
        plt.tight_layout()
        plt.show(block=False)

-    def update_global_map(self, x: float, y: float, mode: SimMode):
+    def set_target_points(self, target_pts):
+        """ Обновление списка координат целевых точек """
+        self.target_pts = target_pts
+
+    def set_target_index(self, target_idx):
+        """ Обновление номера целевой точки """
+        self.target_idx = target_idx
+    
+    def update_global_map(self, x: float, y: float):
        """Обновляет глобальную карту"""
        self.current_x = x
        self.current_y = y
        self.trajectory_x.append(x)
        self.trajectory_y.append(y)
-        self.trajectory_modes.append(mode)
        
        self.ax_global_map.clear()
        self.ax_global_map.set_title('Global Map - Траектория полета беспилотника')
@@ -121,35 +130,27 @@ class VisualizationManager:
        self.ax_global_map.axvline(x=0, color='k', linestyle='-', alpha=0.3)
        
        if len(self.trajectory_x) > 1:
-            # Разделяем траекторию по режимам
-            operator_indices = [i for i, m in enumerate(self.trajectory_modes) if m == SimMode.OPERATOR]
-            autonome_indices = operator_indices[-1:] + [i for i, m in enumerate(self.trajectory_modes) if m == SimMode.AUTONOME]
-            
            # Рисуем траекторию оператора (синий цвет)
-            if len(operator_indices) > 1:
-                operator_x = [self.trajectory_x[i] for i in operator_indices]
-                operator_y = [self.trajectory_y[i] for i in operator_indices]
-                self.ax_global_map.plot(operator_x, operator_y, 'b-', linewidth=2, label='Режим оператора')
-            
-            # Рисуем траекторию автономного режима (красный цвет)
-            if len(autonome_indices) > 1:
-                autonome_x = [self.trajectory_x[i] for i in autonome_indices]
-                autonome_y = [self.trajectory_y[i] for i in autonome_indices]
-                self.ax_global_map.plot(autonome_x, autonome_y, 'r-', linewidth=2, label='Автономный режим')
+            self.ax_global_map.plot(self.trajectory_x, self.trajectory_y, 'b-', linewidth=2, label='Режим оператора')

            # Рисуем траекторию БПЛА (пунктирная линия, тонкая)
            if len(self.drone_trajectory_x) > 1:
                self.ax_global_map.plot(self.drone_trajectory_x, self.drone_trajectory_y, 
                                      'g--', linewidth=1, alpha=0.7, label='Данные по одометрии')

-            # Рисуем начальную точку (зеленая)
-            self.ax_global_map.plot(self.trajectory_x[0], self.trajectory_y[0], 'go', markersize=8, label='Начальная точка')
-            
            # Рисуем текущую позицию (черная)
            self.ax_global_map.plot(self.current_x, self.current_y, 'ko', markersize=6, label='Текущая позиция')

-            # Рисуем целевую точку (0, 0) - желтая
-            self.ax_global_map.plot(0, 0, 'yo', markersize=8, label='Цель (0, 0)')
+            # Рисуем ориентиры
+            for i in range(len(self.target_pts)):
+                if i != self.target_idx:
+                    pt = self.target_pts[i]
+                    self.ax_global_map.plot(pt[0], pt[1], 'go', markersize=8)
+
+            # Рисуем текущую целевую точку
+            if self.target_idx < len(self.target_pts):
+                pt = self.target_pts[self.target_idx]
+                self.ax_global_map.plot(pt[0], pt[1], 'yo', markersize=8, label='Цель (0, 0)')

            self.ax_global_map.legend()

@@ -159,6 +160,12 @@ class VisualizationManager:
            x_min, x_max = min(self.trajectory_x), max(self.trajectory_x)
            y_min, y_max = min(self.trajectory_y), max(self.trajectory_y)

+            for pt in self.target_pts:
+                x_min = min(x_min, pt[0])
+                x_max = max(x_max, pt[0])
+                y_min = min(y_min, pt[1])
+                y_max = max(y_max, pt[1])
+            
            # Учитываем также траекторию БПЛА при масштабировании
            if len(self.drone_trajectory_x) > 0:
                x_min = min(x_min, min(self.drone_trajectory_x))