diff --git a/autopilot.py b/autopilot.py index dee2b9f..0e7319d 100644 --- a/autopilot.py +++ b/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") diff --git a/utility.py b/utility.py index c9524e9..5a87117 100644 --- a/utility.py +++ b/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