From 1f4be10da5e082d06f003f330c5dc970ba04a548 Mon Sep 17 00:00:00 2001 From: amudda Date: Tue, 11 Aug 2026 13:13:18 -0400 Subject: [PATCH] added solvepnp error handling as well as checks to prune degenerate corners --- vision/opencv_processing.py | 64 +++++++++++++++++++++++++++++++++++-- 1 file changed, 61 insertions(+), 3 deletions(-) diff --git a/vision/opencv_processing.py b/vision/opencv_processing.py index ec2c05d..0342a87 100644 --- a/vision/opencv_processing.py +++ b/vision/opencv_processing.py @@ -270,11 +270,69 @@ def cv_processing_loop(self): undistorted_corners = cv2.fisheye.undistortPoints( full_img_corners.reshape(-1, 1, 2), self.K, self.D, P=self.K ) - - success, rvec, tvec = cv2.solvePnP( - self.GATE_3D_CORNERS, undistorted_corners, self.K, np.zeros(4), flags=cv2.SOLVEPNP_SQPNP + + pts2d = undistorted_corners.reshape(-1,2) + + ## validate corner geometry before calling solvePnP + + # Reject NaN / inf + if not np.isfinite(pts2d).all(): + print("[WARN] Invalid gate corners:", pts2d) + continue + + # Reject udplicate / nearly-duplicate corners + min_corner_dist = min( + np.linalg.norm(pts2d[i] - pts2d[j]) + for i in range(4) + for j in range(i + 1, 4) + ) + + if min_corner_dist < 3.0: + print("[WARN] Gate corners collapsed:", pts2d) + continue + + + # Reject tiny / degenerate quadrilaterals + gate_area = abs( + cv2.contourArea( + pts2d.astype(np.float32) + ) + ) + + if gate_area < 25.0: + print("[WARN] Gate area too small:", gate_area) + continue + + # Exactly reproduce the coordinate-space SQPnP cares about. + normalized = cv2.undistortPoints( + pts2d.reshape(-1, 1, 2), + self.K, + None + ).reshape(-1, 2) + + point_variance = ( + np.var(normalized[:, 0]) + + np.var(normalized[:, 1]) ) + if point_variance < 1e-5: + print( + "[WARN] SQPnP point variance too small:", + point_variance, + pts2d + ) + continue + + try: + success, rvec, tvec = cv2.solvePnP( + self.GATE_3D_CORNERS, undistorted_corners, self.K, np.zeros(4), flags=cv2.SOLVEPNP_SQPNP + ) + + except cv2.error as e: + print("[WARN] solvePnP failed:", e) + continue + + if success: rvec, tvec = cv2.solvePnPRefineLM( self.GATE_3D_CORNERS, undistorted_corners, self.K, np.zeros(4), rvec, tvec