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

83
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)
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.ax_drone_view.clear()
self.ax_drone_view.set_title('Вид БПЛА')
self.error_times.append(frame_count)
self.position_errors.append(error)
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_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)
self.ax_drone_view.imshow(image_rgb)
self.ax_drone_view.axis('off')
# Автоматически масштабируем оси
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)
plt.pause(duration)