diff --git a/include/detect/uv_led_detect_adaptive.cpp b/include/detect/uv_led_detect_adaptive.cpp index 098b0cf..397e9f8 100644 --- a/include/detect/uv_led_detect_adaptive.cpp +++ b/include/detect/uv_led_detect_adaptive.cpp @@ -25,8 +25,8 @@ struct PointComparator { namespace uvdar { -UVDARLedDetectAdaptive::UVDARLedDetectAdaptive(int neighborhoodSize, double point_similarity_threshold, std::string adaptive_method) : neighborhoodSize_(neighborhoodSize), -point_similarity_threshold_(point_similarity_threshold), adaptive_method_(adaptive_method){} +UVDARLedDetectAdaptive::UVDARLedDetectAdaptive(int neighborhoodSize, double point_similarity_threshold, std::string adaptive_method, bool adaptive_debug ) : neighborhoodSize_(neighborhoodSize), +point_similarity_threshold_(point_similarity_threshold), adaptive_method_(adaptive_method), adaptive_debug_(adaptive_debug){} UVDARLedDetectAdaptive::~UVDARLedDetectAdaptive() {} @@ -80,13 +80,17 @@ const std::vector& standardPoints) { rois.push_back(roi); } - //Print number of ROIs - std::cout << "[UVDARLedDetectAdaptive]: NUMBER OF ROIS: " << rois.size() << std::endl; + if (adaptive_debug_){ + //Print the size of the rois + std::cout << "[UVDARLedDetectAdaptive]: SIZE OF ROIS: " << rois.size() << std::endl; + } std::vector mergedROIs = mergeOverlappingROIs(rois,inputImage.size(), 0.05); // Overlap threshold of 50% - //Print number of merged ROIs - std::cout << "[UVDARLedDetectAdaptive]: NUMBER OF MERGED ROIS: " << mergedROIs.size() << std::endl; + if (adaptive_debug_){ + //Print number of merged ROIs + std::cout << "[UVDARLedDetectAdaptive]: NUMBER OF MERGED ROIS: " << mergedROIs.size() << std::endl; + } for (const auto& roi : mergedROIs) { std::vector roiDetectedPoints = applyAdaptiveThreshold(inputImage, roi); @@ -228,8 +232,11 @@ std::vector UVDARLedDetectAdaptive::applyAdaptiveThreshold(const cv:: // Create a mask to draw the filtered contours cv::Mat mask = cv::Mat::zeros(binaryRoi.size(), CV_8UC1); - //Get the number of contours + + if (adaptive_debug_){ + //Print the contours size std::cout << "[UVDARLedDetectAdaptive]: NUMBER OF CONTOURS: " << contours.size() << std::endl; + } if (contours.size() >= 4){ //Return empty roiDetectedPoints @@ -243,11 +250,16 @@ std::vector UVDARLedDetectAdaptive::applyAdaptiveThreshold(const cv:: double area = cv::contourArea(contour); // Filter based on area if (area < MAX_AREA) { - std::cout << "[UVDARLedDetectAdaptive]: IN AREA: " << area << std::endl; + + if (adaptive_debug_){ + std::cout << "[UVDARLedDetectAdaptive]: IN AREA: " << area << std::endl; + } // Draw the contour on the mask cv::drawContours(mask, std::vector>{contour}, -1, cv::Scalar(255), cv::FILLED); }else{ - std::cout << "[UVDARLedDetectAdaptive]: OUT OF AREA: " << area << std::endl; + if (adaptive_debug_){ + std::cout << "[UVDARLedDetectAdaptive]: OUT OF AREA: " << area << std::endl; + } } } @@ -256,10 +268,18 @@ std::vector UVDARLedDetectAdaptive::applyAdaptiveThreshold(const cv:: // Detect points within this ROI std::vector roiDetectedPoints = detectPointsFromRoi(binaryRoi, roi); - std::cout << "[UVDARLedDetectAdaptive]: ROI DETECTED POINTS: " << roiDetectedPoints.size() << std::endl; + + if (adaptive_debug_){ + //Print the number of detected points + std::cout << "[UVDARLedDetectAdaptive]: ROI NUMBER OF DETECTED POINTS: " << roiDetectedPoints.size() << std::endl; + } if (roiDetectedPoints.size() > 5){ - std::cout << "[UVDARLedDetectAdaptive]: NOISY ROI: " << roiDetectedPoints.size() << std::endl; + + if (adaptive_debug_){ + //Print the number of detected points + std::cout << "[UVDARLedDetectAdaptive]: NOISY ROI: " << roiDetectedPoints.size() << std::endl; + } //saveRoiImage(binaryRoi, point, roiIndex_++, thresholdValue, -1.0); numberDetectedPoints.push_back(roiDetectedPoints.size()); @@ -569,12 +589,6 @@ std::tuple UVDARLedDetectAdaptive::findOptimalThresholdUsingKL(cons //std::cout << "[UVDARLedDetectAdaptive]: COUNT OF ZEROS: " << countZeros << std::endl; //std::cout << "[UVDARLedDetectAdaptive]: COUNT OF ONES: " << countOnes << std::endl; - - - - if (roiImage.empty()) { - throw std::runtime_error("Input image is empty."); - } // Calculate the histogram of the ROI image int histSize = 256; diff --git a/include/detect/uv_led_detect_adaptive.h b/include/detect/uv_led_detect_adaptive.h index 4848ddc..c651d31 100644 --- a/include/detect/uv_led_detect_adaptive.h +++ b/include/detect/uv_led_detect_adaptive.h @@ -28,7 +28,7 @@ namespace uvdar { class UVDARLedDetectAdaptive{ public: - UVDARLedDetectAdaptive(int neighborhoodSize = 25, double point_similarity_threshold = 5.0, std::string adaptive_method = "Otsu"); + UVDARLedDetectAdaptive(int neighborhoodSize = 25, double point_similarity_threshold = 5.0, std::string adaptive_method = "Otsu", bool adaptive_debug = false); ~UVDARLedDetectAdaptive(); bool processImageAdaptive(const cv::Mat& inputImage, const std::vector& trackingPoints, std::vector& detectedPoints, const std::vector& standardPoints); @@ -79,6 +79,7 @@ class UVDARLedDetectAdaptive{ double point_similarity_threshold_; std::string adaptive_method_; + bool adaptive_debug_; int roiIndex_; int MAX_SIZE = 50; int MIN_SIZE = 5; diff --git a/launch/rosbag.launch b/launch/rosbag.launch index e905c3d..41d1fde 100644 --- a/launch/rosbag.launch +++ b/launch/rosbag.launch @@ -99,6 +99,8 @@ + + ["camera_left"] diff --git a/launch/sim_three_sided_combined_marlon.launch b/launch/sim_three_sided_combined_marlon.launch index ea14e0d..f7ab046 100644 --- a/launch/sim_three_sided_combined_marlon.launch +++ b/launch/sim_three_sided_combined_marlon.launch @@ -130,7 +130,9 @@ - + + + ["camera_left", "camera_right", "camera_back"] ["points_seen_left", "points_seen_right", "points_seen_back"] @@ -237,7 +239,7 @@ - > + > diff --git a/src/detector.cpp b/src/detector.cpp index c79eba3..8b7a408 100644 --- a/src/detector.cpp +++ b/src/detector.cpp @@ -54,6 +54,8 @@ class UVDARDetector : public nodelet::Nodelet{ param_loader.loadParam("threshold", _threshold_, 200); param_loader.loadParam("adaptive_threshold",_adaptive_threshold_,bool(false)); param_loader.loadParam("adaptive_method",_adaptive_method_,std::string("Otsu")); + param_loader.loadParam("adaptive_debug",_adaptive_debug_,bool(false)); + //Print adaptive threshold if(_adaptive_threshold_){ @@ -137,7 +139,8 @@ class UVDARDetector : public nodelet::Nodelet{ uvda_.push_back(std::make_unique( 20, 5.0, - _adaptive_method_ + _adaptive_method_, + _adaptive_debug_ )); if (!uvda_.back()){ ROS_ERROR("[UVDARDetector]: Failed to initialize ADAPTIVE-based marker detection!"); @@ -801,6 +804,7 @@ class UVDARDetector : public nodelet::Nodelet{ int _threshold_; bool _adaptive_threshold_; std::string _adaptive_method_; + bool _adaptive_debug_; double _initial_delay_ = 5.0;