feat: add GAN

autopilot.py

@@ -8,6 +8,7 @@ import cv2
 import numpy as np
 from PIL import Image
 
+import sian_similarity
 from timer import Timer
 
 from vision_chunk import VisionChunk
@@ -58,8 +59,16 @@ class AutoPilot(Pilot):
     # Положение на основе ориентира
     reserved_pos: Position | None
     proccessing_time: float
+    use_sian_similarity: bool
 
-    def __init__(self, points = [], chunks = [], viz_manager=None, pixel_ratio: float = 1.):
+    def __init__(
+        self,
+        points = [],
+        chunks = [],
+        viz_manager=None,
+        pixel_ratio: float = 1.,
+        use_sian_similarity: bool = False,
+    ):
         self.prev_chunk = None
         self.pos = Position(0, 0, 1, 0, 0, 0)
         self.chunks = chunks
@@ -67,6 +76,7 @@ class AutoPilot(Pilot):
         self.vis_manager = viz_manager  # Менеджер визуализации
         self.reserved_pos = None
         self.pixel_ratio = pixel_ratio
+        self.use_sian_similarity = use_sian_similarity
 
         # Пороговые значения качества сопоставления/гомографии
         self.min_inliers: int = 12
@@ -153,17 +163,33 @@ class AutoPilot(Pilot):
         landmark_timer = Timer()
         landmark_timer.start()
 
-        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[closest_chunk_idx]
+        if current_chunk is None or not self.chunks:
+            return None
+
+        if self.use_sian_similarity:
+            similarity_scores = sian_similarity.get_similarity_scores(current_chunk, self.chunks)
+            best_chunk_idx = int(np.argmax(similarity_scores))
+            best_similarity_score = similarity_scores[best_chunk_idx]
+
+            print(f"[LANDMARK]: best similarity={best_similarity_score:.4f}, idx={best_chunk_idx}")
+
+            if best_similarity_score < sian_similarity.get_threshold():
+                print("[LANDMARK]: not similar")
+                return None
+
+            print("[LANDMARK]: similar")
+            landmark_chunk = self.chunks[best_chunk_idx]
+        else:
+            cur_pos = np.array([self.pos.x, self.pos.y])
+            closest_chunk_idx = ((self.chunk_points - cur_pos) ** 2).sum(1).argmin()
+            landmark_chunk = self.chunks[closest_chunk_idx]
         
         if constants.DEBUG_FPS:
-            print(f"[LANDMARK]: Closest chunk finding: {landmark_timer.loop() * 1000:.2f} ms")
+            print(f"[LANDMARK]: Landmark chunk finding: {landmark_timer.loop() * 1000:.2f} ms")
 
         # Краевой случай: отсутствие чанков
-        if current_chunk is None or landmark_chunk is None:
+        if landmark_chunk is None:
             return None
         
         landmark_timer.start()
@@ -312,10 +338,10 @@ class AutoPilot(Pilot):
         # Пытаемся найти ориентир на картинке:
         self.prev_chunk = current_chunk
         # Для улучшения среднего FPS
-        # if self.frame_count % 5 == 0:
-        #     pos_by_chunk = self.get_position_by_chunk()
-        #     if pos_by_chunk is not None:
-        #         self.pos = pos_by_chunk
+        if self.frame_count % 5 == 0:
+            pos_by_chunk = self.get_position_by_chunk()
+            if pos_by_chunk is not None:
+                self.pos = pos_by_chunk
 
         command = self.make_command()
         self.timer.reset()