Update

config/selected.txt

@@ -1 +1 @@
-blinking_sequences/manchesterPropeties.txt
+blinking_sequences/L12.txt

include/detect/uv_led_detect_adaptive.cpp

@@ -3,6 +3,7 @@
 #include <set>
 #include <filesystem>  // C++17 and above
 #include <opencv2/imgcodecs.hpp>
+
 #include <cmath>
 #include <numeric>
 #include <iomanip> // Include for std::fixed and std::setprecision
@@ -169,6 +170,10 @@ std::vector<cv::Point> UVDARLedDetectAdaptive::applyAdaptiveThreshold(const cv::
     int MAX_AREA = 50;
 
 
+    // Compute and plot histograms
+    cv::Mat histOriginal = plotHistogram(grayImage);
+
+
    /*  cv::Mat grayImage;
     if (image.channels() == 3) {
         cv::cvtColor(image, grayImage, cv::COLOR_BGR2GRAY);
@@ -203,18 +208,34 @@ std::vector<cv::Point> UVDARLedDetectAdaptive::applyAdaptiveThreshold(const cv::
     cv::Mat enhancedImage;
     cv::addWeighted(grayImage, 0.25, unsharpMask, 1.75, 0, enhancedImage);
 
-    //saveRoiImage(enhancedImage, point, roiIndex_++, 0, 0.0);
+
+    cv::Mat histEnhanced = plotHistogram(enhancedImage);
+
+    // Specify the output directory
+    std::string outputDir = "/home/rivermar/Desktop/MRS_Master_Project/roi_images";
+
+    // Ensure the output directory exists
+    fs::create_directories(outputDir);
+
+
+
+    //saveRoiImage(histOriginal, 0, roiIndex_++, 0, 0.0);
+    //saveRoiImage(histEnhanced, 0, roiIndex_++, 0, 0.0);
 
     cv::Mat binaryRoi;
     //cv::adaptiveThreshold(roiImage, binaryRoi, 255, cv::ADAPTIVE_THRESH_GAUSSIAN_C, cv::THRESH_BINARY, 11, 2);
 
     //Print the adaptive method
     //std::cout << "[UVDARLedDetectAdaptive]: ADAPTIVE METHOD: " << adaptive_method_ << std::endl;
-
+    cv::Mat binaryRoiOriginal;
     if( adaptive_method_ == "Otsu" || adaptive_method_ == "otsu"){
         //Apply Otsu's thresholding with the enhanced ROI
         int thresholdValue= cv::threshold(enhancedImage, binaryRoi, 0, 255, cv::THRESH_BINARY | cv::THRESH_OTSU); // Apply Otsu's thresholding
         thresholdValue_ = thresholdValue;
+
+        //Apply Otsu's thresholding with the original ROI
+
+        int thresholdValueOriginal = cv::threshold(grayImage, binaryRoiOriginal, 0, 255, cv::THRESH_BINARY | cv::THRESH_OTSU); // Apply Otsu's thresholding
     }
     else{
         //std::cout << "[UVDARLedDetectAdaptive]: APPLYING KL DIVERGENCE" << std::endl;
@@ -225,6 +246,25 @@ std::vector<cv::Point> UVDARLedDetectAdaptive::applyAdaptiveThreshold(const cv::
         minKLDivergence_ = minKLDivergence;
         cv::threshold(enhancedImage,binaryRoi, thresholdValue_, 255, cv::THRESH_BINARY);
     }
+
+
+    //Save the original and enhanced images
+    cv::imwrite(outputDir + "/im_" + std::to_string(index) + ".png", grayImage);
+    index++;
+    // Save the histograms adding an index for each histogram
+    cv::imwrite(outputDir + "/im_" + std::to_string(index) + ".png", histOriginal);
+    index++;
+    //Save the binary ROI using the original image
+    cv::imwrite(outputDir + "/im_" + std::to_string(index) + ".png", binaryRoiOriginal);
+    index++;
+    //Save the enhanced image
+    cv::imwrite(outputDir + "/im_" + std::to_string(index) + ".png", enhancedImage);
+    index++;
+    cv::imwrite(outputDir + "/im_" + std::to_string(index) + ".png", histEnhanced);
+    index++;
+    //Save the binary ROI
+    cv::imwrite(outputDir + "/im_" + std::to_string(index) + ".png", binaryRoi);
+    index++;
 
     // Find contours in the binary ROI
     std::vector<std::vector<cv::Point>> contours;
@@ -773,6 +813,46 @@ void UVDARLedDetectAdaptive::generateVisualizationAdaptive(const cv::Mat& inputI
 }
 //}
 
+
+cv::Mat UVDARLedDetectAdaptive::plotHistogram(const cv::Mat& image) {
+    // Compute the histogram
+    int histSize = 256;
+    float range[] = { 0, 256 };
+    const float* histRange = { range };
+    cv::Mat hist;
+    cv::calcHist(&image, 1, 0, cv::Mat(), hist, 1, &histSize, &histRange, true, false);
+
+
+
+     // Apply logarithmic scaling to the histogram values
+    hist += 1; // Avoid log(0)
+    cv::log(hist, hist);
+
+    // Create an image to display the histogram
+    int hist_w = 512; int hist_h = 400;
+    int bin_w = std::round((double)hist_w / histSize);
+    cv::Mat histImage(hist_h, hist_w, CV_8UC1, cv::Scalar(0));
+
+
+    // Find the maximum value of the histogram for scaling
+    double maxVal;
+    cv::minMaxLoc(hist, 0, &maxVal);
+
+    // Draw the histogram
+    for (int i = 0; i < histSize; i++) {
+        int binVal = std::round(hist.at<float>(i) * hist_h / maxVal);
+        if (binVal > hist_h) binVal = hist_h; // Cap the bin value to fit within the image height
+        cv::line(histImage,
+                 cv::Point(bin_w * i, hist_h),
+                 cv::Point(bin_w * i, hist_h - binVal),
+                 cv::Scalar(255), 2, 8, 0);
+    }
+
+    return histImage;
+
+
+}
+
 /* saveRoiImage //{ */
 
 

include/detect/uv_led_detect_adaptive.h

@@ -4,6 +4,7 @@
 #include <opencv2/imgcodecs.hpp>
 #include <opencv2/core/core.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
+
 #include <vector>
 #include <filesystem> 
 #include <cmath>
@@ -69,8 +70,9 @@ class UVDARLedDetectAdaptive{
     double calculateKLDivergence2(const cv::Mat& hist, const std::vector<double>& Q, int start, int end);
 
     std::tuple<int, double> findOptimalThresholdUsingKL(const cv::Mat& roiImage);
-    std::tuple<int, double> findOptimalThresholdUsingEntropy(const cv::Mat& roiImage); 
+    std::tuple<int, double> findOptimalThresholdUsingEntropy(const cv::Mat& roiImage);
 
+    cv::Mat plotHistogram(const cv::Mat& image);
     void saveRoiImage(const cv::Mat& binaryRoi, const cv::Point& center, int index, int thresholdValue, double klDivergence);
 
     int neighborhoodSize_;
@@ -92,6 +94,8 @@ class UVDARLedDetectAdaptive{
     std::vector<int> thresholdValue;
     std::vector<float> klDivergence;
     std::vector<int> validRoi;
+    int index = 0;
+
 
 };
 

launch/rosbag.launch

@@ -8,7 +8,7 @@
   <arg unless="$(arg standalone)" name="nodelet_manager" value="$(arg uav_name)_uvdar_nodelet_manager"/>
   <arg     if="$(arg standalone)" name="nodelet_manager" value=""/>
 
-  <arg name="threshold" default="100"/>
+  <arg name="threshold" default="150"/>
 
   <arg name="calibrations_folder" default="$(find mrs_uav_deployment)/config/uvdar_calibrations"/>
 
@@ -97,7 +97,7 @@
       <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_threshold" type="bool" value="false"/>
       <param name="adaptive_method" type="string" value="otsu"/>
       <param name="adaptive_debug" type="bool" value="true"/>
 
@@ -141,6 +141,9 @@
       <param name="use_camera_for_visualization"         type="bool" value="true"/>
       <param name="visualization_rate"         type="double" value="2"/>
 
+      <param name="pub_tracking_stats"      type="bool" value="true"/>
+
+
       <param name="use_4DHT"             type="bool" value="$(arg use_4DHT)"/>
 
 

scripts/.tmuxinator.yml

@@ -1,7 +1,7 @@
 name: simulation
 root: ./
 startup_window: launch
-pre_window: UAV_NAME=uav38
+pre_window: UAV_NAME=uav7
 windows:
   - core:
       layout: even-vertical
@@ -10,7 +10,7 @@ windows:
   - launch:
       layout: even-vertical
       panes:
-        - waitForRos; roslaunch uvdar_core rosbag.launch uav_name:=uav38 use_4DHT:=false debug:=false
+        - waitForRos; roslaunch uvdar_core rosbag.launch uav_name:=uav7 use_4DHT:=false debug:=false
   # - kf:
       # layout: even-vertical
       # panes:
@@ -19,7 +19,7 @@ windows:
   - rqtviz:
       layout: even-vertical
       panes:
-        #- waitForRos; sleep 3; rosbag record -O ~/Desktop/MRS_Master_Project/rosbags/exposure_5000_topics.bag /uav7/uvdar_bluefox/left/image_raw /uav7/uvdar/points_seen_left /uav7/uvdar/blinkers_seen_back /uav7/uvdar/omta_all_seq_info_back 
+        - waitForRos; sleep 3; rosbag record -O ~/Desktop/MRS_Master_Project/rosbags/good_sequence/diff_power_18m_th150_topics.bag /uav7/uvdar_bluefox/left/image_raw /uav7/uvdar/points_seen_left /uav7/uvdar/blinkers_seen_back /uav7/uvdar/ami_all_seq_info_back 
         #- rosbag record -O rosbags/first/omta_38.bag -e "(.*)_camp"
         # - rosbag record -O rosbags/first/omta_39.bag -e "(.*)_camp"
         # - rosbag record -O rosbags/first/4dht_38.bag -e "(.*)_camp"
@@ -36,13 +36,13 @@ windows:
         # - waitForRos; sleep 3; rosbag play --clock ~/experiments/20_04/two_tx/two_tx/rx/occlusion_1/rx.bag
         # - 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 ~/Desktop/MRS_Master_Project/rosbags/marlon/filtered_image_sun_temesvar_court.bag
+        - waitForRos; sleep 3; rosbag play --clock ~/Desktop/MRS_Master_Project/experiments/diff_power_18m_0615.bag
         # - waitForRos; sleep 3; rosbag play --clock ~/experiments/19_04/demo_first/rx1/all_three/rx1.bag
 
   - topics:
       layout: even-vertical
       panes:
-        - waitForRos; sleep 3; rostopic echo /uav38/uvdar/adaptive_logging_back
+        - waitForRos; sleep 3; rostopic echo /uav7/uvdar/ami_all_seq_info_back
 
  # - 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: