From 92b6b0b0feb13ba97d7764c1f0cfaf1fe0d369af Mon Sep 17 00:00:00 2001 From: russian_proger Date: Sun, 21 Dec 2025 17:40:42 +0300 Subject: [PATCH] ref: decomposite geolocation --- autopilot.py | 118 +++++++++++-------------------------------------- chunk.py | 0 geolocation.py | 67 ++++++++++++++++++++++++++++ main.py | 6 +-- simulator.py | 95 +++++---------------------------------- 5 files changed, 109 insertions(+), 177 deletions(-) create mode 100644 chunk.py create mode 100644 geolocation.py diff --git a/autopilot.py b/autopilot.py index cc9f260..136a095 100644 --- a/autopilot.py +++ b/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 - x: float # Координата X БПЛА - y: float # Координата Y БПЛА - zoom: float # Коэффициент увеличения + geo: Geolocation # Ориентиры 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.x = 0.0 - self.y = 0.0 - self.zoom = 1.0 + self.geo = Geolocation(0, 0, 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.x, self.y, self.angle, self.zoom = self.calc_position( - homography, - self.x, self.y, self.angle, self.zoom - ) + self.geo @= homography - if self.reserved_pos is not None: - self.reserved_pos = self.calc_position( - homography, *self.reserved_pos - ) + if self.reserved_geo is not None: + self.reserved_geo @= homography def get_drone_state(self) -> dict: """ Возвращает текущее состояние БПЛА """ return { - 'x': self.x, - 'y': self.y, - 'angle': self.angle, - 'angle_degrees': math.degrees(self.angle), + 'x': self.geo.x, + 'y': self.geo.y, + 'angle': self.geo.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_zoom = 1 + landmark = Geolocation(landmark_world_x, landmark_world_y, 1, 0) 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][1] - self.y) ** 2 + (self.points[self.target_idx][0] - self.geo.x) ** 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][1] - self.y) ** 2 + (self.points[self.target_idx][0] - self.geo.x) ** 2 + + (self.points[self.target_idx][1] - self.geo.y) ** 2 ) - if self.reserved_pos is not None: - self.x, self.y, self.angle, self.zoom = self.reserved_pos - self.reserved_pos = None + if self.reserved_geo is not None: + self.geo = self.reserved_geo + 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.y = y - self.angle = angle - self.frame_count = 0 + self.geo = Geolocation(x, y, 1, angle) diff --git a/chunk.py b/chunk.py new file mode 100644 index 0000000..e69de29 diff --git a/geolocation.py b/geolocation.py new file mode 100644 index 0000000..afadfe3 --- /dev/null +++ b/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) \ No newline at end of file diff --git a/main.py b/main.py index 9662616..1a4469f 100644 --- a/main.py +++ b/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_global_map(simulator.current_x, simulator.current_y) - vis_manager.update_error_plot(i, pilot.x, pilot.y, simulator.current_x, simulator.current_y) + vis_manager.update_drone_trajectory(pilot.geo.x, pilot.geo.y) + vis_manager.update_global_map(simulator.geo.x, simulator.geo.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) diff --git a/simulator.py b/simulator.py index b802267..1bea570 100644 --- a/simulator.py +++ b/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.current_x = 0.0 - self.current_y = 0.0 - self.zoom = 1 + self.geo = Geolocation(0, 0, 1, 0) # Создаем папку для изображений, если её нет os.makedirs('./images', exist_ok=True) @@ -69,14 +65,14 @@ class Simulator: Обновляет траекторию полета беспилотника """ # Обновляем текущие координаты - self.current_x += dx * self.zoom - self.current_y += dy * self.zoom + self.geo.x += dx * self.geo.z + 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 - dy = math.sin(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.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("Симуляция завершена. Окно визуализации остается открытым для анализа.")