feat: dynamic map; refactor code and fix bugs

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