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

feat: add perspective transform

Browse Source
This commit is contained in:
russian_proger
2026-01-10 17:47:32 +03:00
parent 155bf17847
commit 17594bc8fc
6 changed files with 150 additions and 254 deletions
Download Patch File Download Diff File Expand all files Collapse all files

152
autopilot.py
View File

@@ -11,7 +11,9 @@ from timer import Timer
from vision_chunk import VisionChunk
from visualization import VisualizationManager
from geolocation import Geolocation
from position import Position
from constants import CAMERA_HEIGHT
from utility import estimate_transformation_matrix
random.seed(1)
@@ -41,7 +43,7 @@ class PilotCommand:
class AutoPilot(Pilot):
# Состояние одометрии
geo: Geolocation
pos: Position
chunks: list[VisionChunk] # Ориентиры
target_idx: int # Текущий ориентир
@@ -53,15 +55,15 @@ class AutoPilot(Pilot):
vis_manager: VisualizationManager # Менеджер визуализации (опционально)
# Положение на основе ориентира
reserved_geo: Geolocation | None
reserved_pos: Position | None
def __init__(self, points = [], chunks = [], viz_manager=None):
self.prev_chunk = None
self.geo = Geolocation(0, 0, 1, 0)
self.pos = Position(0, 0, 1, 0, 0, 0)
self.chunks = chunks
self.frame_count = 0
self.vis_manager = viz_manager # Менеджер визуализации
self.reserved_geo = None
self.reserved_pos = None
# Пороговые значения качества сопоставления/гомографии
self.min_inliers: int = 12
@@ -76,7 +78,7 @@ class AutoPilot(Pilot):
self.timer = Timer()
def get_position(self) -> tuple[float, float]:
return self.geo.x, self.geo.y
return self.pos.x, self.pos.y
def calculate_optical_flow(self, prev_chunk: VisionChunk, current_chunk: VisionChunk):
"""
@@ -87,7 +89,6 @@ class AutoPilot(Pilot):
current_gray = current_chunk.to_cv2_gray()
h, w = prev_gray.shape[:2]
cx, cy = w // 2, h // 2
# Создаем сетку точек для отслеживания (аналогично вашему step=20)
step = 35
@@ -120,99 +121,34 @@ class AutoPilot(Pilot):
return None, None
# Преобразуем в отцентрированные координаты (Y вверх)
src_pts = np.float32([[x - cx, cy - y] for x, y in good_old]).reshape(-1, 1, 2)
dst_pts = np.float32([[x - cx, cy - y] for x, y in good_new]).reshape(-1, 1, 2)
src_pts = np.float32([[x, y] for x, y in good_old]).reshape(-1, 1, 2)
dst_pts = np.float32([[x, y] for x, y in good_new]).reshape(-1, 1, 2)
return src_pts, dst_pts
def estimate_transformation_matrix(self, src_pts: np.ndarray, dst_pts: np.ndarray):
"""
Оценивает матрицу трансформации на основе сопоставленных точек
Точки уже отцентрированы относительно центра изображения
"""
# Используем RANSAC для оценки матрицы гомографии
print("Count of points: ", len(src_pts), len(dst_pts))
H, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 3.0, maxIters=1000)
# RH, rmask = cv2.findHomography(dst_pts, src_pts, cv2.RANSAC, 1.0)
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]
# [0 0 1 ]
# Масштаб и поворот
a11, a12 = H[0, 0], H[0, 1]
a21, a22 = H[1, 0], H[1, 1]
# Смещение (уже отцентрировано)
tx, ty = -H[0, 2], -H[1, 2]
# print(" [Pilot] translate:", tx, ty)
# Вычисляем угол поворота
angle = -np.arctan2(a21, a11)
# Вычисляем масштаб
scale_x = np.sqrt(a11**2 + a21**2)
scale_y = np.sqrt(a12**2 + a22**2)
scale = (scale_x + scale_y) / 2
return {
'translation': (tx, ty), # Уже отцентрировано
'rotation': angle,
'scale': scale,
'homography': H,
'mask': mask,
'inliers': inliers,
'inliers_ratio': inlier_ratio,
'rmse': rmse
}
def update_drone_position(self, transformation_info: dict):
def update_drone_position(self, homography_matrix: np.ndarray):
"""
Обновляет позицию и угол БПЛА на основе трансформации изображения
Координаты уже отцентрированы относительно центра изображения
"""
homography = transformation_info['homography']
self.geo @= homography
self.pos.iapply(homography_matrix)
if self.reserved_geo is not None:
self.reserved_geo @= homography
if self.reserved_pos is not None:
self.reserved_pos.iapply(homography_matrix)
def get_drone_state(self) -> dict:
"""
Возвращает текущее состояние БПЛА
"""
return {
'x': self.geo.x,
'y': self.geo.y,
'angle': self.geo.yaw,
'angle_degrees': math.degrees(self.geo.yaw),
'x': self.pos.x,
'y': self.pos.y,
'angle': self.pos.yaw,
'angle_degrees': math.degrees(self.pos.yaw),
'frame_count': self.frame_count
}
def get_position_by_chunk(self) -> Geolocation | None:
def get_position_by_chunk(self) -> Position | None:
# Пытаемся найти ориентир на картинке:
current_chunk = self.prev_chunk
landmark_chunk = self.chunks[self.target_idx]
@@ -244,7 +180,7 @@ class AutoPilot(Pilot):
# if False:
# Считаем абсолютную позу относительно координат ориентира
landmark_world_x, landmark_world_y = self.points[self.target_idx]
landmark = Geolocation(landmark_world_x, landmark_world_y, 1, 0)
landmark = Position(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})")
@@ -263,16 +199,17 @@ class AutoPilot(Pilot):
# Расстояние до цели
distance_to_target = math.sqrt(
(self.points[self.target_idx][0] - self.geo.x) ** 2 +
(self.points[self.target_idx][1] - self.geo.y) ** 2
(self.points[self.target_idx][0] - self.pos.x) ** 2 +
(self.points[self.target_idx][1] - self.pos.y) ** 2
)
# Вычисляем оптический поток для покадрового сравнения
optical_flow_timer = Timer()
optical_flow_timer.start()
src_pts, dst_pts = self.calculate_optical_flow(self.prev_chunk, current_chunk)
optical_flow_timer.stop()
print(f"Optical flow calculating: {optical_flow_timer.get_elapsed() * 1000:.2f} ms")
matching_timer = Timer()
matching_timer.start()
# src_pts, dst_pts = self.calculate_optical_flow(self.prev_chunk, current_chunk)
src_pts, dst_pts, _, _, _ = self.prev_chunk.detect_and_match_keypoints(current_chunk)
matching_timer.stop()
print(f"Matching calculating: {matching_timer.get_elapsed() * 1000:.2f} ms")
# Оцениваем смещение БПЛА
if src_pts is not None and dst_pts is not None:
@@ -280,35 +217,32 @@ class AutoPilot(Pilot):
matrix_estimation_timer = Timer()
matrix_estimation_timer.start()
transformation_info = self.estimate_transformation_matrix(src_pts, dst_pts)
homography_matrix = estimate_transformation_matrix(src_pts, dst_pts)
matrix_estimation_timer.stop()
print(f"Transformation matrix estimating: {matrix_estimation_timer.get_elapsed() * 1000:.2f} ms")
if transformation_info:
# Обновляем позицию и угол БПЛА (одометрия по кадрам)
self.update_drone_position(transformation_info)
print(f"Transformation matrix updating: {matrix_estimation_timer.get_elapsed() * 1000:.2f} ms")
if homography_matrix is not None:
self.update_drone_position(homography_matrix)
self.timer.stop()
# Обновляем визуализацию
if self.vis_manager:
# Используем визуализацию движения точек по сетке
homography_matrix = transformation_info['homography']
self.vis_manager.update_homography_grid(
current_chunk.to_numpy(),
homography_matrix,
grid_step=85)
grid_step=35)
self.timer.start()
chunk_timer = Timer()
chunk_timer.start()
# Пытаемся найти ориентир на картинке:
self.prev_chunk = current_chunk
# npos = self.get_position_by_chunk()
# if npos is not None:
# self.reserved_geo = npos
# self.reserved_pos = npos
chunk_timer.stop()
print(f"Chunk timer: {chunk_timer.get_elapsed() * 1000:.2f} ms")
@@ -320,18 +254,18 @@ class AutoPilot(Pilot):
return PilotCommand(stop=True)
distance_to_target = math.sqrt(
(self.points[self.target_idx][0] - self.geo.x) ** 2 +
(self.points[self.target_idx][1] - self.geo.y) ** 2
(self.points[self.target_idx][0] - self.pos.x) ** 2 +
(self.points[self.target_idx][1] - self.pos.y) ** 2
)
if self.reserved_geo is not None:
self.geo = self.reserved_geo
self.reserved_geo = None
if self.reserved_pos is not None:
self.pos = self.reserved_pos
self.reserved_pos = None
# Вычисляем угол к цели
target_angle = math.atan2(self.points[self.target_idx][1] - self.geo.y, self.points[self.target_idx][0] - self.geo.x)
target_angle = math.atan2(self.points[self.target_idx][1] - self.pos.y, self.points[self.target_idx][0] - self.pos.x)
angle_trajectory = self.geo.yaw + math.pi / 2
angle_trajectory = self.pos.yaw + math.pi / 2
# print("[ANGLE]", angle_trajectory, "->", target_angle)
@@ -355,4 +289,4 @@ class AutoPilot(Pilot):
def reset_position(self, x: float = 0.0, y: float = 0.0, angle: float = 0.0):
"""Сбрасывает позицию и угол БПЛА"""
self.geo = Geolocation(x, y, 1, angle)
self.pos = Position(x, y, 1, angle)

28
main.py
View File

@@ -142,9 +142,19 @@ def main():
if i == zoom_next_event:
r = random.randint(0, 1)
direction = ['up', 'down'][r]
simulator.change_zoom(direction)
# simulator.change_zoom(direction)
zoom_next_event = i + random.randint(20, 40)
# if i > 0:
# simulator.set_pitch(np.sin(i / 10) * 5)
# simulator.set_roll(np.sin(i / 15) * 5)
# simulator.set_zoom(1.0 + np.sin(i / 10) * 0.3)
if command.stop:
break
# simulator.handle(command.dangle, command.velocity)
chunk = simulator.get_chunk()
command = pilot.handle(chunk)
@@ -157,26 +167,24 @@ def main():
vis_manager.pause(0.2)
vis_manager.set_target_index(pilot.target_idx)
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)
vis_manager.update_drone_trajectory(pilot.pos.x, pilot.pos.y)
vis_manager.update_global_map(simulator.pos.x, simulator.pos.y)
vis_manager.update_error_plot(i, pilot.pos.x, pilot.pos.y, simulator.pos.x, simulator.pos.y)
errors.append(np.hypot(pilot.geo.x - simulator.geo.x, pilot.geo.y - simulator.geo.y))
errors.append(np.hypot(pilot.pos.x - simulator.pos.x, pilot.pos.y - simulator.pos.y))
if last_chunk_index != pilot.target_idx:
last_chunk_index = pilot.target_idx
chunk_errors.append(errors[-1])
chunk_improves.append(errors[-1] - errors[max(len(errors) - 2, 0)])
if command.stop:
break
simulator.handle(command.dangle, command.velocity)
vis_manager.update_display()
vis_manager.pause(0.2)
last_proc_times = proc_time[-10:]
print("Average FPS:", 1 / last_proc_times.mean())
print("Pilot coords:", pilot.pos)
print("Simulator coords:", simulator.pos)
simulator.handle(command.dangle, command.velocity)
print("Errors:", errors)
print("MSE:", (np.array(errors) ** 2).mean())

140
simulator.py
View File

@@ -5,94 +5,29 @@ from time import sleep
from PIL import Image
import cv2
import numpy as np
from geolocation import Geolocation
from position import Position
from vision_chunk import VisionChunk
from yandex_map import YandexMap
import constants
import utility
class Simulator:
def __init__(self, yandex_map: YandexMap = None):
self.yandex_map = yandex_map
self.geo = Geolocation(0, 0, 1, 0)
# Параметры камеры (в градусах)
self.pitch = 0.0 # тангаж (-10 до 10)
self.roll = 0.0 # крен (-10 до 10)
# Используем новый конструктор с yaw, pitch, roll
self.pos = Position(x=0, y=0, z=1, yaw=0, pitch=0, roll=0)
os.makedirs('./images', exist_ok=True)
def set_pitch(self, pitch: float):
def set_pitch(self, pitch_deg: float):
"""Установить тангаж камеры (градусы, -10 до 10)"""
self.pitch = max(-10, min(10, pitch))
clamped_pitch = max(-10, min(10, pitch_deg))
self.pos.pitch = math.radians(clamped_pitch)
def set_roll(self, roll: float):
def set_roll(self, roll_deg: float):
"""Установить крен камеры (градусы, -10 до 10)"""
self.roll = max(-10, min(10, roll))
def _calculate_camera_angles(self, velocity: float, dangle: float):
"""Автоматический расчёт тангажа и крена на основе скорости и поворота"""
# Тангаж: чем больше скорость, тем больше тангаж (до 10°)
self.pitch = min(10, velocity / 10)
# Крен: чем больше угол поворота, тем больше крен
self.roll = max(-10, min(10, math.degrees(dangle) * 2))
def _get_perspective_points(self, image_width: int, image_height: int,
yaw_deg: float) -> tuple:
"""
Вычисляет 4 точки для перспективной трансформации.
Учитывает тангаж, крен, поворот и масштаб.
"""
center_x = image_width / 2
center_y = image_height / 2
# Базовый размер области захвата (квадрат)
base_size = min(image_width, image_height) * 0.7
half_size = base_size / 2
# Исходные углы квадрата (до применения перспективы)
corners = np.float32([
[center_x - half_size, center_y - half_size], # top-left
[center_x + half_size, center_y - half_size], # top-right
[center_x + half_size, center_y + half_size], # bottom-right
[center_x - half_size, center_y + half_size], # bottom-left
])
# Применяем смещения для имитации тангажа (pitch)
# Положительный тангаж - дрон наклонён вперёд, камера смотрит дальше
pitch_offset = self.pitch * 3 # коэффициент для видимого эффекта
corners[0][1] -= pitch_offset # top-left y
corners[1][1] -= pitch_offset # top-right y
corners[2][1] += pitch_offset # bottom-right y
corners[3][1] += pitch_offset # bottom-left y
# Применяем смещения для имитации крена (roll)
# Положительный крен - дрон наклонён вправо
roll_offset = self.roll * 3
corners[0][0] += roll_offset # top-left x
corners[1][0] -= roll_offset # top-right x
corners[2][0] -= roll_offset # bottom-right x
corners[3][0] += roll_offset # bottom-left x
# Поворот вокруг центра (yaw)
angle_rad = math.radians(yaw_deg)
cos_a = math.cos(angle_rad)
sin_a = math.sin(angle_rad)
rotated_corners = []
for corner in corners:
# Смещаем к началу координат
x = corner[0] - center_x
y = corner[1] - center_y
# Поворачиваем
new_x = x * cos_a - y * sin_a
new_y = x * sin_a + y * cos_a
# Возвращаем обратно
rotated_corners.append([new_x + center_x, new_y + center_y])
return np.float32(rotated_corners)
clamped_roll = max(-10, min(10, roll_deg))
self.pos.roll = math.radians(clamped_roll)
def _apply_perspective_transform(self, image: Image.Image) -> Image.Image:
"""
@@ -100,32 +35,24 @@ class Simulator:
Возвращает квадратное изображение 700x700.
"""
img_array = np.array(image)
h, w = img_array.shape[:2]
# Получаем исходные точки с учётом перспективы
yaw_deg = -math.degrees(self.geo.yaw)
src_points = self._get_perspective_points(w, h, yaw_deg)
# Целевые точки - квадрат 700x700
dst_points = np.float32([
[0, 0],
[700, 0],
[700, 700],
[0, 700]
])
# Вычисляем матрицу трансформации
matrix = cv2.getPerspectiveTransform(src_points, dst_points)
print(img_array.shape)
h, w, _ = img_array.shape
# Применяем трансформацию
transformed = cv2.warpPerspective(img_array, matrix, (700, 700))
pos = self.pos.copy()
pos.x = 0
pos.y = 0
K = utility.calc_camera_matrix(w, h)
K = constants.K
img_array = img_array[:constants.CHUNK_WIDTH, :constants.CHUNK_WIDTH]
transformed = cv2.warpPerspective(img_array, pos.get_homography_matrix(K), (constants.CHUNK_WIDTH, constants.CHUNK_WIDTH))
return Image.fromarray(transformed)
def update_trajectory(self, dx: float, dy: float):
"""Обновляет координаты дрона"""
self.geo.x += dx * self.geo.z
self.geo.y += dy * self.geo.z
self.pos.x += dx * self.pos.z
self.pos.y += dy * self.pos.z
def handle(self, dangle: float, velocity: float = 50) -> None:
"""
@@ -136,19 +63,18 @@ class Simulator:
from selenium.webdriver.common.by import By
from selenium.webdriver.common.action_chains import ActionChains
# Автоматический расчёт углов камеры
self._calculate_camera_angles(velocity, dangle)
html = self.yandex_map.driver.find_element(By.TAG_NAME, 'html')
action = ActionChains(self.yandex_map.driver)
action.move_to_element_with_offset(html, 200, 200)
action.click_and_hold()
self.geo.yaw += dangle
# Обновляем yaw в объекте Position
self.pos.yaw += dangle
velocity = max(velocity, 10)
dx = math.cos(math.pi / 2 + self.geo.yaw) * velocity / self.geo.z
dy = math.sin(math.pi / 2 + self.geo.yaw) * velocity / self.geo.z
# Вычисляем смещение на основе текущего yaw
dx = math.cos(math.pi / 2 + self.pos.yaw) * velocity / self.pos.z
dy = math.sin(math.pi / 2 + self.pos.yaw) * velocity / self.pos.z
self.update_trajectory(dx, dy)
@@ -158,7 +84,7 @@ class Simulator:
def set_zoom(self, zoom_level: float):
"""Программное изменение масштаба"""
self.geo.z = zoom_level
self.pos.z = zoom_level
def get_chunk(self) -> VisionChunk:
"""Получить текущий снимок с камеры дрона"""
@@ -169,3 +95,13 @@ class Simulator:
transformed_im = self._apply_perspective_transform(im)
return VisionChunk(transformed_im)
def get_orientation_info(self) -> dict:
"""Получить полную информацию об ориентации дрона"""
return {
'position': (self.pos.x, self.pos.y, self.pos.z),
'yaw_deg': math.degrees(self.pos.yaw),
'pitch_deg': math.degrees(self.pos.pitch),
'roll_deg': math.degrees(self.pos.roll),
'rotation_matrix': self.pos.get_rotation_matrix()
}

19
utility.py
View File

@@ -1,6 +1,7 @@
from PIL import Image
import cv2
import numpy as np
import constants
def cv2_to_pil(cv_image: np.ndarray) -> Image.Image:
"""
@@ -13,3 +14,21 @@ def cv2_to_pil(cv_image: np.ndarray) -> Image.Image:
def image_to_numpy(self, image: Image.Image) -> np.ndarray:
"""Конвертирует PIL Image в numpy array для OpenCV"""
return np.array(image)
def get_normals(H: np.ndarray, R: np.ndarray, T: np.ndarray) -> np.ndarray:
n = cv2.decomposeHomographyMat(H, constants.K, R, T)
return n
def estimate_transformation_matrix(src_pts: np.ndarray, dst_pts: np.ndarray) -> tuple[np.ndarray, float | None]:
"""Оценивает матрицу трансформации на основе сопоставленных точек"""
# Используем RANSAC для оценки матрицы гомографии
H, _ = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 3.0, maxIters=1000)
return H
def calc_camera_matrix(w: float, h: float):
f = constants._K_FOCUS_DISTANCE
return np.array([
[f, 0, w / 2],
[0, f, h / 2],
[0, 0, 1]
])

4
vision_chunk.py
View File

@@ -161,10 +161,10 @@ class VisionChunk:
for match in good_matches:
pt1 = kp1[match.queryIdx].pt
src_pts.append([pt1[0] - cx1, cy1 - pt1[1]]) # Y вверх!
src_pts.append([pt1[0], pt1[1]])
pt2 = kp2[match.trainIdx].pt
dst_pts.append([pt2[0] - cx2, cy2 - pt2[1]])
dst_pts.append([pt2[0], pt2[1]])
src_pts = np.float32(src_pts).reshape(-1, 1, 2)
dst_pts = np.float32(dst_pts).reshape(-1, 1, 2)

9
visualization.py
View File

@@ -407,7 +407,6 @@ class VisualizationManager:
# Получаем размеры изображения и центр
height, width = current_frame.shape[:2]
center_x, center_y = width // 2, height // 2
# Создаем сетку точек с заданным шагом
grid_points = []
@@ -424,8 +423,8 @@ class VisualizationManager:
grid_points_centered = []
for pt in grid_points:
# Отцентрируем координаты точно так же, как в detect_and_match_keypoints
centered_x = pt[0] - center_x
centered_y = center_y - pt[1] # Инвертируем Y (изображение Y направлен вниз)
centered_x = pt[0]
centered_y = pt[1]
grid_points_centered.append([centered_x, centered_y])
grid_points_centered = np.array(grid_points_centered, dtype=np.float32)
@@ -442,8 +441,8 @@ class VisualizationManager:
transformed_points = []
for pt in transformed_points_centered:
# Обратное преобразование от центрированных координат к координатам изображения
img_x = pt[0] + center_x
img_y = center_y - pt[1] # Инвертируем Y обратно
img_x = pt[0]
img_y = pt[1] # Инвертируем Y обратно
transformed_points.append([img_x, img_y])
transformed_points = np.array(transformed_points, dtype=np.float32)