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

feat/pitch-roll #1

Merged
zenloger merged 24 commits from feat/pitch-roll into main 2026-02-20 16:50:16 +03:00
Conversation 0 Commits 24 Files Changed 44 +133 -38
5 changed files with 133 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 05c249ed78 - Show all commits

6
autopilot.py
View File

@@ -94,7 +94,7 @@ class AutoPilot(Pilot):
h, w = prev_gray.shape[:2]
# Создаем сетку точек для отслеживания (аналогично вашему step=20)
step = 35
step = 20
grid_points = []
for y in range(step, h - step, step):
for x in range(step, w - step, step):
@@ -250,8 +250,8 @@ class AutoPilot(Pilot):
# Вычисляем оптический поток для покадрового сравнения
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)
# 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")

67
main.py
View File

@@ -19,13 +19,13 @@ import pickle
import random
import utility
def get_map(map_name: str = 'google', initial_lat=49.103814, initial_lon=55.794258, initial_zoom=18):
if map_name == 'google': return GoogleMap(initial_lat, initial_lon, initial_zoom)
if map_name == 'yandex': return YandexMap(initial_lat, initial_lon, initial_zoom)
def get_map(map_name: str = 'google', lat=49.103814, lon=55.794258, zoom=18):
if map_name == 'google': return GoogleMap(lat, lon, zoom)
if map_name == 'yandex': return YandexMap(lat, lon, zoom)
return None
def make_global_photo(filename, initial_zoom=13, map_name: str = 'google'):
online_map: YandexMap | GoogleMap = get_map(map_name, initial_zoom=initial_zoom)
def make_global_photo(filename, map_name: str = 'google', lat=49.103814, lon=55.794258, zoom=13):
online_map: YandexMap | GoogleMap = get_map(map_name, lat, lon, zoom)
online_map.save_photo(filename)
online_map.destroy()
@@ -36,7 +36,7 @@ def get_trajectory_points(bg_img: str) -> list[(float, float)]:
points = list(map(lambda p: [p[0] / trajectoryDrawer.img.shape[1], p[1] / trajectoryDrawer.img.shape[0]], trajectoryDrawer.points))
return points
def build(name: str, map_name: str = 'google'):
def build(name: str, map_name: str, lat: float, lon: float):
# Создание папки с информацией о маршруте
dir = Path('trajectories')
@@ -47,9 +47,9 @@ def build(name: str, map_name: str = 'google'):
dir_chunks = dir / 'chunks'
dir_chunks.mkdir()
make_global_photo('map.jpg', 15, map_name)
make_global_photo('map.jpg', map_name, lat, lon, 15)
points = get_trajectory_points('map.jpg')
online_map: YandexMap | GoogleMap = get_map(map_name, initial_zoom=15)
online_map: YandexMap | GoogleMap = get_map(map_name, lat, lon, 15)
width, height = online_map.get_size()
@@ -66,7 +66,7 @@ def build(name: str, map_name: str = 'google'):
sleep(2)
geo = online_map.get_geolocation()
online_map.open(geo['lat'], geo['lon'], 18)
sleep(20)
sleep(5)
points_coords_pixel = points_coords.copy() / online_map.pixel_ratio
@@ -120,7 +120,7 @@ def build(name: str, map_name: str = 'google'):
sleep(15)
online_map.destroy()
def run(name: str, map_name: str = 'yandex'):
def run(name: str, map_name: str):
dir = Path('trajectories')
assert dir.exists()
dir /= name
@@ -158,7 +158,7 @@ def run(name: str, map_name: str = 'yandex'):
vis_manager.update_display()
vis_manager.pause(1)
vis_manager.set_target_points(points)
vis_manager.set_target_points(data['points'])
proc_time = np.array([])
@@ -198,7 +198,7 @@ def run(name: str, map_name: str = 'yandex'):
vis_manager.pause(0.2)
last_proc_times = proc_time[-10:]
print(F"Image #{i}")
print(F"\nImage #{i}")
print("Average FPS:", 1 / last_proc_times.mean())
print("Pilot coords:", pilot.pos)
print("Simulator coords:", simulator.pos)
@@ -214,15 +214,15 @@ def run(name: str, map_name: str = 'yandex'):
print("Average FPS:", 1 / proc_time.mean())
vis_manager.show_final()
def main(mode: str, name: str):
def main(mode: str, name: str, lat: float, lon: float, ref: str, sim: str):
if name is None:
name = utility.generate_folder_name()
if mode == 'build' or mode == 'standalone':
build(name)
build(name, ref, lat, lon)
if mode == 'run' or mode == 'standalone':
run(name)
run(name, sim)
def parse_args():
"""Парсер аргументов командной строки"""
@@ -244,6 +244,39 @@ def parse_args():
help='Название траектории (обязательно для режимов build и run)'
)
# Координаты
parser.add_argument(
'--lat',
type=float,
default=49.103814,
help='Широта (по умолчанию 49.103814)'
)
parser.add_argument(
'--lon',
type=float,
default=55.794258,
help='Долгота (по умолчанию 55.794258)'
)
# Источник эталонных изображений (ориентиров)
parser.add_argument(
'--reference',
type=str,
default='google',
choices=['google', 'yandex'],
help='Откуда берутся эталонные изображения (ориентиры): google или yandex (по умолчанию google)'
)
# Место проведения симуляции
parser.add_argument(
'--simulation',
type=str,
default='yandex',
choices=['google', 'yandex'],
help='Где проводится симуляция: google или yandex (по умолчанию yandex)'
)
# Парсим аргументы
args = parser.parse_args()
@@ -253,7 +286,7 @@ def parse_args():
return args
if __name__ == "__main__":
args = parse_args()
main(args.mode, args.name)
main(args.mode, args.name, args.lat, args.lon, args.reference, args.simulation)

41
test_chunks.ipynb
View File

File diff suppressed because one or more lines are too long

53
visualization.py
View File

@@ -3,14 +3,16 @@
Модуль для управления общим окном визуализации
"""
from PIL import Image
from enum import Enum
from scipy.interpolate import make_interp_spline
import cv2
import matplotlib
import matplotlib.axes
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')
@@ -93,14 +95,14 @@ class VisualizationManager:
self.ax_matches.axis('off')
# Сопоставление точек (средний средний угол)
self.ax_chunk_matches = self.fig.add_subplot(gs[1, 2])
self.ax_chunk_matches = self.fig.add_subplot(gs[1, 1:3])
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_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])
@@ -157,7 +159,7 @@ class VisualizationManager:
# Рисуем текущую целевую точку
if self.target_idx < len(self.target_pts):
pt = self.target_pts[self.target_idx]
self.ax_global_map.plot(pt[0], pt[1], 'yo', markersize=8, label='Цель (0, 0)')
self.ax_global_map.plot(pt[0], pt[1], 'yo', markersize=8, label='Цель')
self.ax_global_map.legend()
@@ -208,7 +210,37 @@ class VisualizationManager:
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_error_plot.plot(self.error_times, self.position_errors, 'b-',
linewidth=1, alpha=0.4, label='Погрешность данных')
if len(self.error_times) > 5:
# Сглаженный график
smoothed_times = np.linspace(self.error_times[0], self.error_times[-1], 300)
spl = make_interp_spline(self.error_times, self.position_errors, k=3)
smoothed_errors = spl(smoothed_times)
self.ax_error_plot.plot(smoothed_times, smoothed_errors, 'orange',
linewidth=2, label='Сглаженный тренд')
# if len(self.position_errors) > 5: # Достаточно данных для сглаживания
# window_size = min(11, len(self.position_errors) // 3) # Адаптивный размер окна
# if window_size % 2 == 0: # Должен быть нечетным
# window_size += 1
# # Метод скользящего среднего
# smoothed_errors = np.convolve(
# self.position_errors,
# np.ones(window_size) / window_size,
# mode='valid'
# )
# # Корректируем временную ось для сглаженных данных
# offset = (window_size - 1) // 2
# smoothed_times = self.error_times[offset:offset + len(smoothed_errors)]
self.ax_error_plot.legend(loc='upper right')
# Автоматически масштабируем оси
if len(self.position_errors) > 0:
@@ -219,6 +251,7 @@ class VisualizationManager:
else:
self.ax_error_plot.set_ylim(0, 1)
def update_matches(self, img1: np.ndarray, img2: np.ndarray,
kp1, kp2, matches, transformation_info=None):
"""Обновляет визуализацию сопоставления точек"""

4
yandex_map.py
View File

@@ -44,6 +44,8 @@ class YandexMap:
self.driver.execute_script("arguments[0].remove();", self.driver.find_element(By.XPATH, "//nav[@class='map-controls']"))
self.driver.execute_script("arguments[0].remove();", self.driver.find_element(By.XPATH, "//footer"))
self.move(39, -9)
sleep(0.2)
def open(self, lat, lon, zoom):
@@ -59,6 +61,8 @@ class YandexMap:
action.click(self.driver.find_element(By.CLASS_NAME, 'sidebar-toggle-button'))
action.perform()
self.move(39, -9)
def save_photo(self, filename: str) -> bytes:
return self.driver.save_screenshot(filename)