autopilot.py

@@ -154,19 +154,88 @@ class AutoPilot(Pilot):
 
         current_chunk = self.prev_chunk
         landmark_chunk = self.chunks[closest_chunk_idx]
+        
+        # Краевой случай: отсутствие чанков
+        if current_chunk is None or landmark_chunk is None:
+            return None
+        
         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:
             was_enabled = self.timer.enabled
-            if was_enabled: self.timer.stop()
-            self.vis_manager.update_chunk_matches(landmark_chunk.to_numpy(), current_chunk.to_numpy(), kp1, kp2, matches)
-            if was_enabled: self.timer.start()
+            if was_enabled: 
+                self.timer.stop()
+            self.vis_manager.update_chunk_matches(
+                landmark_chunk.to_numpy(), 
+                current_chunk.to_numpy(), 
+                kp1, kp2, matches
+            )
+            if was_enabled: 
+                self.timer.start()
 
-        if src_pts is not None and dst_pts is not None:
-            landmark_transform = estimate_transformation_matrix(src_pts, dst_pts)
-            if landmark_transform is not None:
-                return landmark_chunk.pos.apply(landmark_transform)
-        return None
+        # Краевой случай: нет точек или недостаточно матчей
+        if src_pts is None or dst_pts is None:
+            return None
+        
+        num_matches = len(src_pts)
+        if num_matches < 20:
+            return None
+
+        # Оценка матрицы гомографии
+        landmark_transform, mask = estimate_transformation_matrix(src_pts, dst_pts)
+        num_inliers = int(np.sum(mask))
+        
+        # Краевой случай: матрица не найдена
+        if landmark_transform is None or mask is None:
+            return None
+
+        # === КРИТЕРИИ ПРИНЯТИЯ РЕШЕНИЯ ===
+        
+        # 1. Минимальное количество инлайеров (абсолютное)
+        MIN_INLIERS_ABSOLUTE = 6
+        if num_inliers < MIN_INLIERS_ABSOLUTE:
+            return None
+        
+        # 2. Процент инлайеров от общего числа матчей
+        inlier_ratio = num_inliers / num_matches
+        MIN_INLIER_RATIO = 0.25  # Минимум 25% инлайеров
+        if inlier_ratio < MIN_INLIER_RATIO:
+            return None
+        
+        # 3. Проверка качества гомографии (детерминант для выявления вырожденных случаев)
+        det = np.linalg.det(landmark_transform[:2, :2])
+        # Детерминант должен быть близок к 1 (без сильного масштабирования)
+        if abs(det) < 0.1 or abs(det) > 10.0:
+            return None
+        
+        # 4. Проверка на валидность перспективного преобразования
+        # Элементы третьей строки не должны быть слишком большими
+        if abs(landmark_transform[2, 0]) > 0.01 or abs(landmark_transform[2, 1]) > 0.01:
+            return None
+        
+        # 5. Дополнительная проверка: средняя ошибка репроекции для инлайеров
+        inlier_src = src_pts[mask.ravel() == 1]
+        inlier_dst = dst_pts[mask.ravel() == 1]
+        
+        # Преобразуем точки через найденную гомографию
+        transformed_pts = cv2.perspectiveTransform(inlier_src, landmark_transform)
+        
+        # Вычисляем ошибку репроекции
+        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  # пиксели
+        if mean_error > MAX_MEAN_REPROJECTION_ERROR:
+            return None
+        
+        # 6. Проверка стабильности: если слишком много хороших совпадений, но мало инлайеров - подозрительно
+        if num_matches > 50 and inlier_ratio < 0.15:
+            return None
+        
+        # === ВСЕ ПРОВЕРКИ ПРОЙДЕНЫ ===
+        print("[INFO]: Landmark Chunk Correction Applied")
+        return landmark_chunk.pos.apply(landmark_transform)
 
 
     def handle(self, current_chunk: VisionChunk) -> PilotCommand:
@@ -192,7 +261,7 @@ class AutoPilot(Pilot):
 
             matrix_estimation_timer = Timer()
             matrix_estimation_timer.start()
-            homography_matrix = estimate_transformation_matrix(src_pts, dst_pts)
+            homography_matrix, _ = estimate_transformation_matrix(src_pts, dst_pts)
             matrix_estimation_timer.stop()
             print(f"Transformation matrix updating: {matrix_estimation_timer.get_elapsed() * 1000:.2f} ms")
 

utility.py

@@ -23,11 +23,10 @@ def get_normals(H: np.ndarray, R: np.ndarray, T: np.ndarray) -> np.ndarray:
     n = cv2.decomposeHomographyMat(H, constants.K, R, T)
     return n
 
-def estimate_transformation_matrix(src_pts: np.ndarray, dst_pts: np.ndarray) -> np.ndarray | None:
+def estimate_transformation_matrix(src_pts: np.ndarray, dst_pts: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
     """Оценивает матрицу трансформации на основе сопоставленных точек"""
     # Используем RANSAC для оценки матрицы гомографии
-    H, _ = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 3.0, maxIters=1000)
-    return H
+    return cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 3.0, maxIters=1000)
 
 def calc_camera_matrix(w: float, h: float):
     f = constants._K_FOCUS_DISTANCE