ref: decomposite geolocation
This commit is contained in:
118
autopilot.py
118
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)
|
||||
|
||||
Reference in New Issue
Block a user