diff --git a/old/advanced_autopilot.py b/old/advanced_autopilot.py deleted file mode 100644 index 41178f7..0000000 --- a/old/advanced_autopilot.py +++ /dev/null @@ -1,418 +0,0 @@ -""" -Расширенная система автопилота с улучшенным отслеживанием координат и угла БПЛА -""" - -import random -import cv2 -import numpy as np -from PIL import Image -import math -from typing import Optional, Tuple, Dict, List -from dataclasses import dataclass -import time - -random.seed(1) - -@dataclass -class DroneState: - """Структура для хранения состояния БПЛА""" - x: float = 0.0 - y: float = 0.0 - altitude: float = 10.0 - angle: float = 0.0 - velocity_x: float = 0.0 - velocity_y: float = 0.0 - angular_velocity: float = 0.0 - timestamp: float = 0.0 - confidence: float = 1.0 - -class AdvancedAutoPilot: - """ - Расширенный автопилот с улучшенным отслеживанием координат и угла БПЛА - """ - - def __init__(self, - initial_x: float = 0.0, - initial_y: float = 0.0, - initial_altitude: float = 10.0, - camera_fov: float = 60.0, - image_width: int = 640, - image_height: int = 480): - - # Состояние БПЛА - self.state = DroneState( - x=initial_x, - y=initial_y, - altitude=initial_altitude, - timestamp=time.time() - ) - - # Параметры камеры - self.camera_fov = math.radians(camera_fov) - self.image_width = image_width - self.image_height = image_height - - # Вычисляем коэффициент перевода пикселей в метры - self.update_pixel_to_meter_ratio() - - # История состояний для фильтрации - self.state_history: List[DroneState] = [] - self.max_history_size = 10 - - # Параметры фильтрации - self.velocity_filter_alpha = 0.3 # Коэффициент сглаживания скорости - self.position_filter_alpha = 0.7 # Коэффициент сглаживания позиции - - # Инициализация детекторов - self.orb_detector = cv2.ORB_create( - nfeatures=2000, - scaleFactor=1.2, - nlevels=8, - edgeThreshold=31, - firstLevel=0, - WTA_K=2, - patchSize=31, - fastThreshold=20 - ) - - self.bf_matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True) - - # Предыдущее изображение - self.prev_image: Optional[np.ndarray] = None - self.prev_timestamp: float = 0.0 - - # Счетчики - self.frame_count = 0 - self.successful_tracking_frames = 0 - - # Статистика - self.tracking_stats = { - 'total_frames': 0, - 'successful_frames': 0, - 'failed_frames': 0, - 'avg_processing_time': 0.0 - } - - def update_pixel_to_meter_ratio(self): - """Обновляет коэффициент перевода пикселей в метры на основе высоты и FOV камеры""" - # Вычисляем размер пикселя на земле при текущей высоте - ground_distance = 2 * self.state.altitude * math.tan(self.camera_fov / 2) - self.pixel_to_meter_ratio = ground_distance / self.image_width - - def detect_and_match_keypoints(self, img1: np.ndarray, img2: np.ndarray) -> Tuple[Optional[np.ndarray], Optional[np.ndarray], Optional[List], Optional[List], Optional[List]]: - """Обнаруживает и сопоставляет ключевые точки между двумя изображениями""" - # Обнаружение ключевых точек и дескрипторов - kp1, des1 = self.orb_detector.detectAndCompute(img1, None) - kp2, des2 = self.orb_detector.detectAndCompute(img2, None) - - if des1 is None or des2 is None or len(kp1) < 10 or len(kp2) < 10: - return None, None, None, None, None - - # Сопоставление ключевых точек - matches = self.bf_matcher.match(des1, des2) - matches = sorted(matches, key=lambda x: x.distance) - - # Фильтрация хороших совпадений - good_matches = [] - for match in matches: - if match.distance < 50: # Порог расстояния - good_matches.append(match) - - if len(good_matches) < 8: # Увеличиваем минимальное количество совпадений - return None, None, None, None, None - - # Извлечение координат сопоставленных точек - src_pts = np.float32([kp1[m.queryIdx].pt for m in good_matches]).reshape(-1, 1, 2) - dst_pts = np.float32([kp2[m.trainIdx].pt for m in good_matches]).reshape(-1, 1, 2) - - return src_pts, dst_pts, good_matches, kp1, kp2 - - def estimate_transformation_matrix(self, src_pts: np.ndarray, dst_pts: np.ndarray) -> Optional[Dict]: - """Оценивает матрицу трансформации с улучшенной обработкой ошибок""" - try: - # Используем RANSAC для оценки матрицы гомографии - H, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 3.0) - - if H is None: - return None - - # Проверяем качество матрицы - if not self.is_valid_homography(H): - return None - - # Извлекаем параметры трансформации - a11, a12 = H[0, 0], H[0, 1] - a21, a22 = H[1, 0], H[1, 1] - tx, ty = H[0, 2], H[1, 2] - - # Вычисляем угол поворота - 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 - - # Проверяем разумность масштаба - if scale < 0.5 or scale > 2.0: - return None - - return { - 'translation': (tx, ty), - 'rotation': angle, - 'scale': scale, - 'homography': H, - 'mask': mask, - 'inliers_ratio': np.sum(mask) / len(mask) if mask is not None else 0.0 - } - except Exception as e: - print(f"Ошибка при оценке трансформации: {e}") - return None - - def is_valid_homography(self, H: np.ndarray) -> bool: - """Проверяет валидность матрицы гомографии""" - if H is None: - return False - - # Проверяем, что матрица не вырождена - det = np.linalg.det(H) - if abs(det) < 1e-6: - return False - - # Проверяем, что элементы матрицы разумны - if np.any(np.abs(H) > 1000): - return False - - return True - - def update_drone_position(self, transformation_info: Dict, dt: float): - """Обновляет позицию и состояние БПЛА с учетом временного интервала""" - tx, ty = transformation_info['translation'] - rotation = transformation_info['rotation'] - scale = transformation_info['scale'] - inliers_ratio = transformation_info.get('inliers_ratio', 1.0) - - # Конвертируем смещение в пикселях в метры - dx_meters = tx * self.pixel_to_meter_ratio - dy_meters = ty * self.pixel_to_meter_ratio - - # Применяем поворот к смещению (учитываем текущий угол БПЛА) - cos_angle = math.cos(self.state.angle) - sin_angle = math.sin(self.state.angle) - - # Поворачиваем смещение в глобальные координаты - dx_global = dx_meters * cos_angle - dy_meters * sin_angle - dy_global = dx_meters * sin_angle + dy_meters * cos_angle - - # Вычисляем скорости - velocity_x = dx_global / dt if dt > 0 else 0.0 - velocity_y = dy_global / dt if dt > 0 else 0.0 - angular_velocity = rotation / dt if dt > 0 else 0.0 - - # Применяем фильтрацию - self.state.velocity_x = (self.velocity_filter_alpha * velocity_x + - (1 - self.velocity_filter_alpha) * self.state.velocity_x) - self.state.velocity_y = (self.velocity_filter_alpha * velocity_y + - (1 - self.velocity_filter_alpha) * self.state.velocity_y) - self.state.angular_velocity = (self.velocity_filter_alpha * angular_velocity + - (1 - self.velocity_filter_alpha) * self.state.angular_velocity) - - # Обновляем позицию с фильтрацией - new_x = self.state.x + dx_global - new_y = self.state.y + dy_global - new_angle = self.state.angle + rotation - - self.state.x = (self.position_filter_alpha * new_x + - (1 - self.position_filter_alpha) * self.state.x) - self.state.y = (self.position_filter_alpha * new_y + - (1 - self.position_filter_alpha) * self.state.y) - self.state.angle = (self.position_filter_alpha * new_angle + - (1 - self.position_filter_alpha) * self.state.angle) - - # Нормализуем угол - self.state.angle = math.atan2(math.sin(self.state.angle), math.cos(self.state.angle)) - - # Обновляем временную метку и уверенность - self.state.timestamp = time.time() - self.state.confidence = min(1.0, inliers_ratio * 1.5) # Увеличиваем уверенность при хороших совпадениях - - def handle(self, image: Image.Image) -> bool: - """Обрабатывает новый кадр и обновляет состояние БПЛА""" - start_time = time.time() - self.frame_count += 1 - self.tracking_stats['total_frames'] += 1 - - # Конвертируем изображение - current_image = np.array(image) - current_timestamp = time.time() - - if self.prev_image is None: - self.prev_image = current_image - self.prev_timestamp = current_timestamp - return False - - # Вычисляем временной интервал - dt = current_timestamp - self.prev_timestamp - - # Обнаруживаем и сопоставляем ключевые точки - src_pts, dst_pts, matches, kp1, kp2 = self.detect_and_match_keypoints(self.prev_image, current_image) - - if src_pts is not None and dst_pts is not None: - # Оцениваем матрицу трансформации - transformation_info = self.estimate_transformation_matrix(src_pts, dst_pts) - - if transformation_info: - # Обновляем позицию БПЛА - self.update_drone_position(transformation_info, dt) - - # Добавляем состояние в историю - self.add_state_to_history() - - # Обновляем статистику - self.successful_tracking_frames += 1 - self.tracking_stats['successful_frames'] += 1 - - # Выводим информацию - self.print_tracking_info(transformation_info) - - # Визуализация - self.visualize_tracking(current_image, kp1, kp2, matches, transformation_info) - - success = True - else: - self.tracking_stats['failed_frames'] += 1 - success = False - else: - self.tracking_stats['failed_frames'] += 1 - success = False - - # Обновляем предыдущее изображение - self.prev_image = current_image - self.prev_timestamp = current_timestamp - - # Обновляем статистику времени обработки - processing_time = time.time() - start_time - self.tracking_stats['avg_processing_time'] = ( - (self.tracking_stats['avg_processing_time'] * (self.frame_count - 1) + processing_time) / self.frame_count - ) - - return success - - def add_state_to_history(self): - """Добавляет текущее состояние в историю""" - # Создаем копию текущего состояния - state_copy = DroneState( - x=self.state.x, - y=self.state.y, - altitude=self.state.altitude, - angle=self.state.angle, - velocity_x=self.state.velocity_x, - velocity_y=self.state.velocity_y, - angular_velocity=self.state.angular_velocity, - timestamp=self.state.timestamp, - confidence=self.state.confidence - ) - - self.state_history.append(state_copy) - - # Ограничиваем размер истории - if len(self.state_history) > self.max_history_size: - self.state_history.pop(0) - - def get_drone_state(self) -> Dict: - """Возвращает текущее состояние БПЛА в виде словаря""" - return { - 'x': self.state.x, - 'y': self.state.y, - 'altitude': self.state.altitude, - 'angle': self.state.angle, - 'angle_degrees': math.degrees(self.state.angle), - 'velocity_x': self.state.velocity_x, - 'velocity_y': self.state.velocity_y, - 'angular_velocity': self.state.angular_velocity, - 'confidence': self.state.confidence, - 'frame_count': self.frame_count, - 'timestamp': self.state.timestamp - } - - def get_tracking_stats(self) -> Dict: - """Возвращает статистику отслеживания""" - success_rate = (self.tracking_stats['successful_frames'] / - max(1, self.tracking_stats['total_frames'])) * 100 - - return { - **self.tracking_stats, - 'success_rate_percent': success_rate, - 'current_altitude': self.state.altitude, - 'pixel_to_meter_ratio': self.pixel_to_meter_ratio - } - - def set_altitude(self, altitude: float): - """Устанавливает высоту БПЛА и пересчитывает масштаб""" - self.state.altitude = altitude - self.update_pixel_to_meter_ratio() - - def reset_position(self, x: float = 0.0, y: float = 0.0, angle: float = 0.0): - """Сбрасывает позицию и угол БПЛА""" - self.state.x = x - self.state.y = y - self.state.angle = angle - self.state.velocity_x = 0.0 - self.state.velocity_y = 0.0 - self.state.angular_velocity = 0.0 - self.state_history.clear() - self.frame_count = 0 - self.successful_tracking_frames = 0 - - def print_tracking_info(self, transformation_info: Dict): - """Выводит информацию об отслеживании""" - state = self.get_drone_state() - stats = self.get_tracking_stats() - - print(f"Frame {self.frame_count}:") - print(f" Position: ({state['x']:.3f}, {state['y']:.3f}) m") - print(f" Angle: {state['angle_degrees']:.1f}°") - print(f" Velocity: ({state['velocity_x']:.2f}, {state['velocity_y']:.2f}) m/s") - print(f" Confidence: {state['confidence']:.2f}") - print(f" Success Rate: {stats['success_rate_percent']:.1f}%") - - def visualize_tracking(self, current_image: np.ndarray, kp1, kp2, matches, transformation_info: Dict): - """Визуализирует процесс отслеживания""" - if kp1 is None or kp2 is None or matches is None: - return - - # Рисуем сопоставления - img_matches = cv2.drawMatches(self.prev_image, kp1, current_image, kp2, matches, None, - flags=cv2.DrawMatchesFlags_NOT_DRAW_SINGLE_POINTS) - - # Добавляем информацию о состоянии - state = self.get_drone_state() - stats = self.get_tracking_stats() - - info_lines = [ - f"Position: ({state['x']:.2f}, {state['y']:.2f}) m", - f"Angle: {state['angle_degrees']:.1f}°", - f"Velocity: ({state['velocity_x']:.2f}, {state['velocity_y']:.2f}) m/s", - f"Altitude: {state['altitude']:.1f} m", - f"Confidence: {state['confidence']:.2f}", - f"Success Rate: {stats['success_rate_percent']:.1f}%" - ] - - for i, line in enumerate(info_lines): - cv2.putText(img_matches, line, (10, 30 + i * 25), - cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2) - - cv2.imshow('Advanced Drone Tracking', img_matches) - cv2.waitKey(1) - - def act(self) -> float: - """Возвращает угол поворота для управления дроном""" - return self.state.angle - - def get_trajectory_history(self) -> Tuple[List[float], List[float], List[float]]: - """Возвращает историю траектории""" - x_history = [state.x for state in self.state_history] - y_history = [state.y for state in self.state_history] - angle_history = [math.degrees(state.angle) for state in self.state_history] - - return x_history, y_history, angle_history \ No newline at end of file diff --git a/old/autopilot.zip b/old/autopilot.zip deleted file mode 100644 index 3daeb25..0000000 Binary files a/old/autopilot.zip and /dev/null differ diff --git a/old/examples.py b/old/examples.py deleted file mode 100644 index 1a3e291..0000000 --- a/old/examples.py +++ /dev/null @@ -1,222 +0,0 @@ -""" -Примеры использования системы отслеживания БПЛА -""" - -import numpy as np -from PIL import Image -import cv2 -from autopilot import AutoPilot -import matplotlib.pyplot as plt - -def example_basic_usage(): - """Базовый пример использования автопилота""" - print("=== Базовый пример использования ===") - - # Создаем автопилот с начальными параметрами - autopilot = AutoPilot( - initial_x=0.0, # Начальная X координата - initial_y=0.0, # Начальная Y координата - initial_altitude=15.0 # Начальная высота - ) - - # Создаем тестовые изображения - img1 = create_simple_test_image() - img2 = create_simple_test_image(offset_x=20, offset_y=10) - - # Обрабатываем изображения - autopilot.handle(img1) - autopilot.handle(img2) - - # Получаем состояние - state = autopilot.get_drone_state() - print(f"Позиция БПЛА: ({state['x']:.2f}, {state['y']:.2f})") - print(f"Угол БПЛА: {state['angle_degrees']:.1f}°") - print(f"Высота: {state['altitude']:.1f}м") - print(f"Обработано кадров: {state['frame_count']}") - -def example_altitude_change(): - """Пример изменения высоты и пересчета масштаба""" - print("\n=== Пример изменения высоты ===") - - autopilot = AutoPilot(initial_altitude=10.0) - - # Создаем изображения - img1 = create_simple_test_image() - img2 = create_simple_test_image(offset_x=10, offset_y=5) - - # Обрабатываем при высоте 10м - autopilot.handle(img1) - autopilot.handle(img2) - - state1 = autopilot.get_drone_state() - print(f"При высоте 10м: ({state1['x']:.2f}, {state1['y']:.2f})") - - # Изменяем высоту на 20м - autopilot.set_altitude(20.0) - - # Обрабатываем те же изображения при новой высоте - autopilot.reset_position() - autopilot.handle(img1) - autopilot.handle(img2) - - state2 = autopilot.get_drone_state() - print(f"При высоте 20м: ({state2['x']:.2f}, {state2['y']:.2f})") - - # Коэффициент должен измениться в 2 раза - ratio = state2['x'] / state1['x'] if state1['x'] != 0 else 0 - print(f"Коэффициент изменения: {ratio:.2f} (ожидается ~2.0)") - -def example_circular_movement(): - """Пример отслеживания кругового движения""" - print("\n=== Пример кругового движения ===") - - autopilot = AutoPilot() - - # Создаем базовое изображение - base_img = create_simple_test_image() - - # Параметры кругового движения - radius = 30 - center_x, center_y = 320, 240 - num_steps = 8 - - x_history = [] - y_history = [] - angle_history = [] - - # Симулируем круговое движение - for i in range(num_steps): - angle = 2 * np.pi * i / num_steps - - # Вычисляем смещение - dx = radius * np.cos(angle) - dy = radius * np.sin(angle) - - # Создаем смещенное изображение - current_img = create_simple_test_image( - offset_x=int(dx), - offset_y=int(dy), - rotation=angle * 0.1 - ) - - # Обрабатываем - autopilot.handle(current_img) - - # Сохраняем историю - state = autopilot.get_drone_state() - x_history.append(state['x']) - y_history.append(state['y']) - angle_history.append(state['angle_degrees']) - - print(f"Шаг {i+1}: ({state['x']:.2f}, {state['y']:.2f}), угол: {state['angle_degrees']:.1f}°") - - # Визуализируем траекторию - plot_circular_trajectory(x_history, y_history, angle_history) - -def example_position_reset(): - """Пример сброса позиции""" - print("\n=== Пример сброса позиции ===") - - autopilot = AutoPilot() - - # Создаем и обрабатываем изображения - img1 = create_simple_test_image() - img2 = create_simple_test_image(offset_x=15, offset_y=10) - - autopilot.handle(img1) - autopilot.handle(img2) - - state_before = autopilot.get_drone_state() - print(f"До сброса: ({state_before['x']:.2f}, {state_before['y']:.2f})") - - # Сбрасываем позицию - autopilot.reset_position(x=100.0, y=200.0, angle=np.pi/4) - - state_after = autopilot.get_drone_state() - print(f"После сброса: ({state_after['x']:.2f}, {state_after['y']:.2f}), угол: {state_after['angle_degrees']:.1f}°") - -def create_simple_test_image(offset_x=0, offset_y=0, rotation=0): - """Создает простое тестовое изображение с заданным смещением""" - width, height = 640, 480 - - # Создаем базовое изображение с геометрическими фигурами - img = np.zeros((height, width, 3), dtype=np.uint8) - - # Добавляем фигуры - cv2.rectangle(img, (100, 100), (200, 200), (255, 0, 0), -1) - cv2.circle(img, (400, 300), 50, (0, 255, 0), -1) - cv2.line(img, (50, 400), (550, 400), (0, 0, 255), 5) - cv2.rectangle(img, (300, 150), (350, 200), (255, 255, 0), -1) - - # Применяем трансформацию - if offset_x != 0 or offset_y != 0 or rotation != 0: - cos_rot = np.cos(rotation) - sin_rot = np.sin(rotation) - - M = np.float32([ - [cos_rot, -sin_rot, offset_x], - [sin_rot, cos_rot, offset_y] - ]) - - img = cv2.warpAffine(img, M, (width, height), - borderMode=cv2.BORDER_REPLICATE) - - return Image.fromarray(img) - -def plot_circular_trajectory(x_history, y_history, angle_history): - """Визуализирует круговую траекторию""" - fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(15, 6)) - - # График траектории - ax1.plot(x_history, y_history, 'b-o', linewidth=2, markersize=8) - ax1.plot(x_history[0], y_history[0], 'go', markersize=12, label='Начало') - ax1.plot(x_history[-1], y_history[-1], 'ro', markersize=12, label='Конец') - ax1.set_xlabel('X координата (м)') - ax1.set_ylabel('Y координата (м)') - ax1.set_title('Круговая траектория БПЛА') - ax1.grid(True) - ax1.legend() - ax1.axis('equal') - - # График угла - ax2.plot(range(len(angle_history)), angle_history, 'r-o', linewidth=2, markersize=8) - ax2.set_xlabel('Номер шага') - ax2.set_ylabel('Угол поворота (градусы)') - ax2.set_title('Изменение угла поворота') - ax2.grid(True) - - plt.tight_layout() - plt.show() - -def example_error_handling(): - """Пример обработки ошибок и граничных случаев""" - print("\n=== Пример обработки ошибок ===") - - autopilot = AutoPilot() - - # Создаем изображение без текстуры (однородное) - blank_img = Image.fromarray(np.zeros((480, 640, 3), dtype=np.uint8)) - - # Обрабатываем - должно работать без ошибок - try: - autopilot.handle(blank_img) - autopilot.handle(blank_img) - print("Обработка однородного изображения прошла успешно") - except Exception as e: - print(f"Ошибка при обработке: {e}") - - # Проверяем состояние - state = autopilot.get_drone_state() - print(f"Состояние после обработки: ({state['x']:.2f}, {state['y']:.2f})") - -if __name__ == "__main__": - print("Примеры использования системы отслеживания БПЛА\n") - - # Запускаем все примеры - example_basic_usage() - example_altitude_change() - example_circular_movement() - example_position_reset() - example_error_handling() - - print("\nВсе примеры выполнены успешно!") \ No newline at end of file diff --git a/old/main.py b/old/main.py deleted file mode 100644 index 035720b..0000000 --- a/old/main.py +++ /dev/null @@ -1,13 +0,0 @@ -from autopilot import AutoPilot, RandomPilot -from simulator import Simulator -from visualization import VisualizationManager - -# Создаем менеджер визуализации -viz_manager = VisualizationManager("Drone Autopilot - Global Map & Detection") - -# Создаем симулятор с AutoPilot для обработки изображений -# Передаем менеджер визуализации в автопилот -simulator = Simulator(RandomPilot(), AutoPilot(viz_manager=viz_manager), viz_manager=viz_manager) - -# Запускаем симуляцию -simulator.loop() diff --git a/old/requirements.txt b/old/requirements.txt deleted file mode 100644 index f0159e0..0000000 --- a/old/requirements.txt +++ /dev/null @@ -1,56 +0,0 @@ -asttokens==3.0.0 -attrs==25.3.0 -certifi==2025.1.31 -cffi==1.17.1 -colorama==0.4.6 -comm==0.2.2 -contourpy==1.3.1 -cycler==0.12.1 -debugpy==1.8.14 -decorator==5.2.1 -executing==2.2.0 -fonttools==4.57.0 -h11==0.14.0 -idna==3.10 -ipykernel==6.29.5 -ipython==9.3.0 -ipython_pygments_lexers==1.1.1 -jedi==0.19.2 -jupyter_client==8.6.3 -jupyter_core==5.8.1 -kiwisolver==1.4.8 -matplotlib==3.10.1 -matplotlib-inline==0.1.7 -nest-asyncio==1.6.0 -numpy==2.2.4 -opencv-python==4.11.0.86 -outcome==1.3.0.post0 -packaging==24.2 -parso==0.8.4 -pillow==11.2.1 -platformdirs==4.3.8 -prompt_toolkit==3.0.51 -psutil==7.0.0 -pure_eval==0.2.3 -pycparser==2.22 -Pygments==2.19.2 -pyparsing==3.2.3 -PySocks==1.7.1 -python-dateutil==2.9.0.post0 -pywin32==310 -pyzmq==27.0.0 -scipy==1.15.2 -selenium==4.31.0 -six==1.17.0 -sniffio==1.3.1 -sortedcontainers==2.4.0 -stack-data==0.6.3 -tornado==6.5.1 -traitlets==5.14.3 -trio==0.29.0 -trio-websocket==0.12.2 -typing_extensions==4.13.2 -urllib3==2.4.0 -wcwidth==0.2.13 -websocket-client==1.8.0 -wsproto==1.2.0 diff --git a/old/simulator.py b/old/simulator.py deleted file mode 100644 index d37c8f5..0000000 --- a/old/simulator.py +++ /dev/null @@ -1,173 +0,0 @@ -import math -from io import BytesIO -from time import sleep - -from PIL import Image -from selenium import webdriver -from selenium.webdriver.common.by import By -from selenium.webdriver.common.action_chains import ActionChains - -from autopilot import Pilot -from visualization import VisualizationManager, SimMode - -import os - -class Simulator: - driver: webdriver.Chrome - mode: SimMode - - operatorPilot: Pilot - autonomePilot: Pilot - - angle: float - - # Менеджер визуализации - viz_manager: VisualizationManager - current_x: float - current_y: float - - def __init__(self, operatorPilot: Pilot, autonomePilot: Pilot, viz_manager: VisualizationManager = None): - self.mode = SimMode.OPERATOR - self.operatorPilot = operatorPilot - self.autonomePilot = autonomePilot - - # Инициализация переменных для траектории - self.current_x = 0.0 - self.current_y = 0.0 - - # Создаем менеджер визуализации - self.viz_manager = viz_manager - - # Передаем менеджер визуализации в автопилот, если он поддерживает это - if hasattr(self.autonomePilot, 'viz_manager'): - self.autonomePilot.viz_manager = self.viz_manager - - # Создаем папку для изображений, если её нет - os.makedirs('./images', exist_ok=True) - - options = webdriver.ChromeOptions() - # options.add_experimental_option("detach", True) - self.driver = webdriver.Chrome(options) - self.driver.get("https://yandex.ru/maps/43/kazan/?ll=49.103814%2C55.794258&z=14") - sleep(2) - - action = ActionChains(self.driver) - - # Закрытие левой панели - action.click(self.driver.find_element(By.CLASS_NAME, 'sidebar-toggle-button')) - action.move_to_element_with_offset(self.driver.find_element(By.XPATH, "//div[@class='rounded-controls']/div[@class='rounded-controls__child'][5]//button"), 5, 5) - action.perform() - - # Режим спутника - sleep(1) - action.move_to_element_with_offset(self.driver.find_element(By.XPATH, "//div[@class='rounded-controls']/div[@class='rounded-controls__child'][5]//button"), -500, -500) - action.click() - action.perform() - - self.angle = 0 - - def rotate_image_like_drone(self, image: Image.Image, angle: float) -> Image.Image: - """ - Поворачивает картинку как будто съемка ведется с летящего дрона. - Выделяет концентрический квадрат, поворачивает его и извлекает результат. - """ - # Получаем размеры изображения - width, height = image.size - square_size = min(width, height) - cropped_image = image.crop((0, 0, square_size, square_size)) - cropped_image = cropped_image.rotate(angle / math.pi * 180, expand=True) - - # Определяем размер концентрического квадрата (80% от минимальной стороны) - local_square_size = int(square_size / 2 ** 0.5) - - # Вычисляем координаты для центрирования квадрата - left = (cropped_image.width - local_square_size) // 2 - top = (cropped_image.height - local_square_size) // 2 - right = left + local_square_size - bottom = top + local_square_size - - # Вырезаем концентрический квадрат - final_image = cropped_image.crop((left, top, right, bottom)) - - return final_image - - def update_trajectory(self, dx: float, dy: float): - """ - Обновляет траекторию полета беспилотника - """ - # Обновляем текущие координаты - self.current_x += dx - self.current_y += dy - - def update_map(self): - """ - Обновляет карту траектории полета - """ - self.viz_manager.update_global_map(self.current_x, self.current_y, self.mode) - - 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(self.angle) * velocity - dy = math.sin(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("Симуляция завершена. Окно визуализации остается открытым для анализа.") diff --git a/old/test.ipynb b/old/test.ipynb deleted file mode 100644 index 505f655..0000000 --- a/old/test.ipynb +++ /dev/null @@ -1,55 +0,0 @@ -{ - "cells": [ - { - "cell_type": "code", - "execution_count": 4, - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "100.00078395707533 -0.0007839570753276348\n" - ] - } - ], - "source": [ - "h = 100\n", - "R = 6378 * 1000\n", - "\n", - "def solve_x2(a, b, c):\n", - " d = b ** 2 - 4 * a * c\n", - " x1 = (-b - d ** 0.5) / (2 * a)\n", - " x2 = (-b + d ** 0.5) / (2 * a)\n", - " return x1, x2\n", - "\n", - "x1, x2 = solve_x2(2, -2 * (h + R), 2 * h * R + h ** 2)\n", - "\n", - "y = h - x1\n", - "\n", - "print(x1, y)" - ] - } - ], - "metadata": { - "kernelspec": { - "display_name": ".venv", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.13.1" - } - }, - "nbformat": 4, - "nbformat_minor": 2 -} diff --git a/old/test_center_coordinates.py b/old/test_center_coordinates.py deleted file mode 100644 index 099060d..0000000 --- a/old/test_center_coordinates.py +++ /dev/null @@ -1,96 +0,0 @@ -#!/usr/bin/env python3 -""" -Тестовый скрипт для проверки работы с координатами относительно центра изображения -""" - -import numpy as np -from PIL import Image -import cv2 -from autopilot import AutoPilot - -def test_center_coordinates(): - """Тестирует работу с координатами относительно центра изображения""" - print("Тест координат относительно центра изображения...") - - # Создаем тестовое изображение - width, height = 640, 480 - test_image = np.zeros((height, width, 3), dtype=np.uint8) - - # Добавляем некоторые объекты для отслеживания - cv2.circle(test_image, (width//2, height//2), 50, (255, 255, 255), -1) # Центр - cv2.circle(test_image, (100, 100), 20, (255, 0, 0), -1) # Левый верхний угол - cv2.circle(test_image, (width-100, height-100), 20, (0, 255, 0), -1) # Правый нижний угол - - # Создаем второе изображение с небольшим смещением - test_image2 = np.zeros((height, width, 3), dtype=np.uint8) - cv2.circle(test_image2, (width//2 + 10, height//2 + 5), 50, (255, 255, 255), -1) # Смещенный центр - cv2.circle(test_image2, (110, 105), 20, (255, 0, 0), -1) # Смещенный левый верхний угол - cv2.circle(test_image2, (width-90, height-95), 20, (0, 255, 0), -1) # Смещенный правый нижний угол - - # Конвертируем в PIL Image - pil_image1 = Image.fromarray(test_image) - pil_image2 = Image.fromarray(test_image2) - - # Создаем автопилот - pilot = AutoPilot(initial_x=0.0, initial_y=0.0) - - print(f"Размер изображения: {width}x{height}") - print(f"Ожидаемый центр: ({width//2}, {height//2})") - - # Обрабатываем первое изображение - pilot.handle(pil_image1) - print(f"Центр изображения после первого кадра: {pilot.image_center}") - - # Обрабатываем второе изображение - pilot.handle(pil_image2) - print(f"Центр изображения после второго кадра: {pilot.image_center}") - - # Получаем состояние дрона - drone_state = pilot.get_drone_state() - print(f"Позиция дрона: ({drone_state['x']:.2f}, {drone_state['y']:.2f})") - print(f"Угол дрона: {drone_state['angle_degrees']:.1f}°") - - print("\nАнализ отцентрированных координат:") - print("Ожидаемое смещение центра: (10, 5) пикселей") - print("Ожидаемое смещение левого верхнего угла: (10, 5) пикселей") - print("Ожидаемое смещение правого нижнего угла: (10, 5) пикселей") - - print("Тест завершен!") - -def test_keypoint_centering(): - """Тестирует отцентрирование ключевых точек""" - print("\nТест отцентрирования ключевых точек...") - - # Создаем простое изображение с одной точкой - width, height = 100, 100 - test_image = np.zeros((height, width, 3), dtype=np.uint8) - cv2.circle(test_image, (25, 25), 5, (255, 255, 255), -1) # Точка в (25, 25) - - # Создаем второе изображение с той же точкой - test_image2 = np.zeros((height, width, 3), dtype=np.uint8) - cv2.circle(test_image2, (25, 25), 5, (255, 255, 255), -1) - - # Конвертируем в PIL Image - pil_image1 = Image.fromarray(test_image) - pil_image2 = Image.fromarray(test_image2) - - # Создаем автопилот - pilot = AutoPilot(initial_x=0.0, initial_y=0.0) - - print(f"Размер изображения: {width}x{height}") - print(f"Центр изображения: ({width//2}, {height//2}) = (50, 50)") - print(f"Позиция точки: (25, 25)") - print(f"Ожидаемые отцентрированные координаты: (25-50, 25-50) = (-25, -25)") - - # Обрабатываем изображения - pilot.handle(pil_image1) - pilot.handle(pil_image2) - - drone_state = pilot.get_drone_state() - print(f"Результат: позиция дрона ({drone_state['x']:.2f}, {drone_state['y']:.2f})") - - print("Тест завершен!") - -if __name__ == "__main__": - test_center_coordinates() - test_keypoint_centering() \ No newline at end of file diff --git a/old/test_tracking.py b/old/test_tracking.py deleted file mode 100644 index 097af23..0000000 --- a/old/test_tracking.py +++ /dev/null @@ -1,204 +0,0 @@ -#!/usr/bin/env python3 -""" -Тестовый скрипт для проверки системы отслеживания траектории беспилотника -""" - -import numpy as np -from PIL import Image -import cv2 -from autopilot import AutoPilot, RandomPilot -from simulator import Simulator, SimMode -import matplotlib.pyplot as plt -import time - -def create_test_image(width=640, height=480, pattern_type="random"): - """Создает тестовое изображение с различными паттернами""" - if pattern_type == "random": - # Создаем изображение с случайными точками - img = np.random.randint(0, 255, (height, width, 3), dtype=np.uint8) - # Добавляем несколько четких объектов - cv2.rectangle(img, (100, 100), (200, 200), (255, 0, 0), -1) - cv2.circle(img, (400, 300), 50, (0, 255, 0), -1) - cv2.line(img, (50, 400), (550, 400), (0, 0, 255), 5) - elif pattern_type == "grid": - # Создаем сетку - img = np.zeros((height, width, 3), dtype=np.uint8) - for i in range(0, width, 50): - cv2.line(img, (i, 0), (i, height), (255, 255, 255), 1) - for i in range(0, height, 50): - cv2.line(img, (0, i), (width, i), (255, 255, 255), 1) - elif pattern_type == "circles": - # Создаем изображение с кругами - img = np.zeros((height, width, 3), dtype=np.uint8) - for i in range(5): - x = np.random.randint(50, width-50) - y = np.random.randint(50, height-50) - radius = np.random.randint(20, 80) - color = (np.random.randint(0, 255), np.random.randint(0, 255), np.random.randint(0, 255)) - cv2.circle(img, (x, y), radius, color, -1) - - return Image.fromarray(img) - -def simulate_drone_movement(base_image, dx, dy, rotation=0): - """Симулирует движение БПЛА, создавая смещенное изображение""" - img_array = np.array(base_image) - height, width = img_array.shape[:2] - - # Создаем матрицу трансформации - cos_rot = np.cos(rotation) - sin_rot = np.sin(rotation) - - # Матрица поворота и смещения - M = np.float32([ - [cos_rot, -sin_rot, dx], - [sin_rot, cos_rot, dy] - ]) - - # Применяем трансформацию - transformed = cv2.warpAffine(img_array, M, (width, height), - borderMode=cv2.BORDER_REPLICATE) - - return Image.fromarray(transformed) - -def test_drone_tracking(): - """Тестирует систему отслеживания БПЛА""" - print("Запуск тестирования системы отслеживания БПЛА...") - - # Создаем автопилот - autopilot = AutoPilot(initial_x=0.0, initial_y=0.0, initial_altitude=10.0) - - # Создаем базовое изображение - base_image = create_test_image(pattern_type="circles") - - # Траектория движения (dx, dy, rotation) - trajectory = [ - (10, 0, 0), # Движение вперед - (10, 5, 0.1), # Движение вперед с небольшим поворотом - (0, 10, 0), # Движение вправо - (-5, 5, -0.1), # Движение назад-вправо с поворотом - (5, -5, 0.05), # Движение вперед-влево - (0, -10, 0), # Движение влево - (-10, 0, 0), # Движение назад - ] - - # Массивы для хранения истории позиций - x_history = [] - y_history = [] - angle_history = [] - - # Обрабатываем каждое изображение в траектории - for i, (dx, dy, rotation) in enumerate(trajectory): - print(f"\n--- Кадр {i+1} ---") - - # Создаем смещенное изображение - current_image = simulate_drone_movement(base_image, dx, dy, rotation) - - # Обрабатываем изображение - autopilot.handle(current_image) - - # Получаем состояние БПЛА - state = autopilot.get_drone_state() - x_history.append(state['x']) - y_history.append(state['y']) - angle_history.append(state['angle_degrees']) - - # Обновляем базовое изображение для следующего кадра - base_image = current_image - - # Небольшая пауза для визуализации - time.sleep(0.5) - - # Закрываем окно OpenCV - cv2.destroyAllWindows() - - # Визуализируем траекторию - plot_trajectory(x_history, y_history, angle_history) - -def plot_trajectory(x_history, y_history, angle_history): - """Визуализирует траекторию движения БПЛА""" - fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(15, 6)) - - # График траектории - ax1.plot(x_history, y_history, 'b-o', linewidth=2, markersize=8) - ax1.plot(x_history[0], y_history[0], 'go', markersize=12, label='Начало') - ax1.plot(x_history[-1], y_history[-1], 'ro', markersize=12, label='Конец') - ax1.set_xlabel('X координата (м)') - ax1.set_ylabel('Y координата (м)') - ax1.set_title('Траектория движения БПЛА') - ax1.grid(True) - ax1.legend() - ax1.axis('equal') - - # График угла поворота - ax2.plot(range(len(angle_history)), angle_history, 'r-o', linewidth=2, markersize=8) - ax2.set_xlabel('Номер кадра') - ax2.set_ylabel('Угол поворота (градусы)') - ax2.set_title('Изменение угла поворота БПЛА') - ax2.grid(True) - - plt.tight_layout() - plt.show() - -def test_single_frame(): - """Тестирует обработку одного кадра""" - print("Тестирование обработки одного кадра...") - - autopilot = AutoPilot() - - # Создаем два похожих изображения - img1 = create_test_image(pattern_type="circles") - img2 = simulate_drone_movement(img1, 15, 10, 0.05) - - # Обрабатываем первое изображение - autopilot.handle(img1) - - # Обрабатываем второе изображение - autopilot.handle(img2) - - # Получаем состояние - state = autopilot.get_drone_state() - print(f"Позиция БПЛА: ({state['x']:.2f}, {state['y']:.2f})") - print(f"Угол БПЛА: {state['angle_degrees']:.1f}°") - - cv2.destroyAllWindows() - -def test_trajectory_tracking(): - """Тестирует систему отслеживания траектории""" - print("Запуск теста системы отслеживания траектории...") - - # Создаем пилотов - operator_pilot = RandomPilot(velocity=1.0) - autonome_pilot = AutoPilot(initial_x=0.0, initial_y=0.0) - - # Создаем симулятор - simulator = Simulator(operator_pilot, autonome_pilot) - - try: - # Запускаем симуляцию - simulator.loop() - - print("Симуляция завершена!") - print(f"Всего точек траектории: {len(simulator.trajectory_x)}") - print(f"Режим оператора: {sum(1 for mode in simulator.trajectory_modes if mode == SimMode.OPERATOR)} точек") - print(f"Автономный режим: {sum(1 for mode in simulator.trajectory_modes if mode == SimMode.AUTONOME)} точек") - - except Exception as e: - print(f"Ошибка во время симуляции: {e}") - finally: - # Закрываем браузер - simulator.driver.quit() - -if __name__ == "__main__": - print("Выберите тест:") - print("1. Тест одного кадра") - print("2. Тест полной траектории") - - choice = input("Введите номер теста (1 или 2): ").strip() - - if choice == "1": - test_single_frame() - elif choice == "2": - test_trajectory_tracking() - else: - print("Неверный выбор. Запускаю тест одного кадра...") - test_single_frame() \ No newline at end of file diff --git a/old/test_unified_visualization.py b/old/test_unified_visualization.py deleted file mode 100644 index 8e6efdd..0000000 --- a/old/test_unified_visualization.py +++ /dev/null @@ -1,65 +0,0 @@ -#!/usr/bin/env python3 -""" -Тестовый скрипт для проверки единой системы визуализации -""" - -import numpy as np -from PIL import Image -import cv2 -from autopilot import AutoPilot, RandomPilot -from simulator import Simulator -from visualization import VisualizationManager, SimMode - -def test_unified_visualization(): - """Тестирует единую систему визуализации""" - print("Тест единой системы визуализации...") - - # Создаем менеджер визуализации - viz_manager = VisualizationManager("Test - Unified Visualization") - - # Создаем автопилот с менеджером визуализации - autonome_pilot = AutoPilot(initial_x=0.0, initial_y=0.0, viz_manager=viz_manager) - - # Создаем симулятор - simulator = Simulator(RandomPilot(), autonome_pilot) - - # Создаем тестовые изображения - width, height = 640, 480 - test_image1 = np.zeros((height, width, 3), dtype=np.uint8) - test_image2 = np.zeros((height, width, 3), dtype=np.uint8) - - # Добавляем объекты для отслеживания - cv2.circle(test_image1, (width//2, height//2), 50, (255, 255, 255), -1) - cv2.circle(test_image1, (100, 100), 20, (255, 0, 0), -1) - - cv2.circle(test_image2, (width//2 + 10, height//2 + 5), 50, (255, 255, 255), -1) - cv2.circle(test_image2, (110, 105), 20, (255, 0, 0), -1) - - # Конвертируем в PIL Image - pil_image1 = Image.fromarray(test_image1) - pil_image2 = Image.fromarray(test_image2) - - print("Обрабатываем первое изображение...") - autonome_pilot.handle(pil_image1) - - print("Обрабатываем второе изображение...") - autonome_pilot.handle(pil_image2) - - # Обновляем визуализацию - viz_manager.update_display() - - # Получаем состояние дрона - drone_state = autonome_pilot.get_drone_state() - print(f"Позиция дрона: ({drone_state['x']:.2f}, {drone_state['y']:.2f})") - print(f"Угол дрона: {drone_state['angle_degrees']:.1f}°") - - print("Тест завершен! Окно визуализации должно показывать:") - print("- Глобальную карту с траекторией") - print("- Детекцию ключевых точек") - print("- Сопоставление точек между кадрами") - - # Показываем финальное состояние - viz_manager.show_final() - -if __name__ == "__main__": - test_unified_visualization() \ No newline at end of file diff --git a/old/test_visualization_only.py b/old/test_visualization_only.py deleted file mode 100644 index 2f00f0b..0000000 --- a/old/test_visualization_only.py +++ /dev/null @@ -1,67 +0,0 @@ -#!/usr/bin/env python3 -""" -Упрощенный тест для проверки системы визуализации без браузера -""" - -import numpy as np -from PIL import Image -import cv2 -from autopilot import AutoPilot -from visualization import VisualizationManager, SimMode - -def test_visualization_only(): - """Тестирует только систему визуализации без симулятора""" - print("Тест системы визуализации...") - - # Создаем менеджер визуализации - viz_manager = VisualizationManager("Test - Visualization Only") - - # Создаем автопилот с менеджером визуализации - autonome_pilot = AutoPilot(initial_x=0.0, initial_y=0.0, viz_manager=viz_manager) - - # Создаем тестовые изображения - width, height = 640, 480 - test_image1 = np.zeros((height, width, 3), dtype=np.uint8) - test_image2 = np.zeros((height, width, 3), dtype=np.uint8) - - # Добавляем объекты для отслеживания - cv2.circle(test_image1, (width//2, height//2), 50, (255, 255, 255), -1) - cv2.circle(test_image1, (100, 100), 20, (255, 0, 0), -1) - cv2.circle(test_image1, (width-100, height-100), 20, (0, 255, 0), -1) - - cv2.circle(test_image2, (width//2 + 10, height//2 + 5), 50, (255, 255, 255), -1) - cv2.circle(test_image2, (110, 105), 20, (255, 0, 0), -1) - cv2.circle(test_image2, (width-90, height-95), 20, (0, 255, 0), -1) - - # Конвертируем в PIL Image - pil_image1 = Image.fromarray(test_image1) - pil_image2 = Image.fromarray(test_image2) - - print("Обрабатываем первое изображение...") - autonome_pilot.handle(pil_image1) - - print("Обрабатываем второе изображение...") - autonome_pilot.handle(pil_image2) - - # Симулируем движение дрона - print("Симулируем движение дрона...") - for i in range(10): - # Обновляем глобальную карту - viz_manager.update_global_map(i * 10, i * 5, SimMode.OPERATOR if i < 5 else SimMode.AUTONOME) - viz_manager.update_display() - - # Получаем состояние дрона - drone_state = autonome_pilot.get_drone_state() - print(f"Позиция дрона: ({drone_state['x']:.2f}, {drone_state['y']:.2f})") - print(f"Угол дрона: {drone_state['angle_degrees']:.1f}°") - - print("Тест завершен! Окно визуализации должно показывать:") - print("- Глобальную карту с траекторией (синий - оператор, красный - автономный)") - print("- Детекцию ключевых точек на текущем кадре") - print("- Сопоставление точек между кадрами") - - # Показываем финальное состояние - viz_manager.show_final() - -if __name__ == "__main__": - test_visualization_only() \ No newline at end of file diff --git a/old/visualization.py b/old/visualization.py deleted file mode 100644 index de882b0..0000000 --- a/old/visualization.py +++ /dev/null @@ -1,277 +0,0 @@ -#!/usr/bin/env python3 -""" -Модуль для управления общим окном визуализации -""" - -import matplotlib.pyplot as plt -import matplotlib.patches as patches -import numpy as np -from enum import Enum -import cv2 -from PIL import Image -import matplotlib - -# Настройки matplotlib -matplotlib.use('TkAgg') -plt.rcParams['figure.raise_window'] = False - -class SimMode(Enum): - OPERATOR = 1 - AUTONOME = 2 - -class VisualizationManager: - """ - Менеджер для управления общим окном визуализации - """ - - def __init__(self, window_title="Drone Autopilot Visualization"): - self.window_title = window_title - self.fig = None - self.ax_error_plot = None # График погрешности позиции - self.ax_global_map = None - self.ax_detection = None - self.ax_matches = None - - # Данные для глобальной карты - self.trajectory_x = [] - self.trajectory_y = [] - self.trajectory_modes = [] - self.current_x = 0.0 - self.current_y = 0.0 - - # Данные для траектории БПЛА (его собственное видение) - self.drone_trajectory_x = [] - self.drone_trajectory_y = [] - - # Данные для графика погрешности - self.error_times = [] - self.position_errors = [] - - # Данные для детекции - self.current_frame = None - self.keypoints = [] - self.matches = [] - - self._setup_window() - - def _setup_window(self): - """Настраивает общее окно с несколькими областями""" - plt.ion() - self.fig = plt.figure(figsize=(16, 10)) - self.fig.canvas.manager.window.title(self.window_title) - - # Открываем окно на полный экран - self.fig.canvas.manager.window.state('zoomed') - - # Создаем сетку 2x2 с разными размерами колонок - gs = self.fig.add_gridspec(2, 2, hspace=0.3, wspace=0.3, width_ratios=[1, 1]) - - # График погрешности позиции (левый верхний угол) - self.ax_error_plot = self.fig.add_subplot(gs[0, 0]) - self.ax_error_plot.set_title('Погрешность позиции от времени') - self.ax_error_plot.set_xlabel('Время (кадры)') - self.ax_error_plot.set_ylabel('Погрешность (метры)') - self.ax_error_plot.grid(True, alpha=0.3) - - # Глобальная карта (левый нижний угол) - self.ax_global_map = self.fig.add_subplot(gs[1, 0]) - self.ax_global_map.set_title('Global Map - Траектория полета беспилотника') - self.ax_global_map.set_xlabel('X координата') - self.ax_global_map.set_ylabel('Y координата') - self.ax_global_map.grid(True, alpha=0.3) - self.ax_global_map.axhline(y=0, color='k', linestyle='-', alpha=0.3) - self.ax_global_map.axvline(x=0, color='k', linestyle='-', alpha=0.3) - - # Детекция ключевых точек (правый верхний угол) - self.ax_detection = self.fig.add_subplot(gs[0, 1]) - self.ax_detection.set_title('Keypoint Detection') - self.ax_detection.axis('off') - - # Сопоставление точек (правый нижний угол) - self.ax_matches = self.fig.add_subplot(gs[1, 1]) - self.ax_matches.set_title('Feature Matching') - self.ax_matches.axis('off') - - # Настройки окна - self.fig.canvas.manager.window.attributes('-topmost', False) - - plt.tight_layout() - - def update_global_map(self, x: float, y: float, mode: SimMode): - """Обновляет глобальную карту""" - self.current_x = x - self.current_y = y - self.trajectory_x.append(x) - self.trajectory_y.append(y) - self.trajectory_modes.append(mode) - - self.ax_global_map.clear() - self.ax_global_map.set_title('Global Map - Траектория полета беспилотника') - self.ax_global_map.set_xlabel('X координата') - self.ax_global_map.set_ylabel('Y координата') - self.ax_global_map.grid(True, alpha=0.3) - self.ax_global_map.axhline(y=0, color='k', linestyle='-', alpha=0.3) - self.ax_global_map.axvline(x=0, color='k', linestyle='-', alpha=0.3) - - if len(self.trajectory_x) > 1: - # Разделяем траекторию по режимам - operator_indices = [i for i, m in enumerate(self.trajectory_modes) if m == SimMode.OPERATOR] - autonome_indices = operator_indices[-1:] + [i for i, m in enumerate(self.trajectory_modes) if m == SimMode.AUTONOME] - - # Рисуем траекторию оператора (синий цвет) - if len(operator_indices) > 1: - operator_x = [self.trajectory_x[i] for i in operator_indices] - operator_y = [self.trajectory_y[i] for i in operator_indices] - self.ax_global_map.plot(operator_x, operator_y, 'b-', linewidth=2, label='Режим оператора') - - # Рисуем траекторию автономного режима (красный цвет) - if len(autonome_indices) > 1: - autonome_x = [self.trajectory_x[i] for i in autonome_indices] - autonome_y = [self.trajectory_y[i] for i in autonome_indices] - self.ax_global_map.plot(autonome_x, autonome_y, 'r-', linewidth=2, label='Автономный режим') - - # Рисуем траекторию БПЛА (пунктирная линия, тонкая) - if len(self.drone_trajectory_x) > 1: - self.ax_global_map.plot(self.drone_trajectory_x, self.drone_trajectory_y, - 'g--', linewidth=1, alpha=0.7, label='Данные по одометрии') - - # Рисуем начальную точку (зеленая) - self.ax_global_map.plot(self.trajectory_x[0], self.trajectory_y[0], 'go', markersize=8, label='Начальная точка') - - # Рисуем текущую позицию (черная) - self.ax_global_map.plot(self.current_x, self.current_y, 'ko', markersize=6, label='Текущая позиция') - - # Рисуем целевую точку (0, 0) - желтая - self.ax_global_map.plot(0, 0, 'yo', markersize=8, label='Цель (0, 0)') - - self.ax_global_map.legend() - - # Автоматически масштабируем оси - if len(self.trajectory_x) > 0: - margin = 50 - x_min, x_max = min(self.trajectory_x), max(self.trajectory_x) - y_min, y_max = min(self.trajectory_y), max(self.trajectory_y) - - # Учитываем также траекторию БПЛА при масштабировании - if len(self.drone_trajectory_x) > 0: - x_min = min(x_min, min(self.drone_trajectory_x)) - x_max = max(x_max, max(self.drone_trajectory_x)) - y_min = min(y_min, min(self.drone_trajectory_y)) - y_max = max(y_max, max(self.drone_trajectory_y)) - - x_min = min(x_min, 0) - x_max = max(x_max, 0) - y_min = min(y_min, 0) - y_max = max(y_max, 0) - - self.ax_global_map.set_xlim(x_min - margin, x_max + margin) - self.ax_global_map.set_ylim(y_min - margin, y_max + margin) - - def update_drone_trajectory(self, drone_x: float, drone_y: float): - """Обновляет траекторию БПЛА (его собственное видение позиции)""" - self.drone_trajectory_x.append(drone_x) - self.drone_trajectory_y.append(drone_y) - - def update_error_plot(self, frame_count: int, drone_x: float, drone_y: float, true_x: float, true_y: float): - """Обновляет график погрешности позиции""" - # Вычисляем погрешность как расстояние между реальной и предполагаемой позицией - error = np.sqrt((drone_x - true_x)**2 + (drone_y - true_y)**2) - - self.error_times.append(frame_count) - self.position_errors.append(error) - - self.ax_error_plot.clear() - self.ax_error_plot.set_title('Погрешность позиции от времени') - self.ax_error_plot.set_xlabel('Время (кадры)') - self.ax_error_plot.set_ylabel('Погрешность (метры)') - self.ax_error_plot.grid(True, alpha=0.3) - - if len(self.error_times) > 1: - self.ax_error_plot.plot(self.error_times, self.position_errors, 'b-', linewidth=2) - - # Автоматически масштабируем оси - if len(self.position_errors) > 0: - margin = 0.1 - error_min, error_max = min(self.position_errors), max(self.position_errors) - if error_max > error_min: - self.ax_error_plot.set_ylim(0, error_max + margin) - else: - self.ax_error_plot.set_ylim(0, 1) - - def update_detection(self, image: np.ndarray, keypoints): - """Обновляет визуализацию детекции ключевых точек""" - self.current_frame = image.copy() - self.keypoints = keypoints - - self.ax_detection.clear() - self.ax_detection.set_title('Keypoint Detection') - - if image is not None: - # Конвертируем BGR в RGB для matplotlib - if len(image.shape) == 3 and image.shape[2] == 3: - image_rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB) - else: - image_rgb = image - - self.ax_detection.imshow(image_rgb) - - # Рисуем ключевые точки - if keypoints: - kp_coords = np.array([kp.pt for kp in keypoints]) - self.ax_detection.scatter(kp_coords[:, 0], kp_coords[:, 1], - c='red', s=20, alpha=0.7, marker='o') - - self.ax_detection.axis('off') - - def update_matches(self, img1: np.ndarray, img2: np.ndarray, - kp1, kp2, matches, transformation_info=None): - """Обновляет визуализацию сопоставления точек""" - self.ax_matches.clear() - self.ax_matches.set_title('Feature Matching') - - if img1 is not None and img2 is not None and matches: - # Рисуем сопоставления - img_matches = cv2.drawMatches(img1, kp1, img2, kp2, matches, None, - flags=cv2.DrawMatchesFlags_NOT_DRAW_SINGLE_POINTS) - - # Конвертируем BGR в RGB - if len(img_matches.shape) == 3 and img_matches.shape[2] == 3: - img_matches_rgb = cv2.cvtColor(img_matches, cv2.COLOR_BGR2RGB) - else: - img_matches_rgb = img_matches - - self.ax_matches.imshow(img_matches_rgb) - - # Добавляем информацию о трансформации - if transformation_info: - tx, ty = transformation_info['translation'] - angle = transformation_info['rotation'] - - info_text = f"Translation: ({tx:.2f}, {ty:.2f})" - info_text2 = f"Rotation: {angle:.2f} rad ({np.degrees(angle):.1f}°)" - - self.ax_matches.text(10, 30, info_text, fontsize=8, color='green', - bbox=dict(boxstyle="round,pad=0.3", facecolor="white", alpha=0.8)) - self.ax_matches.text(10, 90, info_text2, fontsize=8, color='green', - bbox=dict(boxstyle="round,pad=0.3", facecolor="white", alpha=0.8)) - - self.ax_matches.axis('off') - - def update_display(self): - """Обновляет отображение всех областей""" - self.fig.canvas.draw() - self.fig.canvas.flush_events() - plt.pause(0.01) - - def close(self): - """Закрывает окно""" - plt.close(self.fig) - - def show_final(self): - """Показывает финальное состояние окна""" - plt.ioff() - print("Симуляция завершена. Окно визуализации остается открытым для анализа.") - plt.pause(100000) - - def pause(self, duration: float): - plt.pause(duration)