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

feat: add error graphic

Browse Source
This commit is contained in:
russian_proger
2025-07-01 12:25:57 +03:00
parent 9a289bd372
commit f78b01dcec
3 changed files with 69 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
autopilot.py
View File

@@ -3,6 +3,7 @@ import cv2
import numpy as np
from PIL import Image
import math
from visualization import VisualizationManager
random.seed(1)
@@ -10,6 +11,7 @@ class Pilot:
def __init__(self): pass
def handle(self, image: Image): pass
def act(self) -> tuple[float, float] | None: pass
def get_position(self) -> tuple[float, float]: pass
class AutoPilot(Pilot):
prev_image: np.ndarray | None
@@ -20,7 +22,7 @@ class AutoPilot(Pilot):
bf_matcher: cv2.BFMatcher
frame_count: int
image_center: tuple # Центр изображения (x, y)
viz_manager: object # Менеджер визуализации (опционально)
viz_manager: VisualizationManager # Менеджер визуализации (опционально)
def __init__(self, viz_manager=None):
self.prev_image = None
@@ -46,6 +48,9 @@ class AutoPilot(Pilot):
# Инициализация матчера для сопоставления ключевых точек
self.bf_matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
def get_position(self):
return self.x, self.y
def image_to_numpy(self, image: Image.Image) -> np.ndarray:
"""Конвертирует PIL Image в numpy array для OpenCV"""
return np.array(image)
@@ -255,6 +260,7 @@ class AutoPilot(Pilot):
# Выводим текущее состояние БПЛА
drone_state = self.get_drone_state()
self.viz_manager.update_drone_trajectory(drone_state['x'], drone_state['y'])
print(f" [Pilot] Drone Position: ({drone_state['x']:.2f}, {drone_state['y']:.2f})")
print(f" [Pilot] Angle: {drone_state['angle_degrees']:.1f}°")
@@ -285,7 +291,7 @@ class AutoPilot(Pilot):
distance_to_target = math.sqrt(self.x**2 + self.y**2)
# Если дрон находится рядом с целью, останавливаемся
if distance_to_target < 10.0:
if distance_to_target < 30.0:
return None
# Вычисляем угол к цели
@@ -320,7 +326,7 @@ class RandomPilot(Pilot):
def act(self) -> tuple[float, float] | None:
self.counter += 1
if self.counter > 40:
if self.counter > 300:
return None
return 1 / (self.counter + 20), 10.0

11
simulator.py
View File

@@ -127,6 +127,8 @@ class Simulator:
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)
@@ -154,14 +156,9 @@ class Simulator:
# Передаем изображение в AutoPilot для анализа
self.autonomePilot.handle(rotated_im)
rotated_im.save(f"./images/{i}.png")
# Обновляем визуализацию каждые несколько итераций для производительности
if i % 5 == 0:
self.update_map()
self.viz_manager.update_display()
self.viz_manager.pause(0.25)
self.update_map()
self.viz_manager.update_display()
# Финальное обновление карты
self.update_map()

79
visualization.py
View File

@@ -27,7 +27,7 @@ class VisualizationManager:
def __init__(self, window_title="Drone Autopilot Visualization"):
self.window_title = window_title
self.fig = None
self.ax_drone_view = None
self.ax_error_plot = None # График погрешности позиции
self.ax_global_map = None
self.ax_detection = None
self.ax_matches = None
@@ -39,14 +39,19 @@ class VisualizationManager:
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.drone_view_image = None
self._setup_window()
def _setup_window(self):
@@ -61,10 +66,12 @@ class VisualizationManager:
# Создаем сетку 2x2 с разными размерами колонок
gs = self.fig.add_gridspec(2, 2, hspace=0.3, wspace=0.3, width_ratios=[1, 1])
# Вид БПЛА (левый верхний угол)
self.ax_drone_view = self.fig.add_subplot(gs[0, 0])
self.ax_drone_view.set_title('Вид БПЛА')
self.ax_drone_view.axis('off')
# График погрешности позиции (левый верхний угол)
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])
@@ -123,6 +130,11 @@ class VisualizationManager:
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='Начальная точка')
@@ -140,6 +152,13 @@ class VisualizationManager:
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)
@@ -148,23 +167,36 @@ class VisualizationManager:
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_view(self, image: np.ndarray):
"""Обновляет вид БПЛА"""
self.drone_view_image = image.copy()
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)
self.ax_drone_view.clear()
self.ax_drone_view.set_title('Вид БПЛА')
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)
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.error_times.append(frame_count)
self.position_errors.append(error)
self.ax_drone_view.imshow(image_rgb)
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)
self.ax_drone_view.axis('off')
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):
"""Обновляет визуализацию детекции ключевых точек"""
@@ -214,18 +246,14 @@ class VisualizationManager:
if transformation_info:
tx, ty = transformation_info['translation']
angle = transformation_info['rotation']
scale = transformation_info['scale']
info_text = f"Translation: ({tx:.2f}, {ty:.2f})"
info_text2 = f"Rotation: {angle:.2f} rad ({np.degrees(angle):.1f}°)"
info_text3 = f"Scale: {scale:.2f}"
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.text(10, 150, info_text3, fontsize=8, color='green',
bbox=dict(boxstyle="round,pad=0.3", facecolor="white", alpha=0.8))
self.ax_matches.axis('off')
@@ -243,6 +271,7 @@ class VisualizationManager:
"""Показывает финальное состояние окна"""
plt.ioff()
print("Симуляция завершена. Окно визуализации остается открытым для анализа.")
plt.pause(100000)
def pause(self, duration: float):
plt.pause(duration)