New automation scripts, changes into the adaptive, adding mutexes and…

… making it better to start tuning the parameters
MarlonRiv · Nov 3, 2024 · cdba7a1 · cdba7a1
1 parent 2369e6c
commit cdba7a1
Show file tree

Hide file tree

Showing 17 changed files with 368 additions and 141 deletions.
diff --git a/include/detect/uv_led_detect_adaptive.cpp b/include/detect/uv_led_detect_adaptive.cpp
@@ -27,11 +27,15 @@ struct PointComparator
 namespace uvdar
 {
 
-  UVDARLedDetectAdaptive::UVDARLedDetectAdaptive(int neighborhoodSize, double point_similarity_threshold, std::string adaptive_method, bool adaptive_debug)
+  UVDARLedDetectAdaptive::UVDARLedDetectAdaptive(int neighborhoodSize, double point_similarity_threshold, std::string adaptive_method, bool adaptive_debug, int contours_size_limit, int contour_max_size_limit, int roi_detected_points_limit)
       : neighborhoodSize_(neighborhoodSize),
         point_similarity_threshold_(point_similarity_threshold),
         adaptive_method_(adaptive_method),
-        adaptive_debug_(adaptive_debug)
+        adaptive_debug_(adaptive_debug),
+        contours_size_limit_(contours_size_limit),
+        contour_max_size_limit_(contour_max_size_limit),
+        roi_detected_points_limit_(roi_detected_points_limit)
+
   {
   }
 
@@ -57,14 +61,12 @@ namespace uvdar
      * True if the process is successful, False otherwise
      */
 
-    {
-      std::scoped_lock lock(mutex_viz_rois_);
+    std::scoped_lock lock(mutex_viz_rois_, mutex_rois_data_);
 
-      // Reset detected points
-      detectedPoints.clear();
-      lastProcessedBinaryROIs_.clear();
-      lastProcessedROIs_.clear();
-    }
+    // Reset detected points
+    detectedPoints.clear();
+    lastProcessedBinaryROIs_.clear();
+    lastProcessedROIs_.clear();
 
     // Reset the number of ROIs
     numRois_ = 0;
@@ -91,11 +93,13 @@ namespace uvdar
     if (adaptive_debug_)
     {
       // Print the size of the rois
-      std::cout << "[UVDARLedDetectAdaptive]: SIZE OF ROIS: " << rois.size() << std::endl;
+      std::cout << "[UVDARLedDetectAdaptive]: NUMBER OF ROIS: " << rois.size() << std::endl;
     }
 
     const auto mergedROIs = mergeOverlappingROIs(rois, inputImage.size(), 0.05);  // Overlap threshold of 5%
 
+    numRois_ = mergedROIs.size();
+
     if (adaptive_debug_)
     {
       // Print number of merged ROIs
@@ -191,22 +195,22 @@ namespace uvdar
     }
 
     // TODO find proper value for noisy ROI
-    if (contours.size() >= 6)
+    if (static_cast<int>(contours.size()) >= contours_size_limit_)
     {
       // Return empty roiDetectedPoints
       std::vector<cv::Point> empty_roiDetectedPoints = {};
       return empty_roiDetectedPoints;
     }
 
     // TODO find proper value for MAX_AREA
-    int MAX_AREA = 50;
+    /* int MAX_AREA = 15; */
 
     for (const auto& contour : contours)
     {
       // Calculate the area of the contour
       double area = cv::contourArea(contour);
       // Filter based on area
-      if (area < MAX_AREA)
+      if (area < contour_max_size_limit_)
       {
         if (adaptive_debug_)
         {
@@ -236,55 +240,52 @@ namespace uvdar
     // 5
 
 
-    std::scoped_lock lock(mutex_viz_rois_);
+    if (static_cast<int>(roiDetectedPoints.size()) > roi_detected_points_limit_)
     {
-      if (roiDetectedPoints.size() > 4)
-      {
 
-        if (adaptive_debug_)
-        {
-          // Print the number of detected points
-          std::cout << "[UVDARLedDetectAdaptive]: NOISY ROI: " << roiDetectedPoints.size() << std::endl;
-        }
-        numberDetectedPoints_.push_back(roiDetectedPoints.size());
-        thresholdValues_.push_back(thresholdValue_);
-        if (adaptive_method_ == "Otsu" || adaptive_method_ == "otsu")
-        {
-          klDivergences_.push_back(0.0);
-        } else
-        {
-          klDivergences_.push_back(minKLDivergence_);
-        }
-        validRois_.push_back(0);
-
-        // Return empty roiDetectedPoints
-        std::vector<cv::Point> empty_roiDetectedPoints = {};
-        return empty_roiDetectedPoints;
-        // Clear the lastProcessedROIs_ and lastProcessedBinaryROIs_ vectors
-        lastProcessedROIs_.clear();
-        lastProcessedBinaryROIs_.clear();
+      if (adaptive_debug_)
+      {
+        // Print the number of detected points
+        std::cout << "[UVDARLedDetectAdaptive]: NOISY ROI: " << roiDetectedPoints.size() << std::endl;
+      }
+      numberDetectedPoints_.push_back(roiDetectedPoints.size());
+      thresholdValues_.push_back(thresholdValue_);
+      if (adaptive_method_ == "Otsu" || adaptive_method_ == "otsu")
+      {
+        klDivergences_.push_back(0.0);
       } else
       {
+        klDivergences_.push_back(minKLDivergence_);
+      }
+      validRois_.push_back(0);
 
-        // This is the reason it seems the visualization is blinking, getting some noisy rois in between and not being used
-        lastProcessedROIs_.push_back(roi);  // Store the ROI for visualization
-        // Store the binary ROI (For debugging/visualization)
-        lastProcessedBinaryROIs_.push_back(binaryRoi);
+      // Return empty roiDetectedPoints
+      std::vector<cv::Point> empty_roiDetectedPoints = {};
+      return empty_roiDetectedPoints;
+      // Clear the lastProcessedROIs_ and lastProcessedBinaryROIs_ vectors
+      lastProcessedROIs_.clear();
+      lastProcessedBinaryROIs_.clear();
+    } else
+    {
 
+      // This is the reason it seems the visualization is blinking, getting some noisy rois in between and not being used
+      lastProcessedROIs_.push_back(roi);  // Store the ROI for visualization
+      // Store the binary ROI (For debugging/visualization)
+      lastProcessedBinaryROIs_.push_back(binaryRoi);
 
-        numberDetectedPoints_.push_back(roiDetectedPoints.size());
-        thresholdValues_.push_back(thresholdValue_);
-        if (adaptive_method_ == "Otsu" || adaptive_method_ == "otsu")
-        {
-          klDivergences_.push_back(0.0);
-        } else
-        {
-          klDivergences_.push_back(minKLDivergence_);
-        }
-        validRois_.push_back(1);
 
-        return roiDetectedPoints;
+      numberDetectedPoints_.push_back(roiDetectedPoints.size());
+      thresholdValues_.push_back(thresholdValue_);
+      if (adaptive_method_ == "Otsu" || adaptive_method_ == "otsu")
+      {
+        klDivergences_.push_back(0.0);
+      } else
+      {
+        klDivergences_.push_back(minKLDivergence_);
       }
+      validRois_.push_back(1);
+
+      return roiDetectedPoints;
     }
   }
   //}
@@ -696,7 +697,7 @@ namespace uvdar
     {
       return;
     }
-   
+
 
     std::scoped_lock lock(mutex_viz_rois_);
     // Overlay binary ROIs
@@ -811,6 +812,7 @@ namespace uvdar
      * A tuple containing the number of detected points, the threshold value, the KL divergence, and the validity of the ROI
      */
 
+    std::scoped_lock lock(mutex_rois_data_);
     ROIData adaptiveData;
     adaptiveData.numRois = numRois_;
     adaptiveData.numberDetectedPoints = numberDetectedPoints_;

diff --git a/include/detect/uv_led_detect_adaptive.h b/include/detect/uv_led_detect_adaptive.h
@@ -33,7 +33,7 @@ namespace uvdar
   {
   public:
     UVDARLedDetectAdaptive(int neighborhoodSize = 25, double point_similarity_threshold = 5.0, std::string adaptive_method = "Otsu",
-                           bool adaptive_debug = false);
+                           bool adaptive_debug = false, int contours_size_limit = 4, int contour_max_size_limit = 15, int roi_detected_points_limit = 5);
     ~UVDARLedDetectAdaptive();
 
     bool processImageAdaptive(const cv::Mat& inputImage, const std::vector<cv::Point>& trackingPoints, std::vector<cv::Point>& detectedPoints,
@@ -88,13 +88,17 @@ namespace uvdar
     double point_similarity_threshold_;
     std::string adaptive_method_;
     bool adaptive_debug_;
+    int contours_size_limit_; 
+    int contour_max_size_limit_; 
+    int roi_detected_points_limit_; 
     int roiIndex_;
     int MAX_SIZE = 50;
     int MIN_SIZE = 5;
     std::vector<cv::Mat> lastProcessedBinaryROIs_;
     std::vector<cv::Rect> lastProcessedROIs_;     // For storing ROI positions
     std::vector<cv::Point> allRoiDetectedPoints;  // For storing ROI positions
     std::mutex mutex_viz_rois_;
+    std::mutex mutex_rois_data_;
 
     int numRois_;
     std::vector<int> numberDetectedPoints_;

diff --git a/launch/rosbag.launch b/launch/rosbag.launch
@@ -14,6 +14,11 @@
 
    <!-- Point detector settings: -->
   <arg name="threshold" default="100"/>
+
+  <!--Adaptive thresholding values-->
+  <arg name="contour_size_limit" default="4"/>
+  <arg name="contour_max_size_limit" default="15"/>
+  <arg name="roi_detected_points_limit" default="5"/>
 
   <!-- Blink processor settings: -->
   <arg name="blink_process_rate" default="10"/>
@@ -107,8 +112,8 @@
 <!-- OR -->
   <group ns="$(arg uav_name)">
 
-     <!-- <node pkg="nodelet" type="nodelet" name="$(arg uav_name)_uvdar_nodelet_manager" args="manager" output="screen" > -->
-   <node pkg="nodelet" type="nodelet" name="$(arg uav_name)_uvdar_nodelet_manager" args="manager" output="screen" launch-prefix="debug_roslaunch">
+     <node pkg="nodelet" type="nodelet" name="$(arg uav_name)_uvdar_nodelet_manager" args="manager" output="screen" >
+   <!-- <node pkg="nodelet" type="nodelet" name="$(arg uav_name)_uvdar_nodelet_manager" args="manager" output="screen" launch-prefix="debug_roslaunch"> -->
       <param name="num_worker_threads" value="8" />
    </node>
 
@@ -129,14 +134,16 @@
       <!-- <param name="gui"             type="bool" value="true"/> -->
       <param name="gui"             type="bool" value="false"/>
       <!-- <param name="publish_visualization"             type="bool" value="true"/> -->
-      <param name="publish_visualization"             type="bool" value="true"/>
+      <param name="publish_visualization"             type="bool" value="false"/>
       <param name="publish"         type="bool" value="$(arg publish)"/>
       <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="otsu"/>
       <param name="adaptive_debug" type="bool" value="false"/>
-
+      <param name="contour_size_limit" type="int" value="$(arg contour_size_limit)"/>
+      <param name="contour_max_size_limit" type="int" value="$(arg contour_max_size_limit)"/>
+      <param name="roi_detected_points_limit" type="int" value="$(arg roi_detected_points_limit)"/>
 
       <!-- <rosparam param="camera_topics"> ["uav6/uvdar_bluefox/left/image_raw"] </rosparam> -->
       <rosparam param="camera_topics"> ["camera_left"] </rosparam>

diff --git a/scripts/.tmuxinator.yml b/scripts/.tmuxinator.yml
@@ -36,11 +36,12 @@ windows:
         # - waitForRos; sleep 3; rosbag play --clock ~/experiments/20_04/two_tx/two_tx/rx/occlusion_2/rx.bag
         # - waitForRos; rosbag play --clock ~/experiments/20_04/tumult/tumult/rx/rx.bag
         - waitForRos; sleep 3; rosbag play --clock ~/bag_files/datasets_08_2024/uav36_rosbags_rx/marlon/69_2024_08_20_14_12_22variant1/_2024-08-20-14-14-09.bag
-  - rqt:
+  - blinkers-seen:
       layout: even-vertical
       panes:
         # - waitForRos; sleep 3; rqt_image_view
         - waitForRos; sleep 3; rostopic echo /uav36/uvdar/blinkers_seen_left 
+        - waitForRos; sleep 3; rostopic echo /uav36/uvdar/adaptive_logging_left 
 
  # - waitForCompile; cd ~/uvdar_rosbag/tracking/uav7/22_2021_04_27_09_11_19_L12/ ; rosbag play --clock -s 30 _2021-04-27-09-11-56.bag
   # - trajectory:

diff --git a/scripts/.~lock.threshold_analysis_summary_adaptive.csv# b/scripts/.~lock.threshold_analysis_summary_adaptive.csv#
@@ -0,0 +1 @@
+,rivermar,rivermar,03.11.2024 18:39,file:///home/rivermar/.config/libreoffice/4;
Original file line number	Diff line number	Diff line change
		@@ -0,0 +1 @@
		,rivermar,rivermar,03.11.2024 18:39,file:///home/rivermar/.config/libreoffice/4;