feat: add fps/landmark debugging

autopilot.py

@@ -3,6 +3,7 @@ from pathlib import Path
 import math
 import random
 
+import constants
 import cv2
 import numpy as np
 from PIL import Image
@@ -149,18 +150,30 @@ class AutoPilot(Pilot):
         }
 
     def get_position_by_chunk(self) -> Position | None:
+        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 constants.DEBUG_FPS:
+            print(f"[LANDMARK]: Closest chunk finding: {landmark_timer.loop() * 1000:.2f} ms")
+
         # Краевой случай: отсутствие чанков
         if current_chunk is None or landmark_chunk is None:
             return None
         
+        landmark_timer.start()
         src_pts, dst_pts, matches, kp1, kp2 = landmark_chunk.detect_and_match_keypoints(current_chunk)
 
+        if constants.DEBUG_FPS:
+            print(f"[LANDMARK]: detect and match keypoints: {landmark_timer.loop() * 1000:.2f} ms")
+
+        landmark_timer.stop()
+
         # Визуализация (если нужна)
         if src_pts is not None and dst_pts is not None and self.vis_manager:
             was_enabled = self.timer.enabled
@@ -174,6 +187,7 @@ class AutoPilot(Pilot):
             if was_enabled: 
                 self.timer.start()
 
+        landmark_timer.start()
         # Краевой случай: нет точек или недостаточно матчей
         if src_pts is None or dst_pts is None:
             return None
@@ -183,8 +197,12 @@ class AutoPilot(Pilot):
             return None
 
         # Оценка матрицы гомографии
+        landmark_timer.loop()
         landmark_transform, mask = estimate_transformation_matrix(src_pts, dst_pts)
         num_inliers = int(np.sum(mask))
+
+        if constants.DEBUG_FPS:
+            print(f"[LANDMARK]: matrix estimation: {landmark_timer.loop() * 1000:.2f} ms")
         
         # Краевой случай: матрица не найдена
         if landmark_transform is None or mask is None:
@@ -199,7 +217,11 @@ class AutoPilot(Pilot):
         
         # 2. Процент инлайеров от общего числа матчей
         inlier_ratio = num_inliers / num_matches
-        MIN_INLIER_RATIO = 0.25  # Минимум 25% инлайеров
+
+        if constants.DEBUG_LANDMARK:
+            print("[LANDMARK]: inlier_ratio=", inlier_ratio)
+
+        MIN_INLIER_RATIO = 0.6
         if inlier_ratio < MIN_INLIER_RATIO:
             return None
         
@@ -225,7 +247,11 @@ class AutoPilot(Pilot):
         reprojection_errors = np.sqrt(np.sum((transformed_pts - inlier_dst) ** 2, axis=2))
         mean_error = np.mean(reprojection_errors)
         
-        MAX_MEAN_REPROJECTION_ERROR = 1.0  # пиксели
+        MAX_MEAN_REPROJECTION_ERROR = 1.1  # пиксели
+
+        if constants.DEBUG_LANDMARK:
+            print("[LANDMARK]: Mean_error=", mean_error)
+
         if mean_error > MAX_MEAN_REPROJECTION_ERROR:
             return None
         
@@ -234,7 +260,11 @@ class AutoPilot(Pilot):
             return None
         
         # === ВСЕ ПРОВЕРКИ ПРОЙДЕНЫ ===
-        print("[INFO]: Landmark Chunk Correction Applied")
+        print("[LANDMARK]: Correction Applied")
+
+        if constants.DEBUG_FPS:
+            print(f"[LANDMARK]: time: {landmark_timer.get_elapsed() * 1000:.2f} ms")
+
         return landmark_chunk.pos.apply(landmark_transform)
 
 
@@ -250,8 +280,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")
 
@@ -279,17 +309,14 @@ class AutoPilot(Pilot):
 
                 self.timer.start()
 
-        chunk_timer = Timer()
-        chunk_timer.start()
-
         # Пытаемся найти ориентир на картинке:
         self.prev_chunk = current_chunk
-        pos_by_chunk = self.get_position_by_chunk()
-        if pos_by_chunk is not None:
-            self.pos = pos_by_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
 
-        chunk_timer.stop()
-        print(f"Chunk timer: {chunk_timer.get_elapsed() * 1000:.2f} ms")
         command = self.make_command()
         self.timer.reset()
         return command