diff --git a/lidar_detector/src/lib/keypoint_detection.cpp b/lidar_detector/src/lib/keypoint_detection.cpp index fa79dd1..6aeedf5 100644 --- a/lidar_detector/src/lib/keypoint_detection.cpp +++ b/lidar_detector/src/lib/keypoint_detection.cpp @@ -395,6 +395,12 @@ pcl::PointCloud keypointDetection(pcl::PointCloud pattern = processCircles(circles_cloud, cloud_without_ground_floor, config.circle_detection); if (config.visualize) { visualize(cloud_calibration_board, pattern, edges_cloud); } + if (pattern.size() < 4) { + std::cerr << "Failed to find all circles, found only " << pattern.size() << std::endl; + pattern.clear(); + return pattern; + } + // Refine circle centers using calibration board geometry if (config.refinement.refine) { // Refinement using kabsch