autopilot.py

@@ -56,14 +56,16 @@ class AutoPilot(Pilot):
 
     # Положение на основе ориентира
     reserved_pos: Position | None
+    proccessing_time: float
 
-    def __init__(self, points = [], chunks = [], viz_manager=None):
+    def __init__(self, points = [], chunks = [], viz_manager=None, pixel_ratio: float = 1.):
         self.prev_chunk = None
         self.pos = Position(0, 0, 1, 0, 0, 0)
         self.chunks = chunks
         self.frame_count = 0
         self.vis_manager = viz_manager  # Менеджер визуализации
         self.reserved_pos = None
+        self.pixel_ratio = pixel_ratio
 
         # Пороговые значения качества сопоставления/гомографии
         self.min_inliers: int = 12
@@ -76,6 +78,7 @@ class AutoPilot(Pilot):
         self.target_idx = 0
 
         self.timer = Timer()
+        self.chunk_points = np.array([[chunk.pos.x, chunk.pos.y] for chunk in self.chunks])
 
     def get_position(self) -> tuple[float, float]:
         return self.pos.x, self.pos.y
@@ -133,9 +136,6 @@ class AutoPilot(Pilot):
         """
         self.pos.iapply(homography_matrix)
 
-        if self.reserved_pos is not None:
-            self.reserved_pos.iapply(homography_matrix)
-
     def get_drone_state(self) -> dict:
         """
         Возвращает текущее состояние БПЛА
@@ -149,9 +149,11 @@ class AutoPilot(Pilot):
         }
 
     def get_position_by_chunk(self) -> Position | None:
-        # Пытаемся найти ориентир на картинке:
+        cur_pos = np.array([self.pos.x, self.pos.y])
+        closest_chunk_idx = ((self.chunk_points - cur_pos) ** 2).sum(1).argmin()
+
         current_chunk = self.prev_chunk
-        landmark_chunk = self.chunks[self.target_idx]
+        landmark_chunk = self.chunks[closest_chunk_idx]
         src_pts, dst_pts, matches, kp1, kp2 = landmark_chunk.detect_and_match_keypoints(current_chunk)
 
         if src_pts is not None and dst_pts is not None and self.vis_manager:
@@ -161,30 +163,9 @@ class AutoPilot(Pilot):
             if was_enabled: self.timer.start()
 
         if src_pts is not None and dst_pts is not None:
-            # Оцениваем матрицу трансформации
-            landmark_transform = self.estimate_transformation_matrix(src_pts, dst_pts)
-            # Если ориентир достоверно найден — скорректируем глобальные координаты и угол
+            landmark_transform = estimate_transformation_matrix(src_pts, dst_pts)
             if landmark_transform is not None:
-                ok_scale = (self.min_scale <= landmark_transform['scale'] <= self.max_scale)
-                ok_inliers = (landmark_transform.get('inliers', 0) >= self.min_inliers)
-                ratio = landmark_transform.get('inliers_ratio', 0.0)
-                ok_ratio = (ratio >= self.min_inlier_ratio)
-                rmse = landmark_transform.get('rmse', None)
-                ok_rmse = (rmse is not None and rmse <= self.max_reproj_rmse)
-                if ok_scale and ok_inliers and ok_ratio and ok_rmse:
-                    # print("[HELP]")
-                    # print("Matrix", landmark_transform['homography'])
-                    # print("Position", self.x, self.y)
-                    # print("Position of point", self.points[self.target_idx])
-                    # print("[PILOT]", rmse, ratio, ok_rmse)
-                # if False:
-                    # Считаем абсолютную позу относительно координат ориентира
-                    landmark_world_x, landmark_world_y = self.points[self.target_idx]
-                    landmark = Position(landmark_world_x, landmark_world_y, 1, 0)
-                    homography = landmark_transform['homography']
-                    # homography = np.linalg.inv(homography)
-                    print(f"  [Pilot] Landmark correction applied (inliers={landmark_transform['inliers']}, ratio={ratio:.2f}, rmse={rmse:.2f})")
-                    return landmark @ homography
+                return landmark_chunk.pos.apply(landmark_transform)
         return None
 
 
@@ -200,8 +181,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")
 
@@ -234,9 +215,9 @@ class AutoPilot(Pilot):
 
         # Пытаемся найти ориентир на картинке:
         self.prev_chunk = current_chunk
-        # npos = self.get_position_by_chunk()
-        # if npos is not None:
-        #     self.reserved_pos = npos
+        pos_by_chunk = self.get_position_by_chunk()
+        if pos_by_chunk is not None:
+            self.pos = pos_by_chunk
 
         chunk_timer.stop()
         print(f"Chunk timer: {chunk_timer.get_elapsed() * 1000:.2f} ms")
@@ -262,10 +243,6 @@ class AutoPilot(Pilot):
                 (self.points[self.target_idx][1] - self.pos.y) ** 2
             )
 
-            if self.reserved_pos is not None:
-                self.pos = self.reserved_pos
-                self.reserved_pos = None
-
         # Вычисляем угол к цели
         target_angle = math.atan2(self.points[self.target_idx][1] - self.pos.y, self.points[self.target_idx][0] - self.pos.x)
         
@@ -279,8 +256,8 @@ class AutoPilot(Pilot):
         if angle_diff >= math.pi:
             angle_diff -= 2 * math.pi
 
-        d_r = max(10, min(35., distance_to_target / 2))
-        d_a_limit = d_r / 10 * 0.01
+        d_r = max(5, min(10., distance_to_target / 2 * self.pixel_ratio))
+        d_a_limit = np.radians(5)
 
         command = PilotCommand(
             max(min(d_a_limit, angle_diff), -d_a_limit),

constants.py

@@ -1,9 +1,15 @@
 import numpy as np
 
 # Ширина 1 пикселя в метрах
-YANDEX_PIXEL_RATIO_18 = 0.332697807435653
-GOOGLE_PIXEL_RATIO_15 = 2766 / 1031
-GOOGLE_PIXEL_RATIO_18 = 346 / 1031
+YANDEX_PIXEL_RATIO = {
+    15: 2830 / 1049,
+    18: 350 / 1049,
+}
+
+GOOGLE_PIXEL_RATIO = {
+    15: 2766 / 1031,
+    18: 346 / 1031,
+}
 
 WINDOW_HEIGHT = 1031
 

google_map.py

@@ -1,10 +1,3 @@
-
-import cv2
-import math
-import numpy as np
-import os
-import utility
-
 from io import BytesIO
 from PIL import Image
 from selenium.webdriver.common.actions.wheel_input import ScrollOrigin
@@ -13,6 +6,13 @@ from selenium.webdriver.common.by import By
 from selenium.webdriver.common.action_chains import ActionChains
 from time import sleep
 
+import constants
+import cv2
+import math
+import numpy as np
+import os
+import utility
+
 def generateURL(lat: float, lon: float, zoom: int):
     return f"https://www.google.com/maps/@{lon},{lat},{zoom}z"
 
@@ -20,11 +20,13 @@ class GoogleMap:
     initial_zoom: int
     initial_lat: float
     initial_lon: float
+    pixel_ratio: float
 
     def __init__(self, initial_lat=49.103814, initial_lon=55.794258, initial_zoom=18):
         self.initial_lat = initial_lat
         self.initial_lon = initial_lon
         self.initial_zoom = initial_zoom
+        self.pixel_ratio = constants.GOOGLE_PIXEL_RATIO[self.initial_zoom]
 
         options = webdriver.ChromeOptions()
         # options.add_experimental_option("detach", True)
@@ -38,6 +40,10 @@ class GoogleMap:
         sleep(5)
 
     def open(self, lat, lon, zoom):
+        self.initial_lat = lat
+        self.initial_lon = lon
+        self.initial_zoom = zoom
+        self.pixel_ratio = constants.GOOGLE_PIXEL_RATIO[self.initial_zoom]
         self.driver.get(generateURL(lat, lon, zoom))
 
     def save_photo(self, filename: str):

main.py

@@ -19,10 +19,15 @@ import pickle
 import random
 import utility
 
-def make_global_photo(filename, initial_zoom=13):
-    google_map = GoogleMap(initial_zoom=initial_zoom)
-    google_map.save_photo(filename)
-    google_map.destroy()
+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)
+    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)
+    online_map.save_photo(filename)
+    online_map.destroy()
 
 def get_trajectory_points(bg_img: str) -> list[(float, float)]:
     trajectoryDrawer = TrajectoryDrawer(bg_img)
@@ -31,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):
+def build(name: str, map_name: str = 'google'):
 
     # Создание папки с информацией о маршруте
     dir = Path('trajectories')
@@ -42,29 +47,31 @@ def build(name: str):
     dir_chunks = dir / 'chunks'
     dir_chunks.mkdir()
 
-    # make_global_photo('map.jpg', 15)
+    make_global_photo('map.jpg', 15, map_name)
     points = get_trajectory_points('map.jpg')
-    google_map = GoogleMap(initial_zoom=15)
+    online_map: YandexMap | GoogleMap = get_map(map_name, initial_zoom=15)
 
-    # Начнём симуляцию полёта с первой точки
-    google_map.make_as_center(*points[0])
-    sleep(3)
-    google_map.scroll(0.5, 0.5, 10, True)
-    sleep(2)
-    geo = google_map.get_geolocation()
-    google_map.open(geo['lat'], geo['lon'], 18)
-    sleep(20)
 
-    width, height = google_map.get_size()
+    width, height = online_map.get_size()
     points_coords = np.array(list(map(lambda p: [
             (p[0] - points[0][0]) * width, (points[0][1] - p[1]) * height
         ], points)))
 
-    points_coords *= constants.GOOGLE_PIXEL_RATIO_15
-    points_coords_pixel = points_coords / constants.GOOGLE_PIXEL_RATIO_18
+    points_coords *= online_map.pixel_ratio
 
-    pilot = autopilot.AutoPilot(points_coords_pixel)
-    simulator = Simulator(google_map)
+    # Начнём симуляцию полёта с первой точки
+    online_map.make_as_center(*points[0])
+    sleep(3)
+    online_map.scroll(0.5, 0.5, 10, True)
+    sleep(2)
+    geo = online_map.get_geolocation()
+    online_map.open(geo['lat'], geo['lon'], 18)
+    sleep(20)
+
+    points_coords_pixel = points_coords.copy() / online_map.pixel_ratio
+
+    pilot = autopilot.AutoPilot(points_coords_pixel, pixel_ratio=online_map.pixel_ratio)
+    simulator = Simulator(online_map)
     pilot.target_idx = 0
 
     pilot.pos = simulator.pos.copy()
@@ -73,24 +80,27 @@ def build(name: str):
     positions: list[Position] = []
 
     print("points_coords_pixel:", points_coords_pixel)
-    for i in range(100000):
+    for i in range(5000):
 
         if command.stop:
             break
 
-        # simulator.handle(command.dangle, command.velocity)
         chunk = simulator.get_chunk()
         pilot.pos = simulator.pos.copy()
         command = pilot.make_command()
-        command.velocity /= constants.GOOGLE_PIXEL_RATIO_18
+        command.velocity /= online_map.pixel_ratio
 
         print("Position:", simulator.pos)
         
         # Save Image
         chunk.save_image(dir_chunks / f"chunk_{len(positions)}.png")
-        positions.append(simulator.pos)
+
+        positions.append(simulator.pos.copy() * online_map.pixel_ratio)
 
         simulator.handle(command.dangle, command.velocity)
+        if i == 0 and map_name == 'google':
+            simulator.pos.x = 0
+            simulator.pos.y = 0
         sleep(1.5)
 
     data = {
@@ -99,18 +109,18 @@ def build(name: str):
         'initial_geolocation': geo
     }
 
+    print(points_coords)
+
     file_positions = dir / 'positions.pkl'
     with file_positions.open('wb') as file:
         pickle.dump(data, file)
 
-    with file_positions.open('rb') as file:
-        r = pickle.load(file)
-        print(r)
+    print("WRITE POINTS:", points)
 
     sleep(15)
-    google_map.destroy()
+    online_map.destroy()
 
-def run(name: str):
+def run(name: str, map_name: str = 'yandex'):
     dir = Path('trajectories')
     assert dir.exists()
     dir /= name
@@ -122,20 +132,24 @@ def run(name: str):
         data = pickle.load(file)
     
     initial_geolocation = data['initial_geolocation']
+
+    lat = initial_geolocation['lat']
+    lon = initial_geolocation['lon']
+    online_map: YandexMap | GoogleMap = get_map(map_name, lat, lon, 18)
+    sleep(2)
+
     chunks: list[VisionChunk] = []
     for i in range(len(data['chunk_positions'])):
         chunk = VisionChunk.load_image(dir_chunks / f"chunk_{i}.png")
-        chunk.pos = data['chunk_positions'][i]
-    
-    points = data['points'] / constants.YANDEX_PIXEL_RATIO_18
+        chunk.pos = data['chunk_positions'][i] / online_map.pixel_ratio
+        chunks.append(chunk)
 
-    yandex_map = YandexMap(initial_geolocation['lat'], initial_geolocation['lon'], 18)
-    sleep(10)
+    points = data['points'] / online_map.pixel_ratio
+    print("READ POINTS:", points)
 
     vis_manager = VisualizationManager()
-    width, height = yandex_map.get_size()
-    pilot = autopilot.AutoPilot(points, chunks, vis_manager)
-    simulator = Simulator(yandex_map)
+    pilot = autopilot.AutoPilot(points, chunks, vis_manager, online_map.pixel_ratio)
+    simulator = Simulator(online_map)
     pilot.target_idx = 0
 
     chunk = simulator.get_chunk()
@@ -148,35 +162,22 @@ def run(name: str):
 
     proc_time = np.array([])
 
-    zoom_next_event = random.randint(5, 10)
-
     errors = []
-    chunk_errors = []
-    chunk_improves = []
-
-    last_chunk_index = 0
+    
+    sleep(1)
 
     for i in range(10000000000):
-        print(f"Image #{i}")
-        if i == zoom_next_event:
-            r = random.randint(0, 1)
-            direction = ['up', 'down'][r]
-            # simulator.change_zoom(direction)
-            zoom_next_event = i + random.randint(20, 40)
-
-
-        # if i > 0:
-        #     simulator.set_pitch(np.sin(i / 10) * 5)
-        #     simulator.set_roll(np.sin(i / 15) * 5)
-        #     simulator.set_zoom(1.0 + np.sin(i / 10) * 0.3)
+        if i > 0:
+            simulator.set_pitch(np.sin(i / 10) * 5)
+            simulator.set_roll(np.sin(i / 15) * 5)
+            simulator.set_zoom(1.0 + np.sin(i / 10) * 0.3)
 
         if command.stop:
             break
 
-        # simulator.handle(command.dangle, command.velocity)
         chunk = simulator.get_chunk()
         command = pilot.handle(chunk)
-        command.velocity /= constants.YANDEX_PIXEL_RATIO_18
+        command.velocity /= online_map.pixel_ratio
 
         proc_time = np.append(proc_time, command.proccessing_time)
         
@@ -192,29 +193,27 @@ def run(name: str):
         vis_manager.update_error_plot(i, pilot.pos.x, pilot.pos.y, simulator.pos.x, simulator.pos.y)
 
         errors.append(np.hypot(pilot.pos.x - simulator.pos.x, pilot.pos.y - simulator.pos.y))
-        if last_chunk_index != pilot.target_idx:
-            last_chunk_index = pilot.target_idx
-            chunk_errors.append(errors[-1])
-            chunk_improves.append(errors[-1] - errors[max(len(errors) - 2, 0)])
 
         vis_manager.update_display()
         vis_manager.pause(0.2)
 
         last_proc_times = proc_time[-10:]
+        print(F"Image #{i}")
         print("Average FPS:", 1 / last_proc_times.mean())
         print("Pilot coords:", pilot.pos)
         print("Simulator coords:", simulator.pos)
+        sleep(0.5)
         simulator.handle(command.dangle, command.velocity)
+        if i == 0 and map_name == 'google':
+            simulator.pos.x = 0
+            simulator.pos.y = 0
 
     print("Errors:", errors)
     print("MSE:", (np.array(errors) ** 2).mean())
     print("RMSE:", (np.array(errors) ** 2).mean() ** 0.5)
-    print("Chunk errors:", chunk_errors)
-    print("Chunk error improves:", chunk_improves)
     print("Average FPS:", 1 / proc_time.mean())
     vis_manager.show_final()
 
-
 def main(mode: str, name: str):
     if name is None:
         name = utility.generate_folder_name()

position.py

@@ -42,10 +42,32 @@ class Position:
             f"roll={math.degrees(self.roll):.1f}°)"
         )
 
+    def __imul__(self, scalar: float):
+        self.x *= scalar
+        self.y *= scalar
+        self.z *= scalar
+        return self
+
+    def __mul__(self, scalar: float) -> 'Position':
+        pos = self.copy()
+        pos *= scalar
+        return pos
+
+    def __itruediv__(self, scalar: float):
+        self.x /= scalar
+        self.y /= scalar
+        self.z /= scalar
+        return self
+
+    def __truediv__(self, scalar: float) -> 'Position':
+        pos = self.copy()
+        pos /= scalar
+        return pos
+
     def get_homography_matrix(self, K_in: np.ndarray = constants.K, K_out: np.ndarray | None = None, sliding: bool = True) -> np.ndarray:
         """ Возвращает матрицу гомографии """
         R = self.get_rotation_matrix()
-        T = self.get_translation_matrix()
+        T = self.get_translation_matrix(K_in)
         if not sliding:
             T[0, 2] = T[1, 2] = 0
         if K_out is None: K_out = K_in
@@ -55,10 +77,10 @@ class Position:
         """Создает полную копию объекта"""
         return Position(self.x, self.y, self.z, self.yaw, self.pitch, self.roll)
     
-    def get_translation_matrix(self) -> np.ndarray:
+    def get_translation_matrix(self, K: np.ndarray = constants.K) -> np.ndarray:
         return np.array([
-            [1, 0, self.x / constants._K_FOCUS_DISTANCE],
-            [0, 1, self.y / constants._K_FOCUS_DISTANCE],
+            [1, 0, self.x / K[0][0]],
+            [0, 1, self.y / K[0][0]],
             [0, 0, self.z]
         ])
 
@@ -102,6 +124,13 @@ class Position:
         R = np.array(R)
         t = np.array(t)
 
+        # print(cv2.decomposeHomographyMat(inv(H), K))
+        # _, _, z, _ = cv2.decomposeHomographyMat(inv(H), K)
+        # print(z)
+        # z = z.copy()
+        # z *= constants._K_FOCUS_DISTANCE
+        # print(z)
+        
         T = inv(R) @ inv(K) @ H @ K
         ind = np.array([A[2][0] ** 2 + A[2][1] ** 2 for A in T])
         top_k = max(1, len(T) // 2)
@@ -122,9 +151,11 @@ class Position:
         self.yaw = rot[2]
 
         t = t[best_id].flatten()
-        self.x += -T[0] * constants._K_FOCUS_DISTANCE * self.z
-        self.y += T[1] * constants._K_FOCUS_DISTANCE * self.z
+        self.x -= T[0] * constants._K_FOCUS_DISTANCE
+        self.y += T[1] * constants._K_FOCUS_DISTANCE
         self.z = 1 + T[2]
+        T[0] *= constants._K_FOCUS_DISTANCE
+        T[1] *= constants._K_FOCUS_DISTANCE
 
     def apply(self, homography_matrix: np.ndarray, K = constants.K) -> 'Position':
         """Применяет матрицу трансформации для вычисления новой позиции и ориентации."""

simulator.py

@@ -54,8 +54,8 @@ class Simulator:
 
     def update_trajectory(self, dx: float, dy: float):
         """Обновляет координаты дрона"""
-        self.pos.x += dx * self.pos.z
-        self.pos.y += dy * self.pos.z
+        self.pos.x += dx
+        self.pos.y += dy
 
     def handle(self, dangle: float, velocity: float = 50) -> None:
         """
@@ -69,8 +69,8 @@ class Simulator:
         velocity = max(velocity, 10)
 
         # Вычисляем смещение на основе текущего yaw
-        dx = math.cos(math.pi / 2 + self.pos.yaw) * velocity / self.pos.z
-        dy = math.sin(math.pi / 2 + self.pos.yaw) * velocity / self.pos.z
+        dx = int(math.cos(math.pi / 2 + self.pos.yaw) * velocity)
+        dy = int(math.sin(math.pi / 2 + self.pos.yaw) * velocity)
 
         self.update_trajectory(dx, dy)
         self.online_map.move(dx, dy)

todo.md

@@ -19,6 +19,6 @@
 | [+] Исследовать причину погрешности при развороте
 [+] Устранение большой погрешности при повороте
 
-[ ] Переделать ключевые точки -> Optical Flow
-[ ] Добавить перспективу
+[+] Переделать ключевые точки -> Optical Flow
+[+] Добавить перспективу
 [ ] Эталоны на Google Maps, полёт тот же

utility.py

@@ -1,5 +1,7 @@
 from PIL import Image
 from datetime import datetime
+from urllib.parse import parse_qs, urlparse, unquote
+
 import constants
 import cv2
 import numpy as np
@@ -52,7 +54,30 @@ def generate_folder_name():
 
 
 
-
+def parse_yandex_maps_url(url):
+    """
+    Парсит URL Яндекс.Карт и извлекает lat, lon и zoom
+    Формат: ?ll=lon,lat&z=zoom
+    """
+    # Декодируем URL (на случай %2C вместо запятых)
+    url = unquote(url)
+    
+    # Парсим URL
+    parsed_url = urlparse(url)
+    params = parse_qs(parsed_url.query)
+    
+    if 'll' in params and 'z' in params:
+        # ll содержит "lon,lat"
+        ll_value = params['ll'][0]
+        lat, lon = map(float, ll_value.split(','))
+        zoom = int(params['z'][0])
+        
+        return {
+            'lat': lat,
+            'lon': lon,
+            'zoom': zoom
+        }
+    return None
 
 def parse_google_maps_url(url):
     """
@@ -88,7 +113,7 @@ async function simulateDrag(element, offsetX, offsetY) {
     const rect = element.getBoundingClientRect();
     const startX = rect.left + rect.width / 2;
     const startY = rect.top + rect.height / 2;
-    const step = 7
+    const step = 10
     const endX = startX + offsetX + step;
     const endY = startY + offsetY + step;
 
@@ -140,6 +165,6 @@ async function simulateDrag(element, offsetX, offsetY) {
 
 {
     const canvas = document.querySelector('canvas.H1VXrf.JRr1M.DnOnV');
-""" + f"simulateDrag(canvas, {-dx}, {dy});" + """
+""" + f"simulateDrag(canvas, {int(-dx)}, {int(dy)});" + """
 }
 """

vision_chunk.py

@@ -28,7 +28,7 @@ class VisionChunk:
         if self.feature_method == "orb":
             self._detector = cv2.ORB_create(
                 nfeatures=10000,
-                scaleFactor=1.2,
+                scaleFactor=1.1,
                 nlevels=32,
                 edgeThreshold=31,
                 firstLevel=0,
@@ -72,14 +72,24 @@ class VisionChunk:
         return self._matcher
 
     def _preprocess(self, img_np: np.ndarray) -> np.ndarray:
-        """CLAHE предобработка для улучшения контраста"""
+        """Предобработка для улучшения сопоставления между снимками разного времени"""
         if len(img_np.shape) == 3:
             gray = cv2.cvtColor(img_np, cv2.COLOR_RGB2GRAY)
         else:
             gray = img_np
-            
+        
+        # Гауссовское размытие для подавления шума и мелких различий
+        blurred = cv2.GaussianBlur(gray, (5, 5), 1.0)
+        
+        # CLAHE для выравнивания контраста между снимками
         clahe = cv2.createCLAHE(clipLimit=3.0, tileGridSize=(8, 8))
-        return clahe.apply(gray)
+        enhanced = clahe.apply(blurred)
+        
+        # Опционально: нормализация гистограммы для устранения различий в освещении
+        normalized = cv2.normalize(enhanced, None, 0, 255, cv2.NORM_MINMAX)
+        
+        return normalized
+
 
     def compute_keypoints(self, force: bool = False) -> Tuple[list[cv2.KeyPoint], Optional[np.ndarray]]:
         if self.keypoints is not None and self.descriptors is not None and not force:
@@ -149,15 +159,6 @@ class VisionChunk:
         if len(good_matches) < 4:
             return None, None, None, None, None
 
-        # Центр изображений
-        img1_cv = self.to_cv2_gray()
-        img2_cv = other.to_cv2_gray()
-        h1, w1 = img1_cv.shape
-        h2, w2 = img2_cv.shape
-        cx1, cy1 = w1 // 2, h1 // 2
-        cx2, cy2 = w2 // 2, h2 // 2
-
-        # Отцентрированные координаты (x_rel, y_rel)
         src_pts = []
         dst_pts = []
         

yandex_map.py

@@ -6,6 +6,7 @@ from selenium.webdriver.common.by import By
 from selenium.webdriver.common.action_chains import ActionChains
 from time import sleep
 
+import constants
 import cv2
 import math
 import numpy as np
@@ -24,6 +25,7 @@ class YandexMap:
         self.initial_lat = initial_lat
         self.initial_lon = initial_lon
         self.initial_zoom = initial_zoom
+        self.pixel_ratio = constants.YANDEX_PIXEL_RATIO[self.initial_zoom]
 
         options = webdriver.ChromeOptions()
         # options.add_experimental_option("detach", True)
@@ -32,9 +34,8 @@ class YandexMap:
         self.driver.maximize_window()
         sleep(2)
 
-        action = ActionChains(self.driver)
-
         # Закрытие левой панели
+        action = ActionChains(self.driver)
         action.click(self.driver.find_element(By.CLASS_NAME, 'sidebar-toggle-button'))
         action.perform()
 
@@ -45,6 +46,19 @@ class YandexMap:
 
         sleep(0.2)
 
+    def open(self, lat, lon, zoom):
+        self.initial_lat = lat
+        self.initial_lon = lon
+        self.initial_zoom = zoom
+        self.pixel_ratio = constants.YANDEX_PIXEL_RATIO[self.initial_zoom]
+        self.driver.get(generateURL(lat, lon, zoom))
+        sleep(2)
+
+        # Закрытие левой панели
+        action = ActionChains(self.driver)
+        action.click(self.driver.find_element(By.CLASS_NAME, 'sidebar-toggle-button'))
+        action.perform()
+
     def save_photo(self, filename: str) -> bytes:
         return self.driver.save_screenshot(filename)
 
@@ -86,4 +100,8 @@ class YandexMap:
     def make_screenshot(self) -> Image.Image:
         png = self.driver.get_screenshot_as_png()
         im = Image.open(BytesIO(png))
-        return utility.cv2_to_pil(np.array(im)[:, :])
+        return utility.cv2_to_pil(np.array(im)[:, :])
+
+    def get_geolocation(self):
+        current_url = self.driver.current_url
+        return utility.parse_yandex_maps_url(current_url)