diff --git a/autopilot.py b/autopilot.py index f510a44..4bf6cf0 100644 --- a/autopilot.py +++ b/autopilot.py @@ -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 # Текущий ориентир @@ -51,17 +53,17 @@ class AutoPilot(Pilot): prev_chunk: VisionChunk | None frame_count: int 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})") @@ -260,55 +196,53 @@ class AutoPilot(Pilot): self.prev_chunk = current_chunk self.timer.stop() return PilotCommand(processing_time=self.timer.get_elapsed()) - + # Расстояние до цели 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: # Оцениваем матрицу трансформации - + 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) + current_chunk.to_numpy(), + homography_matrix, + 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) diff --git a/main.py b/main.py index e47f583..9ccb457 100644 --- a/main.py +++ b/main.py @@ -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()) diff --git a/simulator.py b/simulator.py index 8f07c9d..deb5ebe 100644 --- a/simulator.py +++ b/simulator.py @@ -5,128 +5,55 @@ 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)) - - def set_roll(self, roll: float): + clamped_pitch = max(-10, min(10, pitch_deg)) + self.pos.pitch = math.radians(clamped_pitch) + + 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: """ Применяет перспективную трансформацию к изображению. Возвращает квадратное изображение 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: """ Управление движением дрона. @@ -135,37 +62,46 @@ 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) - + action.move_by_offset(-dx, dy) action.release() action.perform() - + def set_zoom(self, zoom_level: float): """Программное изменение масштаба""" - self.geo.z = zoom_level - + self.pos.z = zoom_level + def get_chunk(self) -> VisionChunk: """Получить текущий снимок с камеры дрона""" png = self.yandex_map.driver.get_screenshot_as_png() im = Image.open(BytesIO(png)) - + # Применяем перспективную трансформацию 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() + } diff --git a/utility.py b/utility.py index 2af04ee..667b173 100644 --- a/utility.py +++ b/utility.py @@ -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: """ @@ -12,4 +13,22 @@ 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) \ No newline at end of file + 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] + ]) diff --git a/vision_chunk.py b/vision_chunk.py index ea1c4e8..2b7ed56 100644 --- a/vision_chunk.py +++ b/vision_chunk.py @@ -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) diff --git a/visualization.py b/visualization.py index d20450a..50259b8 100644 --- a/visualization.py +++ b/visualization.py @@ -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)