From cdba7a171c79fc1df4e141158a6da770559843ce Mon Sep 17 00:00:00 2001 From: Marlon Rivera Date: Sun, 3 Nov 2024 21:28:41 +0100 Subject: [PATCH] New automation scripts, changes into the adaptive, adding mutexes and making it better to start tuning the parameters --- include/detect/uv_led_detect_adaptive.cpp | 110 ++++++------ include/detect/uv_led_detect_adaptive.h | 6 +- launch/rosbag.launch | 15 +- scripts/.tmuxinator.yml | 3 +- ...k.threshold_analysis_summary_adaptive.csv# | 1 + scripts/batch_summary.py | 159 +++++++++++++++++ scripts/extract_all.sh | 4 +- scripts/file_mapping_adaptive_variant01.csv | 1 + scripts/new_processing_raw_csv.py | 4 +- scripts/process_all.sh | 4 +- scripts/processing_raw_csv.py | 4 +- scripts/test_rosbag.yml | 3 +- .../threshold_analysis_summary_adaptive.csv | 10 ++ ...hreshold_analysis_summary_variant01_v2.csv | 8 + ...hreshold_analysis_summary_variant02_v2.csv | 8 + ...hreshold_analysis_summary_variant03_v2.csv | 8 + src/detector.cpp | 161 ++++++++++-------- 17 files changed, 368 insertions(+), 141 deletions(-) create mode 100644 scripts/.~lock.threshold_analysis_summary_adaptive.csv# create mode 100755 scripts/batch_summary.py create mode 100644 scripts/file_mapping_adaptive_variant01.csv create mode 100644 scripts/threshold_analysis_summary_adaptive.csv create mode 100644 scripts/threshold_analysis_summary_variant01_v2.csv create mode 100644 scripts/threshold_analysis_summary_variant02_v2.csv create mode 100644 scripts/threshold_analysis_summary_variant03_v2.csv diff --git a/include/detect/uv_led_detect_adaptive.cpp b/include/detect/uv_led_detect_adaptive.cpp index 797c077..3d73759 100644 --- 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,7 +195,7 @@ namespace uvdar } // TODO find proper value for noisy ROI - if (contours.size() >= 6) + if (static_cast(contours.size()) >= contours_size_limit_) { // Return empty roiDetectedPoints std::vector empty_roiDetectedPoints = {}; @@ -199,14 +203,14 @@ namespace uvdar } // 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(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 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 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 index f635089..59de7a4 100644 --- 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& trackingPoints, std::vector& detectedPoints, @@ -88,6 +88,9 @@ 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; @@ -95,6 +98,7 @@ namespace uvdar std::vector lastProcessedROIs_; // For storing ROI positions std::vector allRoiDetectedPoints; // For storing ROI positions std::mutex mutex_viz_rois_; + std::mutex mutex_rois_data_; int numRois_; std::vector numberDetectedPoints_; diff --git a/launch/rosbag.launch b/launch/rosbag.launch index 589001d..102e172 100644 --- a/launch/rosbag.launch +++ b/launch/rosbag.launch @@ -14,6 +14,11 @@ + + + + + @@ -107,8 +112,8 @@ - - + + @@ -129,14 +134,16 @@ - + - + + + ["camera_left"] diff --git a/scripts/.tmuxinator.yml b/scripts/.tmuxinator.yml index bab718a..aa74e3a 100644 --- 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# new file mode 100644 index 0000000..6fa0e3f --- /dev/null +++ b/scripts/.~lock.threshold_analysis_summary_adaptive.csv# @@ -0,0 +1 @@ +,rivermar,rivermar,03.11.2024 18:39,file:///home/rivermar/.config/libreoffice/4; \ No newline at end of file diff --git a/scripts/batch_summary.py b/scripts/batch_summary.py new file mode 100755 index 0000000..59d4216 --- /dev/null +++ b/scripts/batch_summary.py @@ -0,0 +1,159 @@ +#!/usr/bin/env python + +import os +import pandas as pd +import numpy as np +import matplotlib.pyplot as plt + +# # Define the file paths and thresholds +# file_paths = [ +# '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_adaptive/variant01_adaptive_otsu_invalid_processed.csv', +# '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_adaptive/variant01_adaptive_otsu_valid_processed.csv', +# '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_adaptive/variant01_adaptive_otsu_valid_c4_d5_mxs50_processed.csv', +# '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_adaptive/variant01_adaptive_otsu_c4_d4_mxs15_processed.csv', +# '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_adaptive/variant01_adaptive_otsu_c4_d5_mxs15_processed.csv', +# '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_adaptive/variant01_adaptive_otsu_c5_d5_mxs15_processed.csv', +# '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_adaptive/variant01_adaptive_otsu_processed.csv', +# '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_adaptive/variant01_adaptive_kl_processed.csv', +# '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_adaptive/variant01_adaptive_otsu_c4_d5_mxs15_invalids_processed.csv', +# ] + +#Variant01 +# # Define the file paths and thresholds +# file_paths = [ +# '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_v2/variant01_v2_th40_processed.csv', +# '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_v2/variant01_v2_th60_processed.csv', +# '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_v2/variant01_v2_th80_processed.csv', +# '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_v2/variant01_v2_th100_processed.csv', +# '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_v2/variant01_v2_th120_processed.csv', +# '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_v2/variant01_v2_th160_processed.csv', +# '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_v2/variant01_v2_th200_processed.csv', +# ] + +#Variant02 +# # Define the file paths and thresholds +# file_paths = [ +# '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_v2/variant02_v2_th40_processed.csv', +# '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_v2/variant02_v2_th60_processed.csv', +# '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_v2/variant02_v2_th80_processed.csv', +# '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_v2/variant02_v2_th100_processed.csv', +# '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_v2/variant02_v2_th120_processed.csv', +# '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_v2/variant02_v2_th160_processed.csv', +# '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_v2/variant02_v2_th200_processed.csv', +# ] + +#Variant03 +# Define the file paths and thresholds +file_paths = [ + '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_v2/variant03_v2_th40_processed.csv', + '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_v2/variant03_v2_th60_processed.csv', + '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_v2/variant03_v2_th80_processed.csv', + '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_v2/variant03_v2_th100_processed.csv', + '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_v2/variant03_v2_th120_processed.csv', + '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_v2/variant03_v2_th160_processed.csv', + '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_v2/variant03_v2_th200_processed.csv', +] + + +thresholds = [40, 60, 80, 100, 120, 160, 200] +keys = ['invalid', 'valid', 'valid_4_5_50', 'valid_4_4_15', 'valid_4_5_15', 'valid_5_5_,15','otsu', 'kl','valid_4_5_15_invalids'] + +# Initialize a dataframe to hold the summarized data +summary_data = { + # 'Key': [], + 'Threshold': [], + 'Total Errors Signal 1': [], + 'Total Errors Signal 2': [], + 'Total Instances Signal 1': [], + 'Total Instances Signal 2': [], + 'Error Rate Signal 1': [], + 'Error Rate Signal 2': [], + 'Percentage Fully Present Signal 1': [], + 'Percentage Fully Present Signal 2': [], + 'Decoding Success Rate Signal 1': [], + 'Decoding Success Rate Signal 2': [], + 'Average Decoding Quality Signal 1': [], + 'Average Decoding Quality Signal 2': [], + 'Combined Error Rate': [], + 'Max Error Rate': [], + 'Error Rate Difference (S2 - S1)': [], + 'Relative Error Rate Difference (%)': [] +} + +# Loop through the files and calculate the required metrics +# for path, key in zip(file_paths, keys): +for path, threshold in zip(file_paths, thresholds): + # Load the data + df = pd.read_csv(path) + + # Calculate total errors for each signal + total_errors_signal_1 = df['num_errors_signal_1'].sum() + total_errors_signal_2 = df['num_errors_signal_2'].sum() + + # Calculate total instances signal present + total_signal_1 = df['num_points_signal_1'].sum() + total_signal_2 = df['num_points_signal_2'].sum() + + # Calculate error rates + error_rate_signal_1 = total_errors_signal_1 / total_signal_1 if total_signal_1 != 0 else 0 + error_rate_signal_2 = total_errors_signal_2 / total_signal_2 if total_signal_2 != 0 else 0 + + # Calculate percentage fully present based on number of errors + total_entries = len(df) + fully_present_signal_1 = (df['num_errors_signal_1'] == 0).sum() / total_entries * 100 + fully_present_signal_2 = (df['num_errors_signal_2'] == 0).sum() / total_entries * 100 + + # Calculate decoding success rate + success_rate_signal_1 = ((total_signal_1 - total_errors_signal_1) / total_signal_1) * 100 if total_signal_1 != 0 else 0 + success_rate_signal_2 = ((total_signal_2 - total_errors_signal_2) / total_signal_2) * 100 if total_signal_2 != 0 else 0 + + # Calculate decoding quality for each row + decoding_quality_signal_1 = ((df['num_points_signal_1'] - df['num_errors_signal_1']).clip(0, 3) / 3).mean() * 100 + decoding_quality_signal_2 = ((df['num_points_signal_2'] - df['num_errors_signal_2']).clip(0, 3) / 3).mean() * 100 + + # Append the data to the summary + # summary_data['Key'].append(key) + summary_data['Threshold'].append(threshold) + summary_data['Total Errors Signal 1'].append(total_errors_signal_1) + summary_data['Total Errors Signal 2'].append(total_errors_signal_2) + summary_data['Total Instances Signal 1'].append(total_signal_1) + summary_data['Total Instances Signal 2'].append(total_signal_2) + summary_data['Error Rate Signal 1'].append(error_rate_signal_1) + summary_data['Error Rate Signal 2'].append(error_rate_signal_2) + summary_data['Percentage Fully Present Signal 1'].append(fully_present_signal_1) + summary_data['Percentage Fully Present Signal 2'].append(fully_present_signal_2) + summary_data['Decoding Success Rate Signal 1'].append(success_rate_signal_1) + summary_data['Decoding Success Rate Signal 2'].append(success_rate_signal_2) + summary_data['Average Decoding Quality Signal 1'].append(decoding_quality_signal_1) + summary_data['Average Decoding Quality Signal 2'].append(decoding_quality_signal_2) + +# After looping, calculate performance differences and optimization metrics +# for i in range(len(summary_data['Key'])): +for i in range(len(summary_data['Threshold'])): + error_rate_diff = summary_data['Error Rate Signal 2'][i] - summary_data['Error Rate Signal 1'][i] + relative_error_diff = (error_rate_diff / summary_data['Error Rate Signal 1'][i]) * 100 if summary_data['Error Rate Signal 1'][i] != 0 else 0 + combined_error_rate = summary_data['Error Rate Signal 1'][i] + summary_data['Error Rate Signal 2'][i] + max_error_rate = max(summary_data['Error Rate Signal 1'][i], summary_data['Error Rate Signal 2'][i]) + + summary_data['Error Rate Difference (S2 - S1)'].append(error_rate_diff) + summary_data['Relative Error Rate Difference (%)'].append(relative_error_diff) + summary_data['Combined Error Rate'].append(combined_error_rate) + summary_data['Max Error Rate'].append(max_error_rate) + +# Convert summary data to DataFrame +df_summary = pd.DataFrame(summary_data) + +# Find the optimal threshold based on combined error rate and max error rate +# optimal_combined_threshold = df_summary.loc[df_summary['Combined Error Rate'].idxmin(), 'Key'] +optimal_combined_threshold = df_summary.loc[df_summary['Combined Error Rate'].idxmin(), 'Threshold'] +# optimal_max_threshold = df_summary.loc[df_summary['Max Error Rate'].idxmin(), 'Key'] +optimal_max_threshold = df_summary.loc[df_summary['Max Error Rate'].idxmin(), 'Threshold'] + +# Save the summary to a CSV file +df_summary.to_csv('threshold_analysis_summary_variant03_v2.csv', index=False) + +# Output the optimal thresholds +print(f'Optimal Threshold based on Combined Error Rate: {optimal_combined_threshold}') +print(f'Optimal Threshold based on Max Error Rate: {optimal_max_threshold}') + +# Optionally, you can include the plotting code here for visualization if desired diff --git a/scripts/extract_all.sh b/scripts/extract_all.sh index b9041d6..68b3999 100755 --- a/scripts/extract_all.sh +++ b/scripts/extract_all.sh @@ -1,10 +1,10 @@ #!/bin/bash # Mapping file path -mapping_file="file_mapping_dynamic.csv" +mapping_file="file_mapping_adaptive_variant01.csv" # Output directory for CSV files -output_csv_dir=~/Desktop/MRS_Master_Project/paper/dynamic_raw_csv +output_csv_dir=~/Desktop/MRS_Master_Project/paper/adaptive_raw_csv # Make sure the output directory exists mkdir -p "$output_csv_dir" diff --git a/scripts/file_mapping_adaptive_variant01.csv b/scripts/file_mapping_adaptive_variant01.csv new file mode 100644 index 0000000..32ebb3c --- /dev/null +++ b/scripts/file_mapping_adaptive_variant01.csv @@ -0,0 +1 @@ +/home/rivermar/bag_files/paper_rosbags/new_launchfile/adaptive_variant01/variant01_adaptive_otsu_c4_d5_mxs15_invalids.bag,/home/rivermar/bag_files/datasets_08_2024/uav39_rosbags/76_2024_08_20_14_02_27variant1/_2024-08-20-14-14-09.bag,/home/rivermar/bag_files/datasets_08_2024/uav40_rosbags_tx/37_2024_08_20_14_02_31variant1/_2024-08-20-14-14-10.bag diff --git a/scripts/new_processing_raw_csv.py b/scripts/new_processing_raw_csv.py index 87358fc..26b814d 100755 --- a/scripts/new_processing_raw_csv.py +++ b/scripts/new_processing_raw_csv.py @@ -15,8 +15,8 @@ variant = args.variant # e.g., 1 # Paths -input_csv_path = os.path.expanduser('~/Desktop/MRS_Master_Project/paper/new_raw_csv/' + name_experiment + '.csv') -cleaned_file_path = os.path.expanduser('~/Desktop/MRS_Master_Project/paper/processed_csv_automation_v2/' + name_experiment + '_processed.csv') +input_csv_path = os.path.expanduser('~/Desktop/MRS_Master_Project/paper/adaptive_raw_csv/' + name_experiment + '.csv') +cleaned_file_path = os.path.expanduser('~/Desktop/MRS_Master_Project/paper/processed_csv_automation_adaptive/' + name_experiment + '_processed.csv') # Read data data = pd.read_csv(input_csv_path) diff --git a/scripts/process_all.sh b/scripts/process_all.sh index 5a122df..81b0869 100755 --- a/scripts/process_all.sh +++ b/scripts/process_all.sh @@ -1,8 +1,8 @@ #!/bin/bash # Directories -raw_csv_dir=~/Desktop/MRS_Master_Project/paper/new_raw_csv -processed_csv_dir=~/Desktop/MRS_Master_Project/paper/processed_csv_automation_v2 +raw_csv_dir=~/Desktop/MRS_Master_Project/paper/adaptive_raw_csv +processed_csv_dir=~/Desktop/MRS_Master_Project/paper/processed_csv_automation_adaptive # Make sure the processed_csv directory exists mkdir -p "$processed_csv_dir" diff --git a/scripts/processing_raw_csv.py b/scripts/processing_raw_csv.py index 92d140b..4e183ee 100755 --- a/scripts/processing_raw_csv.py +++ b/scripts/processing_raw_csv.py @@ -17,8 +17,8 @@ subset_rx = args.subset_rx # e.g., 'rx1_s1' # Paths -input_csv_path = os.path.expanduser('~/Desktop/MRS_Master_Project/paper/new_raw_csv/' + name_experiment + '.csv') -cleaned_file_path = os.path.expanduser('~/Desktop/MRS_Master_Project/paper/processed_csv_automation/' + name_experiment + '_'+ subset_rx +'_processed.csv') +input_csv_path = os.path.expanduser('~/Desktop/MRS_Master_Project/paper/adaptive_raw_csv/' + name_experiment + '.csv') +cleaned_file_path = os.path.expanduser('~/Desktop/MRS_Master_Project/paper/processed_csv_automation_adaptive/' + name_experiment + '_'+ subset_rx +'_processed.csv') # Read data data = pd.read_csv(input_csv_path) diff --git a/scripts/test_rosbag.yml b/scripts/test_rosbag.yml index bab718a..aa74e3a 100644 --- a/scripts/test_rosbag.yml +++ b/scripts/test_rosbag.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/threshold_analysis_summary_adaptive.csv b/scripts/threshold_analysis_summary_adaptive.csv new file mode 100644 index 0000000..40f2eda --- /dev/null +++ b/scripts/threshold_analysis_summary_adaptive.csv @@ -0,0 +1,10 @@ +Key,Total Errors Signal 1,Total Errors Signal 2,Total Instances Signal 1,Total Instances Signal 2,Error Rate Signal 1,Error Rate Signal 2,Percentage Fully Present Signal 1,Percentage Fully Present Signal 2,Decoding Success Rate Signal 1,Decoding Success Rate Signal 2,Average Decoding Quality Signal 1,Average Decoding Quality Signal 2,Combined Error Rate,Max Error Rate,Error Rate Difference (S2 - S1),Relative Error Rate Difference (%) +invalid,19666,11474,54905,51008,0.35818231490756763,0.22494510664993728,72.31418370658878,72.4634858812074,64.18176850924324,77.50548933500627,76.250135237477,85.5436546575787,0.5831274215575049,0.35818231490756763,-0.13323720825763036,-37.1981537647981 +valid,11439,6756,51346,45428,0.22278268998558798,0.1487188518094567,76.06607229248739,84.829635773032,77.72173100144121,85.12811481905432,91.93678438961457,89.06627963231738,0.3715015417950447,0.22278268998558798,-0.07406383817613127,-33.24488010308275 +valid_4_5_50,9305,5510,51497,47719,0.18069013728955086,0.11546763343741487,78.37012987012987,86.29220779220779,81.93098627104492,88.45323665625851,91.32467532467533,91.36147186147186,0.2961577707269657,0.18069013728955086,-0.06522250385213599,-36.09632757521168 +valid_4_4_15,8825,5503,51017,47712,0.17298155516788521,0.1153378604963112,78.8051948051948,86.29220779220779,82.70184448321149,88.46621395036888,91.32467532467533,91.36147186147186,0.2883194156641964,0.17298155516788521,-0.057643694671574014,-33.323607604075825 +valid_4_5_15,8825,5503,51020,47715,0.17297138377107016,0.11533060882322121,78.806571001883,86.29309785078891,82.70286162289298,88.46693911767788,91.32523862086877,91.36203276843494,0.28830199259429135,0.17297138377107016,-0.057640774947848944,-33.323879182314485 +"valid_5_5_,15",11518,6985,53501,48218,0.21528569559447486,0.1448629142643826,76.75324675324676,84.86363636363636,78.47143044055251,85.51370857356174,90.87229437229436,89.22510822510823,0.36014860985885744,0.21528569559447486,-0.07042278133009225,-32.71131467217629 +otsu,23013,11312,57064,49836,0.40328403196411045,0.22698450919014368,71.2597649839165,78.42184730519267,59.671596803588955,77.30154908098564,74.34735989846604,83.62765049563447,0.6302685411542541,0.40328403196411045,-0.17629952277396677,-43.71596909387581 +kl,44395,58183,67439,94944,0.6582986105962425,0.6128138692281766,6.929016526686535,2.6212408561365486,34.17013894037575,38.71861307718234,50.106113970920255,79.0910322405852,1.271112479824419,0.6582986105962425,-0.045484741368065906,-6.909439065482593 +valid_4_5_15_invalids,9653,6963,44402,44233,0.21740011711184182,0.157416408563742,75.89094180333541,81.49609023596983,78.25998828881582,84.25835914362581,80.1536225866722,85.96867575484973,0.3748165256755838,0.21740011711184182,-0.05998370854809981,-27.59138741274969 diff --git a/scripts/threshold_analysis_summary_variant01_v2.csv b/scripts/threshold_analysis_summary_variant01_v2.csv new file mode 100644 index 0000000..03ca1d2 --- /dev/null +++ b/scripts/threshold_analysis_summary_variant01_v2.csv @@ -0,0 +1,8 @@ +Threshold,Total Errors Signal 1,Total Errors Signal 2,Total Instances Signal 1,Total Instances Signal 2,Error Rate Signal 1,Error Rate Signal 2,Percentage Fully Present Signal 1,Percentage Fully Present Signal 2,Decoding Success Rate Signal 1,Decoding Success Rate Signal 2,Average Decoding Quality Signal 1,Average Decoding Quality Signal 2,Combined Error Rate,Max Error Rate,Error Rate Difference (S2 - S1),Relative Error Rate Difference (%) +40,3564,3637,45856,46383,0.0777215631542219,0.07841234935213333,92.22929109322254,90.93741885224617,92.2278436845778,92.15876506478666,91.50437115900631,92.48679996537696,0.15613391250635522,0.07841234935213333,0.0006907861979114222,0.8887960687830015 +60,3422,3429,45853,46404,0.07462979521514405,0.0738944918541505,92.34614385873799,91.56712542196831,92.5370204784856,92.61055081458494,91.81814247381634,92.99532588937937,0.14852428706929455,0.07462979521514405,-0.0007353033609935472,-0.985267826172914 +80,3338,3216,45851,46401,0.07280102942138666,0.06930885110234693,92.54545454545455,91.81818181818183,92.71989705786133,93.0691148897653,92.01948051948052,93.47402597402598,0.14210988052373358,0.07280102942138666,-0.0034921783190397376,-4.796880410613872 +100,3265,3253,45865,46381,0.07118717976670665,0.0701364782993036,92.72727272727272,90.66233766233766,92.88128202332933,92.98635217006964,92.20129870129871,93.34415584415584,0.14132365806601024,0.07118717976670665,-0.001050701467403048,-1.4759700705188603 +120,3176,4481,45851,46382,0.0692678458485093,0.09661075417187702,92.96753246753246,82.27272727272728,93.07321541514906,90.3389245828123,92.37012987012987,90.6948051948052,0.16587860002038632,0.09661075417187702,0.02734290832336772,39.474171584846765 +160,3067,8499,45856,46424,0.06688328681088625,0.18307341030501464,93.05753994025197,59.702558773866734,93.31167131891137,81.69265896949854,92.62891284582413,82.09940684937438,0.2499566971159009,0.18307341030501464,0.1161901234941284,173.72071414889965 +200,2894,11749,45845,46460,0.06312574980913949,0.2528842014636246,93.27272727272728,43.922077922077925,93.68742501908605,74.71157985363753,92.96753246753246,75.13203463203463,0.3160099512727641,0.2528842014636246,0.18975845165448513,300.6038775431884 diff --git a/scripts/threshold_analysis_summary_variant02_v2.csv b/scripts/threshold_analysis_summary_variant02_v2.csv new file mode 100644 index 0000000..0b8a97b --- /dev/null +++ b/scripts/threshold_analysis_summary_variant02_v2.csv @@ -0,0 +1,8 @@ +Threshold,Total Errors Signal 1,Total Errors Signal 2,Total Instances Signal 1,Total Instances Signal 2,Error Rate Signal 1,Error Rate Signal 2,Percentage Fully Present Signal 1,Percentage Fully Present Signal 2,Decoding Success Rate Signal 1,Decoding Success Rate Signal 2,Average Decoding Quality Signal 1,Average Decoding Quality Signal 2,Combined Error Rate,Max Error Rate,Error Rate Difference (S2 - S1),Relative Error Rate Difference (%) +40,3651,4179,48506,48061,0.07526903888178782,0.08695199850190383,92.44750092103648,87.31425764460272,92.47309611182122,91.30480014980962,91.80686888534119,89.81538335584756,0.16222103738369165,0.08695199850190383,0.011682959620116007,15.521600639094688 +60,3575,6403,48517,48336,0.0736855122946596,0.1324685534591195,92.47820213680463,73.68905808670023,92.63144877053404,86.75314465408806,91.98493593679645,85.82627205370665,0.2061540657537791,0.1324685534591195,0.05878304116445991,79.77557505387698 +80,3434,7987,48500,48077,0.0708041237113402,0.1661293341930653,92.73475403795369,63.10876374132531,92.91958762886597,83.38706658069347,92.24344408278573,82.05695101230322,0.2369334579044055,0.1661293341930653,0.0953252104817251,134.6322862074452 +100,3281,10824,48500,48041,0.06764948453608248,0.22530754980121145,92.95100085963404,50.362274346064105,93.23505154639176,77.46924501987885,92.55188505464817,76.17380981620205,0.2929570343372939,0.22530754980121145,0.157658065265129,233.05139181221443 +120,3147,13276,48503,48064,0.06488258458239697,0.2762150466045273,93.16548971446116,40.55879643844028,93.5117415417603,72.37849533954727,92.83184935011771,71.20458499641796,0.34109763118692427,0.2762150466045273,0.2113324620221303,325.71523372924645 +160,2819,18607,48305,48139,0.058358347997101746,0.3865265169612996,93.59572639076508,23.369765442711532,94.16416520028983,61.34734830387004,93.09836669532113,60.44455360432273,0.44488486495840135,0.3865265169612996,0.3281681689641978,562.3328627816807 +200,2731,25055,48513,48047,0.056294189186403644,0.5214685620330094,93.69397028122314,10.106840230873143,94.37058108135965,47.853143796699065,93.70420401981252,47.05882352941176,0.577762751219413,0.5214685620330094,0.46517437284660573,826.3275118970116 diff --git a/scripts/threshold_analysis_summary_variant03_v2.csv b/scripts/threshold_analysis_summary_variant03_v2.csv new file mode 100644 index 0000000..a1ee80c --- /dev/null +++ b/scripts/threshold_analysis_summary_variant03_v2.csv @@ -0,0 +1,8 @@ +Threshold,Total Errors Signal 1,Total Errors Signal 2,Total Instances Signal 1,Total Instances Signal 2,Error Rate Signal 1,Error Rate Signal 2,Percentage Fully Present Signal 1,Percentage Fully Present Signal 2,Decoding Success Rate Signal 1,Decoding Success Rate Signal 2,Average Decoding Quality Signal 1,Average Decoding Quality Signal 2,Combined Error Rate,Max Error Rate,Error Rate Difference (S2 - S1),Relative Error Rate Difference (%) +40,14718,17244,47099,30423,0.3124907110554364,0.5668080070998915,25.806661462399166,12.59432734842571,68.75092889445635,43.31919929001084,70.21424234538988,28.57793390580276,0.879298718155328,0.5668080070998915,0.25431729604445513,81.38395384153955 +60,11785,12408,32907,20228,0.3581304889537181,0.6134071583943049,15.20346646571213,15.4577995478523,64.18695110462819,38.659284160569506,66.32127606129113,24.554132127606128,0.9715376473480231,0.6134071583943049,0.25527666944058686,71.28035096547637 +80,20746,18473,47592,28309,0.43591359892418896,0.6525486594369282,7.773368893514604,19.573277824757692,56.40864010758111,34.74513405630718,58.21028209631605,21.327435547171444,1.0884622583611172,0.6525486594369282,0.21663506051273923,49.696788778185116 +100,22762,17325,47349,25903,0.4807282096770787,0.6688414469366483,5.698302218174722,27.21004358290509,51.92717903229212,33.11585530633517,53.312083956720656,18.599709447299375,1.149569656613727,0.6688414469366483,0.18811323725956958,39.130892149210794 +120,15316,9470,29903,12090,0.5121894124335351,0.7832919768403639,5.905752753977969,24.388004895960833,48.78105875664649,21.670802315963606,49.595403236774104,8.907928736570106,1.295481389273899,0.7832919768403639,0.2711025644068288,52.93013830933273 +160,27009,13829,46645,14385,0.5790331225211706,0.9613486270420577,3.629976580796253,10.174342961228207,42.096687747882946,3.86513729579423,42.57958192384421,1.2056553040159597,1.5403817495632284,0.9613486270420577,0.38231550452088714,66.0265345195186 +200,30877,10514,46463,10564,0.6645502873253987,0.9952669443392654,2.0295322968841476,31.65289793794315,33.54497126746013,0.47330556607345703,33.79518202909863,0.10841518679936685,1.6598172316646642,0.9952669443392654,0.33071665701386677,49.7654825107209 diff --git a/src/detector.cpp b/src/detector.cpp index fde3136..1a2810c 100644 --- a/src/detector.cpp +++ b/src/detector.cpp @@ -55,6 +55,9 @@ class UVDARDetector : public nodelet::Nodelet{ 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)); + param_loader.loadParam("contour_size_limit",_contours_size_limit_, 4); + param_loader.loadParam("contour_max_size_limit",_contour_max_size_limit_, 15); + param_loader.loadParam("roi_detected_points_limit",_roi_detected_points_limit_, 5); //Print adaptive threshold @@ -63,6 +66,9 @@ class UVDARDetector : public nodelet::Nodelet{ //Print selected adaptive method ROS_INFO_STREAM("[UVDARDetector]: Adaptive method: " << _adaptive_method_); + ROS_INFO_STREAM("[UVDARDetector]: Contour size limit : " << _contours_size_limit_); + ROS_INFO_STREAM("[UVDARDetector]: Contour max size limit : " << _contour_max_size_limit_); + ROS_INFO_STREAM("[UVDARDetector]: ROI detected points limit : " << _roi_detected_points_limit_); } else{ ROS_INFO_STREAM("[UVDARDetector]: Adaptive thresholding disabled."); @@ -130,8 +136,8 @@ class UVDARDetector : public nodelet::Nodelet{ cals_blinkers_seen_.push_back(callback); timer_process_tracking_.push_back(ros::Timer()); - //trackingPointsPerCamera.resize(_camera_count_); - trackingPointsPerCamera.push_back(std::vector()); + //trackingPointsPerCamera_.resize(_camera_count_); + trackingPointsPerCamera_.push_back(std::vector()); adaptive_detected_points_.push_back(std::vector()); ROS_INFO("[UVDARDetector]: Initializing ADAPTIVE-based marker detection..."); @@ -139,7 +145,10 @@ class UVDARDetector : public nodelet::Nodelet{ 20, 5.0, _adaptive_method_, - _adaptive_debug_ + _adaptive_debug_, + _contours_size_limit_, + _contour_max_size_limit_, + _roi_detected_points_limit_ )); if (!uvda_.back()){ ROS_ERROR("[UVDARDetector]: Failed to initialize ADAPTIVE-based marker detection!"); @@ -241,7 +250,7 @@ class UVDARDetector : public nodelet::Nodelet{ for (size_t i = 0; i < _adaptive_logging_topics.size(); ++i) { pub_adaptive_logging_.push_back(nh_.advertise(_adaptive_logging_topics[i], 1)); - ROI_data.push_back(ROIData()); + /* ROI_data.push_back(ROIData()); */ } } @@ -345,20 +354,21 @@ class UVDARDetector : public nodelet::Nodelet{ /* processTrackingPoints //{ */ void processTrackingPoints([[maybe_unused]]const ros::TimerEvent& te, const uvdar_core::ImagePointsWithFloatStampedConstPtr& msg, int image_index) { - trackingPointsPerCamera[image_index].clear(); + std::scoped_lock lock(mutex_tracking_points_); + trackingPointsPerCamera_[image_index].clear(); for (const auto& point : msg->points) { //cv::Point cvPoint(static_cast(point.x), static_cast(point.y)); //Without casting if (point.value > 0) { - + /* ROS_INFO_STREAM("[UVDARDetector]: Point value " << point.value); */ cv::Point cvPoint(point.x, point.y); - trackingPointsPerCamera[image_index].push_back(cvPoint); + trackingPointsPerCamera_[image_index].push_back(cvPoint); } } - ROS_INFO_STREAM("[UVDARDetector]: Camera " << image_index << " Tracking points: " << trackingPointsPerCamera[image_index].size()); + ROS_INFO_STREAM("[UVDARDetector]: Camera " << image_index << " Tracking points: " << trackingPointsPerCamera_[image_index].size()); } //} @@ -381,7 +391,7 @@ class UVDARDetector : public nodelet::Nodelet{ //} /* processStandard //{ */ - void processStandard(const cv_bridge::CvImageConstPtr& image, int image_index){ + bool processStandard(const cv_bridge::CvImageConstPtr& image, int image_index){ /** * @brief Extracts small bright points from input image * @@ -402,13 +412,16 @@ class UVDARDetector : public nodelet::Nodelet{ ) ){ ROS_ERROR_STREAM("Failed to extract markers from the image!"); - return; + return false; } if(sun_points_[image_index].size() > 30){ ROS_ERROR_STREAM("There are " << sun_points_[image_index].size() << " detected potential sun points! Check your exposure!"); + return false; } + return true; + } //} @@ -445,7 +458,7 @@ class UVDARDetector : public nodelet::Nodelet{ /* publishAdaptive //{ */ void publishAdaptive(const cv_bridge::CvImageConstPtr& image, int image_index, const std::vector& adaptive_detected_points) { - ROS_INFO_STREAM("[UVDARDetector]: Publishing adaptive points. In camera: " << image_index); + /* ROS_INFO_STREAM("[UVDARDetector]: Publishing adaptive points. In camera: " << image_index); */ if (!adaptive_detected_points.empty()) { //Current camera @@ -481,12 +494,12 @@ class UVDARDetector : public nodelet::Nodelet{ /* publishStandard //{ */ void publishStandard(const cv_bridge::CvImageConstPtr& image, int image_index, const std::vector& detected_points) { - ROS_INFO_STREAM("[UVDARDetector]: Detected points in camera " << image_index << ": " << detected_points.size()); + /* ROS_INFO_STREAM("[UVDARDetector]: Detected points in camera " << image_index << ": " << detected_points.size()); */ if (!detected_points.empty()) { - ROS_INFO_STREAM("[UVDARDetector]: Detected points: " << detected_points.size()); + /* ROS_INFO_STREAM("[UVDARDetector]: Detected points: " << detected_points.size()); */ } else { - ROS_INFO_STREAM("[UVDARDetector]: No detected points. In camera: " << image_index); + /* ROS_INFO_STREAM("[UVDARDetector]: No detected points. In camera: " << image_index); */ } @@ -538,63 +551,64 @@ class UVDARDetector : public nodelet::Nodelet{ return; } - if( _adaptive_threshold_ && trackingPointsPerCamera[image_index].size() > 0){ - - ROS_INFO_STREAM("[UVDARDetector]: Tracking points per camera: " << trackingPointsPerCamera[image_index].size()); + { + std::scoped_lock lock(mutex_camera_image_, mutex_tracking_points_); + + if( _adaptive_threshold_ && trackingPointsPerCamera_[image_index].size() > 0){ - { - std::scoped_lock lock(mutex_camera_image_); - images_current_[image_index] = image->image; - sun_points_[image_index].clear(); - detected_points_[image_index].clear(); - adaptive_detected_points_[image_index].clear(); + ROS_INFO_STREAM("[UVDARDetector]: Tracking points per camera: " << trackingPointsPerCamera_[image_index].size()); - //TODO: Check if its worth it, this to be able to detect new points that where not currently detected - ROS_INFO_STREAM("[UVDARDetector]: Processing image with standard thresholding. In camera: " << image_index); - - processStandard(image, image_index); + + /* { */ + /* std::scoped_lock lock(mutex_camera_image_); */ + images_current_[image_index] = image->image; + sun_points_[image_index].clear(); + detected_points_[image_index].clear(); + adaptive_detected_points_[image_index].clear(); - //ROS_INFO_STREAM("[UVDARDetector]: Processing image with tracking points only."); - ROS_INFO_STREAM("[UVDARDetector]: Processing image with adaptive thresholding. In camera: " << image_index); - //Print number of points given - ROS_INFO_STREAM("[UVDARDetector]: Tracking points provided: " << trackingPointsPerCamera[image_index].size()); + //TODO: Check if its worth it, this to be able to detect new points that where not currently detected + /* ROS_INFO_STREAM("[UVDARDetector]: Initial detection with standard method. In camera: " << image_index); */ - processAdaptive(image, image_index, trackingPointsPerCamera[image_index]); - - } - - } - else{ + auto res = processStandard(image, image_index); //TODO Make the processing functions to take reference of the member variables in the inputs, will make it more clear - adaptive_detected_points_[image_index].clear(); - ROS_INFO_STREAM("[UVDARDetector]: Processing with standard detection " << image_index); + if (!res) { + return; + } - /* ROS_INFO_STREAM("[UVDARDetector]: Locking cam image mutex " << image_index << "..."); */ - - { - /* std::scoped_lock lock(*mutex_camera_image_[image_index]); */ - std::scoped_lock lock(mutex_camera_image_); + /* ROS_INFO_STREAM("[UVDARDetector]: Processing image with adaptive thresholding. In camera: " << image_index); */ - if (!uvdf_was_initialized_){ - if (!uvdf_->initDelayed(image->image)){ - ROS_WARN_STREAM_THROTTLE(1.0,"[UVDARDetector]: Failed to initialize, dropping message..."); - return; - } - uvdf_was_initialized_ = true; + processAdaptive(image, image_index, trackingPointsPerCamera_[image_index]); + /* } */ } - - images_current_[image_index] = image->image; - sun_points_[image_index].clear(); - detected_points_[image_index].clear(); + else{ - processStandard(image, image_index); + adaptive_detected_points_[image_index].clear(); + ROS_INFO_STREAM("[UVDARDetector]: Processing with standard detection " << image_index); + /* ROS_INFO_STREAM("[UVDARDetector]: Locking cam image mutex " << image_index << "..."); */ + /* std::scoped_lock lock(*mutex_camera_image_[image_index]); */ + /* std::scoped_lock lock(mutex_camera_image_); */ - } + if (!uvdf_was_initialized_){ + if (!uvdf_->initDelayed(image->image)){ + ROS_WARN_STREAM_THROTTLE(1.0,"[UVDARDetector]: Failed to initialize, dropping message..."); + return; + } + uvdf_was_initialized_ = true; + } + + images_current_[image_index] = image->image; + sun_points_[image_index].clear(); + detected_points_[image_index].clear(); - } + auto res = processStandard(image, image_index); + if (!res) { + return; + } + } + } if (detected_points_[image_index].size()>MAX_POINTS_PER_IMAGE){ ROS_WARN_STREAM("[UVDARDetector]: Over " << MAX_POINTS_PER_IMAGE << " points received. Skipping noisy image."); @@ -638,6 +652,7 @@ class UVDARDetector : public nodelet::Nodelet{ if (_adaptive_threshold_) { int image_index = 0; + std::scoped_lock lock(mutex_camera_image_); cv::Mat white_background = cv::Mat::ones(images_current_[image_index].size(), images_current_[image_index].type()) * 255; uvda_[image_index]->generateVisualizationAdaptive(images_current_[image_index], visualization_image, adaptive_detected_points_[image_index]); publishVisualizationImage(visualization_image); @@ -645,16 +660,16 @@ class UVDARDetector : public nodelet::Nodelet{ //TODO check why its crashing when including the standard generateVisualization /* generateVisualization(image_visualization_); */ - if ((image_visualization_.cols != 0) && (image_visualization_.rows != 0)){ - if (_publish_visualization_){ - ROS_INFO_STREAM("[UVDARDetector]: Publishing visualization."); - pub_visualization_->publish("uvdar_detection_visualization", 0.01, image_visualization_, true); - } - if (_gui_){ - cv::imshow("ocv_uvdar_detection_" + _uav_name_, image_visualization_); - cv::waitKey(25); - } - } + /* if ((image_visualization_.cols != 0) && (image_visualization_.rows != 0)){ */ + /* if (_publish_visualization_){ */ + /* ROS_INFO_STREAM("[UVDARDetector]: Publishing visualization."); */ + /* pub_visualization_->publish("uvdar_detection_visualization", 0.01, image_visualization_, true); */ + /* } */ + /* if (_gui_){ */ + /* cv::imshow("ocv_uvdar_detection_" + _uav_name_, image_visualization_); */ + /* cv::waitKey(25); */ + /* } */ + /* } */ } } //} @@ -743,6 +758,7 @@ class UVDARDetector : public nodelet::Nodelet{ std::unique_ptr pub_visualization_; /* std::vector> mutex_camera_image_; */ std::mutex mutex_camera_image_; + std::mutex mutex_tracking_points_; ros::Timer timer_visualization_; ros::Timer timer_gui_visualization_; ros::Timer timer_publish_visualization_; @@ -757,6 +773,9 @@ class UVDARDetector : public nodelet::Nodelet{ bool _adaptive_threshold_; std::string _adaptive_method_; bool _adaptive_debug_; + int _contours_size_limit_; + int _contour_max_size_limit_; + int _roi_detected_points_limit_; double _initial_delay_ = 5.0; @@ -778,14 +797,12 @@ class UVDARDetector : public nodelet::Nodelet{ std::vector timer_process_tracking_; - std::vector trackingPoints; - std::vector> trackingPointsPerCamera; + /* std::vector trackingPoints; */ + std::vector> trackingPointsPerCamera_; std::vector> uvda_; std::vector> adaptive_detected_points_; std::vector> combinedPoints_; - - - std::vector ROI_data; + /* std::vector ROI_data; */ };