ref: decomposite geolocation

autopilot.py

@@ -10,6 +10,7 @@ from PIL import Image
 from timer import Timer
 
 from visualization import VisualizationManager
+from geolocation import Geolocation
 
 random.seed(1)
 
@@ -39,10 +40,7 @@ class PilotCommand:
 class AutoPilot(Pilot):
 
     # Состояние одометрии
-    angle: float
+    geo: Geolocation
-    x: float  # Координата X БПЛА
-    y: float  # Координата Y БПЛА
-    zoom: float # Коэффициент увеличения
 
     # Ориентиры
     points: list[tuple[float,float]] # Координаты
@@ -59,18 +57,15 @@ class AutoPilot(Pilot):
     vis_manager: VisualizationManager  # Менеджер визуализации (опционально)
   
     # Положение на основе ориентира
-    reserved_pos: tuple[float, float, float] | None
+    reserved_geo: Geolocation | None
 
     def __init__(self, points = [], keypoints = [], viz_manager=None):
         self.prev_image = None
-        self.angle = 0.0
+        self.geo = Geolocation(0, 0, 1, 0)
-        self.x = 0.0
-        self.y = 0.0
-        self.zoom = 1.0
         self.frame_count = 0
         self.image_center = (0, 0)  # Будет обновлено при получении первого изображения
         self.vis_manager = viz_manager  # Менеджер визуализации
-        self.reserved_pos = None
+        self.reserved_geo = None
 
         # Пороговые значения качества сопоставления/гомографии
         self.min_inliers: int = 12
@@ -100,7 +95,7 @@ class AutoPilot(Pilot):
         self.timer = Timer()
 
     def get_position(self) -> tuple[float, float]:
-        return self.x, self.y
+        return self.geo.x, self.geo.y
 
     def image_to_numpy(self, image: Image.Image) -> np.ndarray:
         """Конвертирует PIL Image в numpy array для OpenCV"""
@@ -232,55 +227,6 @@ class AutoPilot(Pilot):
             'inliers_ratio': inlier_ratio,
             'rmse': rmse
         }
-    
-    
-    
-    def calc_position(
-            self,
-            mat: np.ndarray,
-            x_source: float,
-            y_source: float,
-            angle_source: float,
-            zoom_source: float
-        ) -> tuple[float, float, float] | None:
-        """ Возвращает новые координаты и поворот на основе матрицы преобразования """
-
-
-        tx, ty = -mat[0, 2], -mat[1, 2]
-        
-        # Вычисляем угол поворота
-        rotation = -np.arctan2(mat[1, 0], mat[0, 0])
-        scale = np.hypot(mat[0, 0], mat[0, 1])
-        print("Scale: ", scale)
-        # print("[HOMOGRAPHY]", mat)
-        # print("[ROTATION]", rotation)
-        
-        # Координаты уже отцентрированы, поэтому используем их напрямую
-        dx_meters = tx
-        dy_meters = ty
-
-        angle_global = angle_source + rotation
-        
-        # Применяем поворот к смещению (учитываем текущий угол БПЛА)
-        cos_angle = math.cos(angle_global)
-        sin_angle = math.sin(angle_global)
-        
-        # Поворачиваем смещение в глобальные координаты
-        # Обратите внимание: dy_meters инвертирован, так как в изображениях Y направлен вниз
-        dx_global = dx_meters * cos_angle - dy_meters * sin_angle
-        dy_global = dx_meters * sin_angle + dy_meters * cos_angle
-
-        # Обновляем координаты БПЛА
-        print("[Matrix Transformation]: ")
-        print(mat)
-        zoom = zoom_source / scale
-        x = x_source + dx_global * zoom
-        y = y_source + dy_global * zoom
-
-        # Нормализуем угол в диапазоне [-π, π]
-        angle = math.atan2(math.sin(angle_global), math.cos(angle_global))
-
-        return x, y, angle, zoom
 
     def update_drone_position(self, transformation_info: dict):
         """
@@ -288,32 +234,26 @@ class AutoPilot(Pilot):
         Координаты уже отцентрированы относительно центра изображения
         """
         homography = transformation_info['homography']
-        
+        self.geo @= homography
-        self.x, self.y, self.angle, self.zoom = self.calc_position(
-            homography,
-            self.x, self.y, self.angle, self.zoom
-        )
 
-        if self.reserved_pos is not None:
+        if self.reserved_geo is not None:
-            self.reserved_pos = self.calc_position(
+            self.reserved_geo @= homography
-                homography, *self.reserved_pos
-            )
 
     def get_drone_state(self) -> dict:
         """
         Возвращает текущее состояние БПЛА
         """
         return {
-            'x': self.x,
+            'x': self.geo.x,
-            'y': self.y,
+            'y': self.geo.y,
-            'angle': self.angle,
+            'angle': self.geo.angle,
-            'angle_degrees': math.degrees(self.angle),
+            'angle_degrees': math.degrees(self.geo.angle),
             'frame_count': self.frame_count
         }
 
 
     
-    def get_position_by_chunk(self) -> tuple[float, float, float, float] | None:
+    def get_position_by_chunk(self) -> Geolocation | None:
         # Пытаемся найти ориентир на картинке:
         current_image = self.prev_image
         landmark_image = cv2.imread(Path('chunks') / f'chunk_{self.target_idx}.png', cv2.IMREAD_COLOR_RGB)
@@ -345,12 +285,11 @@ class AutoPilot(Pilot):
                 # if False:
                     # Считаем абсолютную позу относительно координат ориентира
                     landmark_world_x, landmark_world_y = self.points[self.target_idx]
-                    landmark_angle = 0
+                    landmark = Geolocation(landmark_world_x, landmark_world_y, 1, 0)
-                    landmark_zoom = 1
                     homography = landmark_transform['homography']
                     # homography = np.linalg.inv(homography)
                     print(f"  [Pilot] Landmark correction applied (inliers={landmark_transform['inliers']}, ratio={ratio:.2f}, rmse={rmse:.2f})")
-                    return self.calc_position(homography, landmark_world_x, landmark_world_y, landmark_angle, landmark_zoom)
+                    return landmark @ homography
         return None
 
 
@@ -376,8 +315,8 @@ class AutoPilot(Pilot):
         
         # Расстояние до цели
         distance_to_target = math.sqrt(
-            (self.points[self.target_idx][0] - self.x) ** 2 +
+            (self.points[self.target_idx][0] - self.geo.x) ** 2 +
-            (self.points[self.target_idx][1] - self.y) ** 2
+            (self.points[self.target_idx][1] - self.geo.y) ** 2
         )
         
         # Обнаруживаем и сопоставляем ключевые точки
@@ -432,7 +371,7 @@ class AutoPilot(Pilot):
         self.prev_image = current_image
         npos = self.get_position_by_chunk()
         if npos is not None:
-            self.reserved_pos = npos
+            self.reserved_geo = npos
 
         if distance_to_target < 35:
             self.target_idx += 1
@@ -441,18 +380,18 @@ class AutoPilot(Pilot):
                 return PilotCommand(stop=True)
 
             distance_to_target = math.sqrt(
-                (self.points[self.target_idx][0] - self.x) ** 2 +
+                (self.points[self.target_idx][0] - self.geo.x) ** 2 +
-                (self.points[self.target_idx][1] - self.y) ** 2
+                (self.points[self.target_idx][1] - self.geo.y) ** 2
             )
 
-            if self.reserved_pos is not None:
+            if self.reserved_geo is not None:
-                self.x, self.y, self.angle, self.zoom = self.reserved_pos
+                self.geo = self.reserved_geo
-                self.reserved_pos = None
+                self.reserved_geo = None
 
         # Вычисляем угол к цели
-        target_angle = math.atan2(self.points[self.target_idx][1] - self.y, self.points[self.target_idx][0] - self.x)
+        target_angle = math.atan2(self.points[self.target_idx][1] - self.geo.y, self.points[self.target_idx][0] - self.geo.x)
         
-        angle_trajectory = self.angle + math.pi / 2
+        angle_trajectory = self.geo.angle + math.pi / 2
 
         # print("[ANGLE]", angle_trajectory, "->", target_angle)
 
@@ -476,7 +415,4 @@ class AutoPilot(Pilot):
 
     def reset_position(self, x: float = 0.0, y: float = 0.0, angle: float = 0.0):
         """Сбрасывает позицию и угол БПЛА"""
-        self.x = x
+        self.geo = Geolocation(x, y, 1, angle)
-        self.y = y
-        self.angle = angle
-        self.frame_count = 0

geolocation.py

@@ -0,0 +1,67 @@
+import math
+import numpy as np
+
+class Geolocation:
+    """ Класс геопозиции """
+    x: float
+    y: float
+    z: float # Масштаб (Высота)
+    angle: float # Угол направления движения
+
+    def __init__(self, x: float = 0, y: float = 0, z: float = 1, angle: float = 0):
+        self.x = x
+        self.y = y
+        self.z = z
+        self.angle = angle
+
+    def __str__(self) -> str:
+        return (f"Geolocation(x={self.x:.2f}, y={self.y:.2f}, "
+                f"z={self.z:.2f}, angle={self.angle:.1f}°)")
+
+    def __matmul__(self, mat: np.array) -> 'Geolocation':
+        return self.copy().apply(mat)
+
+    def __imatmul__(self, mat: np.array) -> 'Geolocation':
+        return self.apply(mat)
+
+    def apply(self, mat: np.ndarray) -> 'Geolocation':
+        """ На основе матрицы трансформации вычисляет новую позицию и направление """
+        
+        tx, ty = -mat[0, 2], -mat[1, 2]
+        
+        # Вычисляем угол поворота
+        rotation = -np.arctan2(mat[1, 0], mat[0, 0])
+        scale = np.hypot(mat[0, 0], mat[0, 1])
+        print("Scale: ", scale)
+        
+        # Координаты уже отцентрированы, поэтому используем их напрямую
+        dx_meters = tx
+        dy_meters = ty
+
+        angle_global = self.angle + rotation
+        
+        # Применяем поворот к смещению (учитываем текущий угол БПЛА)
+        cos_angle = math.cos(angle_global)
+        sin_angle = math.sin(angle_global)
+        
+        # Поворачиваем смещение в глобальные координаты
+        # Обратите внимание: dy_meters инвертирован, так как в изображениях Y направлен вниз
+        dx_global = dx_meters * cos_angle - dy_meters * sin_angle
+        dy_global = dx_meters * sin_angle + dy_meters * cos_angle
+
+        # Обновляем координаты БПЛА
+        self.z /= scale
+        self.x = self.x + dx_global * self.z
+        self.y = self.y + dy_global * self.z
+
+        # Нормализуем угол в диапазоне [-π, π]
+        self.angle = math.atan2(math.sin(angle_global), math.cos(angle_global))
+
+        return self
+
+    def copy(self) -> 'Geolocation':
+        return Geolocation(self.x, self.y, self.z, self.angle)
+
+    @staticmethod
+    def transform(g: 'Geolocation', mat: np.ndarray) -> 'Geolocation | None':
+        return g.copy().apply(mat)

main.py

@@ -132,9 +132,9 @@ def main():
             break
 
         vis_manager.set_target_index(pilot.target_idx)
-        vis_manager.update_drone_trajectory(pilot.x, pilot.y)
+        vis_manager.update_drone_trajectory(pilot.geo.x, pilot.geo.y)
-        vis_manager.update_global_map(simulator.current_x, simulator.current_y)
+        vis_manager.update_global_map(simulator.geo.x, simulator.geo.y)
-        vis_manager.update_error_plot(i, pilot.x, pilot.y, simulator.current_x, simulator.current_y)
+        vis_manager.update_error_plot(i, pilot.geo.x, pilot.geo.y, simulator.geo.x, simulator.geo.y)
 
         simulator.handle(command.dangle, command.velocity)
 

simulator.py

@@ -8,6 +8,7 @@ from selenium.webdriver.common.by import By
 from selenium.webdriver.common.action_chains import ActionChains
 
 from autopilot import Pilot
+from geolocation import Geolocation
 from visualization import VisualizationManager, SimMode
 from yandex_map import YandexMap
 
@@ -16,22 +17,17 @@ from PIL import Image
 import os
 
 class Simulator:
-    angle: float
+    geo: Geolocation
     yandexMap: YandexMap
 
     # Менеджер визуализации
     viz_manager: VisualizationManager
-    current_x: float
-    current_y: float
 
     def __init__(self, yandexMap: YandexMap = None):
         self.yandexMap = yandexMap
 
         # Инициализация переменных для отслеживания траектории
-        self.angle = 0.0
+        self.geo = Geolocation(0, 0, 1, 0)
-        self.current_x = 0.0
-        self.current_y = 0.0
-        self.zoom = 1
         
         # Создаем папку для изображений, если её нет
         os.makedirs('./images', exist_ok=True)
@@ -69,14 +65,14 @@ class Simulator:
         Обновляет траекторию полета беспилотника
         """
         # Обновляем текущие координаты
-        self.current_x += dx * self.zoom
+        self.geo.x += dx * self.geo.z
-        self.current_y += dy * self.zoom
+        self.geo.y += dy * self.geo.z
 
     def update_map(self):
         """
         Обновляет карту траектории полета
         """
-        self.viz_manager.update_global_map(self.current_x, self.current_y, self.mode)
+        self.viz_manager.update_global_map(self.geo.x, self.geo.y, self.mode)
 
     def handle(self, dangle: float, velocity: float = 50) -> None:
         """ Сдвиг камеры """
@@ -86,11 +82,11 @@ class Simulator:
         action.move_to_element_with_offset(html, 200, 200)
         action.click_and_hold()
 
-        self.angle += dangle
+        self.geo.angle += dangle
         # print(f"  [Simulator] angle: {self.angle / math.pi * 180:.1f}°")
         velocity = max(velocity, 10)
-        dx = math.cos(math.pi / 2 + self.angle) * velocity / self.zoom
+        dx = math.cos(math.pi / 2 + self.geo.angle) * velocity / self.geo.z
-        dy = math.sin(math.pi / 2 + self.angle) * velocity / self.zoom
+        dy = math.sin(math.pi / 2 + self.geo.angle) * velocity / self.geo.z
         # print("  [Simulator] dx, dy:", [dx, dy])
         self.update_trajectory(dx, dy)
         action.move_by_offset(-dx, dy)
@@ -103,81 +99,14 @@ class Simulator:
         self.yandexMap.scroll(0.5, 0.5, 2, direction == 'down')
         sleep(0.4)
         if direction == 'down':
-            self.zoom /= 2
+            self.geo.z /= 2
         else:
-            self.zoom *= 2
+            self.geo.z *= 2
 
     def get_photo(self) -> Image.Image:
         png = self.yandexMap.driver.get_screenshot_as_png()
         im = Image.open(BytesIO(png))
 
         # Применяем поворот как будто съемка с дрона
-        rotated_im = self.rotate_image_like_drone(im, -self.angle)
+        rotated_im = self.rotate_image_like_drone(im, -self.geo.angle)
         return rotated_im
-
-    def loop(self):
-
-        html = self.driver.find_element(By.TAG_NAME, 'html')
-        action = ActionChains(self.driver)
-
-        # Добавляем начальную точку в траекторию
-        self.update_trajectory(0, 0)
-        self.viz_manager.update_global_map(self.current_x, self.current_y, self.mode)
-
-        for i in range(1000):
-            signal = None
-            if self.mode == SimMode.OPERATOR:
-                signal = self.operatorPilot.act()
-                if signal is None:
-                    self.mode = SimMode.AUTONOME
-                    # print("Режим возвращения домой!")
-
-            if self.mode == SimMode.AUTONOME:
-                signal = self.autonomePilot.act()
-
-            if signal is None:
-                break
-
-            dangle, velocity = signal
-            drone_x, drone_y = self.autonomePilot.get_position()
-            self.viz_manager.update_error_plot(i, drone_x, drone_y, self.current_x, self.current_y)
-
-            # Сдвиг камеры
-            action = ActionChains(self.driver)
-            action.move_to_element_with_offset(html, 200, 200)
-            action.click_and_hold()
-
-            self.angle += dangle
-            dx = math.cos(math.pi / 2 + self.angle) * velocity
-            dy = math.sin(math.pi / 2 + self.angle) * velocity
-            action.move_by_offset(-dx, dy)
-            action.release()
-            action.perform()
-
-            # Обновляем траекторию
-            self.update_trajectory(dx, dy)
-
-            # Загрузка скриншота
-            png = self.driver.get_screenshot_as_png()
-            im = Image.open(BytesIO(png))
-            im = im.crop([0, 80, im.width-80, im.height-60])
-            
-            # Применяем поворот как будто съемка с дрона
-            rotated_im = self.rotate_image_like_drone(im, math.pi / 2 - self.angle)
-            
-            # Передаем изображение в AutoPilot для анализа
-            self.autonomePilot.handle(rotated_im)
-            
-            # Обновляем визуализацию каждые несколько итераций для производительности
-            self.update_map()
-            self.viz_manager.update_display()
-
-        # Финальное обновление карты
-        self.update_map()
-        self.viz_manager.update_display()
-        print("last position: ", self.driver.current_url)
-        print(f"Финальная позиция: ({self.current_x:.2f}, {self.current_y:.2f})")
-        
-        # Показываем карту до закрытия, но не поднимаем на передний план
-        self.viz_manager.show_final()
-        print("Симуляция завершена. Окно визуализации остается открытым для анализа.")