zenloger/autopilot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

ref: decomposite geolocation

Browse Source
This commit is contained in:
russian_proger
2025-12-21 17:40:42 +03:00
parent 48885d567a
commit 92b6b0b0fe
5 changed files with 109 additions and 177 deletions
Download Patch File Download Diff File Expand all files Collapse all files

118
autopilot.py
View File

@@ -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)

0
chunk.py Normal file
View File

67
geolocation.py Normal file
View File

@@ -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)

6
main.py
View File

@@ -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)

95
simulator.py
View File

@@ -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("Симуляция завершена. Окно визуализации остается открытым для анализа.")