feat: add new arguments, correct yandex map

autopilot.py

@@ -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.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.prev_chunk.detect_and_match_keypoints(current_chunk)
         matching_timer.stop()
         print(f"Matching calculating: {matching_timer.get_elapsed() * 1000:.2f} ms")
 

main.py

@@ -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):
+def get_map(map_name: str = 'google', lat=49.103814, lon=55.794258, zoom=18):
-    if map_name == 'google': return GoogleMap(initial_lat, initial_lon, initial_zoom)
+    if map_name == 'google': return GoogleMap(lat, lon, zoom)
-    if map_name == 'yandex': return YandexMap(initial_lat, initial_lon, initial_zoom)
+    if map_name == 'yandex': return YandexMap(lat, lon, zoom)
     return None
 
-def make_global_photo(filename, initial_zoom=13, map_name: str = 'google'):
+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, initial_zoom=initial_zoom)
+    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)
-

visualization.py

@@ -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 = self.fig.add_subplot(gs[1, 1])
-        self.ax_motion_vectors.set_title('Motion Vectors - Движение ключевых точек')
+        # self.ax_motion_vectors.set_title('Motion Vectors - Движение ключевых точек')
-        self.ax_motion_vectors.axis('off')
+        # 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):
         """Обновляет визуализацию сопоставления точек"""

yandex_map.py

@@ -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)