Added debug param to adaptive

MarlonRiv · Apr 18, 2024 · a8e8eb1 · a8e8eb1
1 parent 20cdd69
commit a8e8eb1
Show file tree

Hide file tree

Showing 5 changed files with 44 additions and 21 deletions.
diff --git 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<cv::Point>& 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<cv::Rect> 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<cv::Point> roiDetectedPoints = applyAdaptiveThreshold(inputImage, roi);
@@ -228,8 +232,11 @@ std::vector<cv::Point> 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<cv::Point> 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<std::vector<cv::Point>>{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<cv::Point> UVDARLedDetectAdaptive::applyAdaptiveThreshold(const cv::
 
     // Detect points within this ROI
     std::vector<cv::Point> 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<int, double> 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
@@ -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<cv::Point>& trackingPoints, std::vector<cv::Point>& detectedPoints, const std::vector<cv::Point>& 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
@@ -99,6 +99,8 @@
       <param name="threshold" type="int" value="$(arg threshold)"/>
       <param name="adaptive_threshold" type="bool" value="true"/>
       <param name="adaptive_method" type="string" value="otsu"/>
+      <param name="adaptive_debug" type="bool" value="true"/>
+
 
       <!-- <rosparam param="camera_topics"> ["uav6/uvdar_bluefox/left/image_raw"] </rosparam> -->
       <rosparam param="camera_topics"> ["camera_left"] </rosparam>

diff --git a/launch/sim_three_sided_combined_marlon.launch b/launch/sim_three_sided_combined_marlon.launch
@@ -130,7 +130,9 @@
       <param name="justReport" type="bool" value="true"/>
       <param name="threshold" type="int" value="$(arg threshold)"/>
       <param name="adaptive_threshold" type="bool" value="true"/>
-      <param name="adaptive_method" type="string" value="kl"/>
+      <param name="adaptive_method" type="string" value="otsu"/>
+      <param name="adaptive_debug" type="bool" value="true"/>
+
 
       <rosparam param="camera_topics"> ["camera_left", "camera_right", "camera_back"] </rosparam>
       <rosparam param="points_seen_topics"> ["points_seen_left", "points_seen_right", "points_seen_back"] </rosparam>
@@ -237,7 +239,7 @@
 
 
       <!-- <node name="uvdar_pose_calculator_node" pkg="uvdar_core" type="uvdar_pose_calculator_node" output="screen" launch-prefix="debug_roslaunch"> --> 
-    <node name="uvdar_pose_calculator_node" pkg="uvdar_core" type="uvdar_pose_calculator_node" output="screen" launch-prefix="bash -c 'sleep 20; $0 $@'">> 
+    <node name="uvdar_pose_calculator_node" pkg="uvdar_core" type="uvdar_pose_calculator_node" output="screen" launch-prefix="bash -c 'sleep 20; $0 $@'">>
         <!-- <node name="uvdar_pose_calculator_node" pkg="uvdar_core" type="uvdar_pose_calculator_node" launch-prefix="valgrind -/-tool=callgrind -/-callgrind-out-file=/home/viktor/callgrind.out -/-instr-atstart=no -/-collect-atstart=yes"> --> 
             <!-- <node name="uvdar_pose_calculator_node" pkg="uvdar_core" type="uvdar_pose_calculator_node" launch-prefix="valgrind -/-tool=callgrind -/-callgrind-out-file=/home/viktor/callgrind.out"> --> 
       <param name="uav_name" type = "string" value="$(arg uav_name)"/>

diff --git 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<UVDARLedDetectAdaptive>(
               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;