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

feat: motion graphic on gomography matrix

Browse Source
This commit is contained in:
russian_proger
2025-10-04 23:08:26 +03:00
parent dc8c869bcf
commit e5715e17da
4 changed files with 135 additions and 91 deletions
Download Patch File Download Diff File Expand all files Collapse all files

59
autopilot.py
View File

@@ -272,11 +272,6 @@ class AutoPilot(Pilot):
height, width = self.prev_image.shape[:2]
self.image_center = (width // 2, height // 2)
# Обновляем визуализацию детекции
if self.vis_manager is not None:
kp, _ = self.orb_detector.detectAndCompute(self.prev_image, None)
self.vis_manager.update_detection(self.prev_image, kp)
return PilotCommand()
# Конвертируем текущее изображение
@@ -314,8 +309,6 @@ class AutoPilot(Pilot):
# Обновляем визуализацию
if self.vis_manager:
# Обновляем детекцию ключевых точек
self.vis_manager.update_detection(current_image, kp2)
# Обновляем сопоставление точек
self.vis_manager.update_matches(
@@ -323,6 +316,16 @@ class AutoPilot(Pilot):
current_image,
kp1, kp2, matches,
transformation_info)
self.vis_manager.update_motion_vectors(
current_image,
kp1, kp2, matches)
mask = transformation_info['mask']
self.vis_manager.update_motion_gomography(
current_image,
kp1, kp2,
np.array(matches)[mask.ravel().astype(bool)])
# Пытаемся найти ориентир на картинке:
landmark_image = cv2.imread(Path('chunks') / f'chunk_{self.target_idx}.png', cv2.IMREAD_COLOR_RGB)
@@ -389,45 +392,3 @@ class AutoPilot(Pilot):
self.y = y
self.angle = angle
self.frame_count = 0
class RandomPilot(Pilot):
counter: int
def __init__(self, velocity: float = 1):
self.counter = 0
def act(self) -> tuple[float, float] | None:
self.counter += 1
if self.counter > 300:
return None
return 1 / (self.counter + 20), 10.0
# def _test():
# randomPilot = RandomPilot()
# point = [0, 0]
# iter_count = 100
# points = [point.copy()]
# for i in range(iter_count):
# dx, dy = randomPilot.step()
# prev_point = point.copy()
# point[0] += dx
# point[1] += dy
# points.append(point.copy())
# coords = list(zip(*points))
# padding = 5
# plt.axis([
# min(coords[0]) - padding, max(coords[0]) + padding,
# min(coords[1]) - padding, max(coords[1]) + padding])
# for i in range(iter_count):
# plt.plot(coords[0][i:i+2], coords[1][i:i+2], color='#5e5')
# plt.pause(0.05)
# sleep(1)
# if __name__ == '__main__':
# _test()

3
main.py
View File

@@ -1,4 +1,3 @@
from autopilot import AutoPilot, RandomPilot
from simulator import Simulator
from visualization import VisualizationManager
from trajectory_drawer import TrajectoryDrawer
@@ -112,7 +111,7 @@ def main():
photo.save(Path('images') / f"photo_{i}.png")
vis_manager.update_display()
vis_manager.pause(1)
vis_manager.pause(0.2)
if command.stop:
break

8
todo.md
View File

@@ -1,4 +1,8 @@
[-] График межкадрового смещения
[+] График межкадрового смещения
| [+] График межкадровых смещениях по версии матрицы гомографии
| [+] Уменьшение погрешность за счёт удаления выбросов
| [-] Исследовать причину погрешности при развороте
[-] Отображение bounding box на графике сопоставления точек кадра с ориентиром
[-] Проверка корректности выявления ориентира на кадре
[-] Исправление коррекции координат на основе сопоставления с ориентиром
@@ -10,4 +14,4 @@
[?] Изменение масштаба во время полёта, обработка этой трансформации
[?] Поворот ориентиров
[?] Ограничение выбора точек, чтобы ориентиры полностью попадали в кадр
[?] Ограничение выбора точек при построении маршрута, чтобы ориентиры полностью попадали в кадр

156
visualization.py
View File

@@ -3,6 +3,7 @@
Модуль для управления общим окном визуализации
"""
import matplotlib.axes
import matplotlib.pyplot as plt
import matplotlib.patches as patches
import numpy as np
@@ -32,6 +33,7 @@ class VisualizationManager:
self.ax_detection = None
self.ax_matches = None
self.ax_chunk_matches = None
self.ax_motion_vectors = None
# Данные для глобальной карты
self.trajectory_x = []
@@ -54,7 +56,7 @@ class VisualizationManager:
self.current_frame = None
self.keypoints = []
self.matches = []
self._setup_window()
def _setup_window(self):
@@ -66,7 +68,7 @@ class VisualizationManager:
# Открываем окно на полный экран
self.fig.canvas.manager.window.state('zoomed')
# Создаем сетку 2x2 с разными размерами колонок
# Создаем сетку 3x3 с разными размерами колонок
gs = self.fig.add_gridspec(2, 3, hspace=0.3, wspace=0.3, width_ratios=[1, 0.7, 1])
# График погрешности позиции (левый верхний угол)
@@ -76,7 +78,7 @@ class VisualizationManager:
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 координата')
@@ -85,21 +87,26 @@ class VisualizationManager:
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, 2])
# Сопоставление точек (правый верхний угол)
self.ax_matches = self.fig.add_subplot(gs[0, 2])
self.ax_matches.set_title('Feature Matching')
self.ax_matches.axis('off')
# Сопоставление точек (правый нижний угол)
self.ax_chunk_matches = self.fig.add_subplot(gs[0, 2])
# Сопоставление точек (средний средний угол)
self.ax_chunk_matches = self.fig.add_subplot(gs[1, 2])
self.ax_chunk_matches.set_title('Chunk Matching')
self.ax_chunk_matches.axis('off')
# Визуализация движения ключевых точек (левый нижний угол)
self.ax_motion_vectors = self.fig.add_subplot(gs[1, 1])
self.ax_motion_vectors.set_title('Motion Vectors - Движение ключевых точек')
self.ax_motion_vectors.axis('off')
# Визуализация движения ключевых точек на основе матрицы гомографии
self.ax_motion_gomography = self.fig.add_subplot(gs[0, 1])
self.ax_motion_gomography.set_title('Keypoint Detection')
self.ax_motion_gomography.axis('off')
# Настройки окна
self.fig.canvas.manager.window.attributes('-topmost', False)
@@ -211,32 +218,7 @@ class VisualizationManager:
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):
"""Обновляет визуализацию сопоставления точек"""
@@ -305,6 +287,104 @@ class VisualizationManager:
self.ax_chunk_matches.axis('off')
def _update_motion_vectors(self, axes: matplotlib.axes.Axes, current_frame: np.ndarray, prev_keypoints, current_keypoints, matches=None):
"""Обновляет визуализацию движения ключевых точек между кадрами"""
axes.clear()
axes.set_title('Motion Vectors - Движение ключевых точек')
if current_frame is not None:
# Конвертируем BGR в RGB для matplotlib
if len(current_frame.shape) == 3 and current_frame.shape[2] == 3:
frame_rgb = cv2.cvtColor(current_frame, cv2.COLOR_BGR2RGB)
else:
frame_rgb = current_frame
# Показываем текущий кадр
axes.imshow(frame_rgb)
# Если есть совпадения, рисуем векторы движения
if matches is not None and len(matches) > 0:
# Получаем координаты ключевых точек
prev_pts = np.array([prev_keypoints[m.queryIdx].pt for m in matches])
curr_pts = np.array([current_keypoints[m.trainIdx].pt for m in matches])
# Вычисляем векторы движения
motion_vectors = curr_pts - prev_pts
# Вычисляем длину и направление векторов
vector_lengths = np.linalg.norm(motion_vectors, axis=1)
vector_angles = np.arctan2(motion_vectors[:, 1], motion_vectors[:, 0])
# Нормализуем длины для цветовой карты (0-1)
if len(vector_lengths) > 0:
max_length = np.max(vector_lengths)
if max_length > 0:
normalized_lengths = vector_lengths / max_length
else:
normalized_lengths = np.zeros_like(vector_lengths)
else:
normalized_lengths = np.array([])
# Рисуем векторы с цветовой индикацией
for i, (start_pt, end_pt, length, angle, norm_length) in enumerate(
zip(prev_pts, curr_pts, vector_lengths, vector_angles, normalized_lengths)):
# Цвет зависит от направления (угол -> HSV)
hue = (angle + np.pi) / (2 * np.pi) # Нормализуем угол к 0-1
saturation = 1.0
value = 0.8 + 0.2 * norm_length # Яркость зависит от длины
# Конвертируем HSV в RGB
import matplotlib.colors as mcolors
rgb = mcolors.hsv_to_rgb([hue, saturation, value])
# Толщина линии зависит от длины вектора
linewidth = max(1, min(5, 2 + 3 * norm_length))
# Рисуем вектор
axes.arrow(
start_pt[0], start_pt[1],
end_pt[0] - start_pt[0], end_pt[1] - start_pt[1],
head_width=3, head_length=5,
fc=rgb, ec=rgb, alpha=0.8, linewidth=linewidth
)
# Добавляем текст с информацией о движении
# if length > 5: # Показываем только для значительных движений
# axes.text(
# end_pt[0] + 5, end_pt[1] + 5,
# f'{length:.1f}px', fontsize=6, color='white',
# bbox=dict(boxstyle="round,pad=0.2", facecolor="black", alpha=0.7)
# )
# Добавляем легенду с цветовой схемой
# legend_text = "Цвет: направление, Яркость: скорость"
# axes.text(
# 10, 30, legend_text, fontsize=8, color='white',
# bbox=dict(boxstyle="round,pad=0.3", facecolor="black", alpha=0.8)
# )
# Статистика движения
# if len(vector_lengths) > 0:
# avg_speed = np.mean(vector_lengths)
# max_speed = np.max(vector_lengths)
# stats_text = f"Средняя скорость: {avg_speed:.1f}px\nМаксимальная: {max_speed:.1f}px"
# axes.text(
# 10, 60, stats_text, fontsize=8, color='white',
# bbox=dict(boxstyle="round,pad=0.3", facecolor="black", alpha=0.8)
# )
axes.axis('off')
def update_motion_vectors(self, current_frame: np.ndarray, prev_keypoints, current_keypoints, matches=None):
self._update_motion_vectors(self.ax_motion_vectors, current_frame, prev_keypoints, current_keypoints, matches)
def update_motion_vectors(self, current_frame: np.ndarray, prev_keypoints, current_keypoints, matches=None):
self._update_motion_vectors(self.ax_motion_vectors, current_frame, prev_keypoints, current_keypoints, matches)
def update_motion_gomography(self, current_frame: np.ndarray, prev_keypoints, current_keypoints, matches=None):
self._update_motion_vectors(self.ax_motion_gomography, current_frame, prev_keypoints, current_keypoints, matches)
def update_display(self):
"""Обновляет отображение всех областей"""
self.fig.canvas.draw()