From 2222a2cbee95dc52a6b73df9f973dc8ad78b4ad2 Mon Sep 17 00:00:00 2001 From: Marlon Rivera Date: Sat, 16 Nov 2024 18:25:44 +0100 Subject: [PATCH] Changes for better tuning --- include/detect/uv_led_detect_adaptive.cpp | 76 ++++++++++++------- include/detect/uv_led_detect_adaptive.h | 14 +++- launch/rosbag.launch | 12 ++- scripts/.tmuxinator.yml | 2 +- ...k.threshold_analysis_summary_adaptive.csv# | 2 +- ...reshold_analysis_summary_variant02_v2.csv# | 1 + scripts/Processing_evaluation_distance.ipynb | 44 ++++++----- scripts/batch_summary.py | 59 +++++++------- scripts/extract_all.sh | 4 +- scripts/file_mapping_adaptive_variant01.csv | 2 +- scripts/file_mapping_adaptive_variant02.csv | 1 + scripts/new_processing_raw_csv.py | 2 +- scripts/process_all.sh | 2 +- scripts/test_rosbag.yml | 2 +- .../threshold_analysis_summary_adaptive.csv | 10 +-- src/detector.cpp | 14 +++- 16 files changed, 152 insertions(+), 95 deletions(-) create mode 100644 scripts/.~lock.threshold_analysis_summary_variant02_v2.csv# create mode 100644 scripts/file_mapping_adaptive_variant02.csv diff --git a/include/detect/uv_led_detect_adaptive.cpp b/include/detect/uv_led_detect_adaptive.cpp index 3d73759..3b5119e 100644 --- a/include/detect/uv_led_detect_adaptive.cpp +++ b/include/detect/uv_led_detect_adaptive.cpp @@ -27,14 +27,21 @@ struct PointComparator namespace uvdar { - 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) + 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, double sigma_x, + double sigma_y, double grayscale_roi_weight, double sharped_roi_weight) : neighborhoodSize_(neighborhoodSize), point_similarity_threshold_(point_similarity_threshold), adaptive_method_(adaptive_method), 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) + roi_detected_points_limit_(roi_detected_points_limit), + sigmaX_(sigma_x), + sigmaY_(sigma_y), + grayscale_ROI_weight_(grayscale_roi_weight), + sharped_ROI_weight_(sharped_roi_weight) + { } @@ -82,6 +89,10 @@ namespace uvdar return false; } + // To include the standardPoints + /* std::vector combinedPoints; */ + // Add all adaptive points + /* combinedPoints.insert(combinedPoints.end(), trackingPoints.begin(), trackingPoints.end()); */ std::vector rois; for (const auto& point : trackingPoints) @@ -151,16 +162,14 @@ namespace uvdar // Apply Gaussian blur to the ROI cv::Mat blurred; - double sigmaX = 6.0; - double sigmaY = 6.0; - cv::GaussianBlur(grayImage, blurred, cv::Size(0, 0), sigmaX, sigmaY); + cv::GaussianBlur(grayImage, blurred, cv::Size(0, 0), sigmaX_, sigmaY_); // Create unsharp mask by subtracting the blurred version from the original image cv::Mat unsharpMask = grayImage - blurred; // Apply the unsharp mask to the original image to enhance edges cv::Mat enhancedImage; - cv::addWeighted(grayImage, 0.25, unsharpMask, 1.75, 0, enhancedImage); + cv::addWeighted(grayImage, grayscale_ROI_weight_, unsharpMask, sharped_ROI_weight_, 0, enhancedImage); // BinaryRoi to save after applying the threshold cv::Mat binaryRoi; @@ -188,6 +197,10 @@ namespace uvdar // Create a mask to draw the filtered contours cv::Mat mask = cv::Mat::zeros(binaryRoi.size(), CV_8UC1); + lastProcessedROIs_.push_back(roi); // Store the ROI for visualization + // Store the binary ROI (For debugging/visualization) + lastProcessedBinaryROIs_.push_back(binaryRoi); + if (adaptive_debug_) { // Print the contours size @@ -197,8 +210,16 @@ namespace uvdar // TODO find proper value for noisy ROI if (static_cast(contours.size()) >= contours_size_limit_) { + if (adaptive_debug_) + { + std::cout << "[UVDARLedDetectAdaptive]: NUMBER OF CONTOURS OUTSIDE OF THE LIMIT: " << contours.size() << std::endl; + } // Return empty roiDetectedPoints std::vector empty_roiDetectedPoints = {}; + thresholdValues_.push_back(thresholdValue_); + klDivergences_.push_back(0.0); + numberDetectedPoints_.push_back(0); + validRois_.push_back(0); return empty_roiDetectedPoints; } @@ -236,9 +257,16 @@ namespace uvdar std::cout << "[UVDARLedDetectAdaptive]: ROI NUMBER OF DETECTED POINTS: " << roiDetectedPoints.size() << std::endl; } - // TODO find the proper value of the detected points - // 5 + 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_); + } if (static_cast(roiDetectedPoints.size()) > roi_detected_points_limit_) { @@ -257,32 +285,23 @@ namespace uvdar { klDivergences_.push_back(minKLDivergence_); } - validRois_.push_back(0); + validRois_.push_back(0); // Return empty roiDetectedPoints std::vector empty_roiDetectedPoints = {}; - return empty_roiDetectedPoints; // Clear the lastProcessedROIs_ and lastProcessedBinaryROIs_ vectors - lastProcessedROIs_.clear(); - lastProcessedBinaryROIs_.clear(); + /* lastProcessedROIs_.clear(); */ + /* lastProcessedBinaryROIs_.clear(); */ + + return empty_roiDetectedPoints; } 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); + /* // 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; @@ -495,6 +514,11 @@ namespace uvdar } if (!isOverlap) { + if (adaptive_debug_) + { + + std::cout << "[UVDARLedDetectAdaptive]: ADDING STANDARD POINT " << std::endl; + } combinedPoints.push_back(standardPoint); } } diff --git a/include/detect/uv_led_detect_adaptive.h b/include/detect/uv_led_detect_adaptive.h index 59de7a4..8f20910 100644 --- a/include/detect/uv_led_detect_adaptive.h +++ b/include/detect/uv_led_detect_adaptive.h @@ -33,7 +33,8 @@ namespace uvdar { public: UVDARLedDetectAdaptive(int neighborhoodSize = 25, double point_similarity_threshold = 5.0, std::string adaptive_method = "Otsu", - bool adaptive_debug = false, int contours_size_limit = 4, int contour_max_size_limit = 15, int roi_detected_points_limit = 5); + bool adaptive_debug = false, int contours_size_limit = 4, int contour_max_size_limit = 15, int roi_detected_points_limit = 5, + double sigma_x = 6.0, double sigma_y = 6.0, double grayscale_roi_weight = 0.25, double sharped_roi_weight = 1.75); ~UVDARLedDetectAdaptive(); bool processImageAdaptive(const cv::Mat& inputImage, const std::vector& trackingPoints, std::vector& detectedPoints, @@ -88,9 +89,14 @@ 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 contours_size_limit_; + int contour_max_size_limit_; + int roi_detected_points_limit_; + double sigmaX_; + double sigmaY_; + double grayscale_ROI_weight_; + double sharped_ROI_weight_; + int roiIndex_; int MAX_SIZE = 50; int MIN_SIZE = 5; diff --git a/launch/rosbag.launch b/launch/rosbag.launch index 102e172..ffd93ff 100644 --- a/launch/rosbag.launch +++ b/launch/rosbag.launch @@ -19,6 +19,10 @@ + + + + @@ -134,16 +138,20 @@ - + - + + + + + ["camera_left"] diff --git a/scripts/.tmuxinator.yml b/scripts/.tmuxinator.yml index aa74e3a..8fa4755 100644 --- a/scripts/.tmuxinator.yml +++ b/scripts/.tmuxinator.yml @@ -35,7 +35,7 @@ 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 ~/bag_files/datasets_08_2024/uav36_rosbags_rx/marlon/69_2024_08_20_14_12_22variant1/_2024-08-20-14-14-09.bag + - waitForRos; sleep 3; rosbag play --clock ~/bag_files/datasets_08_2024/uav36_rosbags_rx/marlon/70_2024_08_20_14_22_18variant2/_2024-08-20-14-23-37.bag - blinkers-seen: layout: even-vertical panes: diff --git a/scripts/.~lock.threshold_analysis_summary_adaptive.csv# b/scripts/.~lock.threshold_analysis_summary_adaptive.csv# index 6fa0e3f..a13b4c2 100644 --- a/scripts/.~lock.threshold_analysis_summary_adaptive.csv# +++ b/scripts/.~lock.threshold_analysis_summary_adaptive.csv# @@ -1 +1 @@ -,rivermar,rivermar,03.11.2024 18:39,file:///home/rivermar/.config/libreoffice/4; \ No newline at end of file +,rivermar,rivermar,16.11.2024 14:29,file:///home/rivermar/.config/libreoffice/4; \ No newline at end of file diff --git a/scripts/.~lock.threshold_analysis_summary_variant02_v2.csv# b/scripts/.~lock.threshold_analysis_summary_variant02_v2.csv# new file mode 100644 index 0000000..22b2c1b --- /dev/null +++ b/scripts/.~lock.threshold_analysis_summary_variant02_v2.csv# @@ -0,0 +1 @@ +,rivermar,rivermar,16.11.2024 14:30,file:///home/rivermar/.config/libreoffice/4; \ No newline at end of file diff --git a/scripts/Processing_evaluation_distance.ipynb b/scripts/Processing_evaluation_distance.ipynb index 38dba1a..3bb7ff5 100644 --- a/scripts/Processing_evaluation_distance.ipynb +++ b/scripts/Processing_evaluation_distance.ipynb @@ -657,15 +657,15 @@ }, { "cell_type": "code", - "execution_count": 2, + "execution_count": 5, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ - "Optimal Threshold based on Combined Error Rate: 40\n", - "Optimal Threshold based on Max Error Rate: 40\n" + "Optimal Threshold based on Combined Error Rate: valid_4_5_15\n", + "Optimal Threshold based on Max Error Rate: valid_4_5_15\n" ] } ], @@ -677,23 +677,29 @@ "\n", "# Define the file paths and thresholds\n", "file_paths = [\n", - " '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_dynamic/motiv_dynamic_th40_processed.csv',\n", - " '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_dynamic/motiv_dynamic_th60_processed.csv',\n", - " '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_dynamic/motiv_dynamic_th80_processed.csv',\n", - " '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_dynamic/motiv_dynamic_th100_processed.csv',\n", - " '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_dynamic/motiv_dynamic_th120_processed.csv',\n", - " '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_dynamic/motiv_dynamic_th160_processed.csv',\n", - " '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_dynamic/motiv_dynamic_th200_processed.csv'\n", + " '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_adaptive/variant01_adaptive_otsu_invalid_processed.csv',\n", + " '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_adaptive/variant01_adaptive_otsu_valid_processed.csv',\n", + " '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_adaptive/variant01_adaptive_otsu_valid_c4_d5_mxs50_processed.csv',\n", + " '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_adaptive/variant01_adaptive_otsu_c4_d4_mxs15_processed.csv',\n", + " '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_adaptive/variant01_adaptive_otsu_c4_d5_mxs15_processed.csv',\n", + " '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_adaptive/variant01_adaptive_otsu_c5_d5_mxs15_processed.csv',\n", + " '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_adaptive/variant01_adaptive_otsu_processed.csv',\n", + " '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_adaptive/variant01_adaptive_kl_processed.csv',\n", + " '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_adaptive/variant01_adaptive_otsu_c4_d5_mxs15_invalids_processed.csv',\n", + "\n", + "\n", + "\n", "]\n", "\n", "\n", "\n", "\n", "thresholds = [40, 60, 80, 100, 120, 160, 200]\n", + "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']\n", "\n", "# Initialize a dataframe to hold the summarized data\n", "summary_data = {\n", - " 'Threshold': [],\n", + " 'Key': [],\n", " 'Total Errors Signal 1': [],\n", " 'Total Errors Signal 2': [],\n", " 'Total Instances Signal 1': [],\n", @@ -709,7 +715,7 @@ "}\n", "\n", "# Loop through the files and calculate the required metrics\n", - "for path, threshold in zip(file_paths, thresholds):\n", + "for path, key in zip(file_paths, keys):\n", " # Load the data\n", " df = pd.read_csv(path)\n", " \n", @@ -731,7 +737,7 @@ " fully_present_signal_2 = (df['num_errors_signal_2'] == 0).sum() / total_entries * 100\n", "\n", " # Append the data to the summary\n", - " summary_data['Threshold'].append(threshold)\n", + " summary_data['Key'].append(key)\n", " summary_data['Total Errors Signal 1'].append(total_errors_signal_1)\n", " summary_data['Total Errors Signal 2'].append(total_errors_signal_2)\n", " summary_data['Total Instances Signal 1'].append(total_signal_1)\n", @@ -742,7 +748,7 @@ " summary_data['Percentage Fully Present Signal 2'].append(fully_present_signal_2)\n", "\n", "# After looping, calculate performance differences and optimization metrics\n", - "for i in range(len(summary_data['Threshold'])):\n", + "for i in range(len(summary_data['Key'])):\n", " error_rate_diff = summary_data['Error Rate Signal 2'][i] - summary_data['Error Rate Signal 1'][i]\n", " 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\n", " combined_error_rate = summary_data['Error Rate Signal 1'][i] + summary_data['Error Rate Signal 2'][i]\n", @@ -757,11 +763,11 @@ "df_summary = pd.DataFrame(summary_data)\n", "\n", "# Find the optimal threshold based on combined error rate and max error rate\n", - "optimal_combined_threshold = df_summary.loc[df_summary['Combined Error Rate'].idxmin(), 'Threshold']\n", - "optimal_max_threshold = df_summary.loc[df_summary['Max Error Rate'].idxmin(), 'Threshold']\n", + "optimal_combined_threshold = df_summary.loc[df_summary['Combined Error Rate'].idxmin(), 'Key']\n", + "optimal_max_threshold = df_summary.loc[df_summary['Max Error Rate'].idxmin(), 'Key']\n", "\n", "# Save the summary to a CSV file\n", - "df_summary.to_csv('threshold_analysis_summary_dynamic.csv', index=False)\n", + "df_summary.to_csv('threshold_analysis_summary_adaptive.csv', index=False)\n", "\n", "# Output the optimal thresholds\n", "print(f'Optimal Threshold based on Combined Error Rate: {optimal_combined_threshold}')\n", @@ -5924,7 +5930,7 @@ }, { "cell_type": "code", - "execution_count": 139, + "execution_count": null, "metadata": {}, "outputs": [ { @@ -5958,7 +5964,7 @@ "## Real world\n", "\n", "# Calculating the overall presence of each signal in both datasets\n", - "overall_presence_adaptive = data_otsu_adaptive[['signal_0','signal_1', 'signal_3']].mean()\n", + "overall_presence_adaptive = 3[['signal_0','signal_1', 'signal_3']].mean()\n", "overall_presence_standard = data_standard[['signal_0','signal_1', 'signal_3']].mean()\n", "overall_presence_adaptive_kl = data_kl_adaptive[['signal_0','signal_1', 'signal_3']].mean()\n", "\n", diff --git a/scripts/batch_summary.py b/scripts/batch_summary.py index 59d4216..e4c7aa1 100755 --- a/scripts/batch_summary.py +++ b/scripts/batch_summary.py @@ -16,8 +16,14 @@ # '/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', +# '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_adaptive/variant01_adaptive_otsu_valid_standard_4_15_5_processed.csv', +# '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_adaptive/variant01_adaptive_otsu_sanity_check_processed.csv', +# '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_adaptive/variant01_otsu_valid_4_5_15_no_merging_processed.csv', # ] - +#adaptive variant02 +file_paths = [ + '/home/rivermar/Desktop/MRS_Master_Project/paper/processed_csv_automation_adaptive/variant02_adaptive_otsu_first_processed.csv', + ] #Variant01 # # Define the file paths and thresholds # file_paths = [ @@ -42,26 +48,27 @@ # '/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', -] +##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'] +# 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', 'valid_and_standards_4_5_15', 'consistency_check', 'no-merging'] +keys = ['first_variant02'] # Initialize a dataframe to hold the summarized data summary_data = { - # 'Key': [], - 'Threshold': [], + 'Key': [], + # 'Threshold': [], 'Total Errors Signal 1': [], 'Total Errors Signal 2': [], 'Total Instances Signal 1': [], @@ -81,8 +88,8 @@ } # 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): +for path, key in zip(file_paths, keys): +# for path, threshold in zip(file_paths, thresholds): # Load the data df = pd.read_csv(path) @@ -112,8 +119,8 @@ 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['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) @@ -128,8 +135,8 @@ 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'])): +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] @@ -144,13 +151,13 @@ 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'] +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) +df_summary.to_csv('threshold_analysis_summary_adaptive.csv', index=False) # Output the optimal thresholds print(f'Optimal Threshold based on Combined Error Rate: {optimal_combined_threshold}') diff --git a/scripts/extract_all.sh b/scripts/extract_all.sh index 68b3999..7419fc9 100755 --- a/scripts/extract_all.sh +++ b/scripts/extract_all.sh @@ -1,10 +1,10 @@ #!/bin/bash # Mapping file path -mapping_file="file_mapping_adaptive_variant01.csv" +mapping_file="file_mapping_adaptive_variant02.csv" # Output directory for CSV files -output_csv_dir=~/Desktop/MRS_Master_Project/paper/adaptive_raw_csv +output_csv_dir=~/Desktop/MRS_Master_Project/paper/adaptive_variant02_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 index 32ebb3c..bf50d81 100644 --- a/scripts/file_mapping_adaptive_variant01.csv +++ b/scripts/file_mapping_adaptive_variant01.csv @@ -1 +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 +/home/rivermar/bag_files/paper_rosbags/new_launchfile/adaptive_variant02/variant02_adaptive_otsu_first.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/file_mapping_adaptive_variant02.csv b/scripts/file_mapping_adaptive_variant02.csv new file mode 100644 index 0000000..8773bdd --- /dev/null +++ b/scripts/file_mapping_adaptive_variant02.csv @@ -0,0 +1 @@ +/home/rivermar/bag_files/paper_rosbags/new_launchfile/adaptive_variant02/variant02_adaptive_otsu_first.bag,/home/rivermar/bag_files/datasets_08_2024/uav39_rosbags/77_2024_08_20_14_22_02variant2/_2024-08-20-14-23-37.bag,/home/rivermar/bag_files/datasets_08_2024/uav40_rosbags_tx/38_2024_08_20_14_22_15variant2/_2024-08-20-14-23-36.bag diff --git a/scripts/new_processing_raw_csv.py b/scripts/new_processing_raw_csv.py index 26b814d..40b1600 100755 --- a/scripts/new_processing_raw_csv.py +++ b/scripts/new_processing_raw_csv.py @@ -15,7 +15,7 @@ variant = args.variant # e.g., 1 # Paths -input_csv_path = os.path.expanduser('~/Desktop/MRS_Master_Project/paper/adaptive_raw_csv/' + name_experiment + '.csv') +input_csv_path = os.path.expanduser('~/Desktop/MRS_Master_Project/paper/adaptive_variant02_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 diff --git a/scripts/process_all.sh b/scripts/process_all.sh index 81b0869..89d6ed5 100755 --- a/scripts/process_all.sh +++ b/scripts/process_all.sh @@ -1,7 +1,7 @@ #!/bin/bash # Directories -raw_csv_dir=~/Desktop/MRS_Master_Project/paper/adaptive_raw_csv +raw_csv_dir=~/Desktop/MRS_Master_Project/paper/adaptive_variant02_raw_csv processed_csv_dir=~/Desktop/MRS_Master_Project/paper/processed_csv_automation_adaptive # Make sure the processed_csv directory exists diff --git a/scripts/test_rosbag.yml b/scripts/test_rosbag.yml index aa74e3a..8fa4755 100644 --- a/scripts/test_rosbag.yml +++ b/scripts/test_rosbag.yml @@ -35,7 +35,7 @@ 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 ~/bag_files/datasets_08_2024/uav36_rosbags_rx/marlon/69_2024_08_20_14_12_22variant1/_2024-08-20-14-14-09.bag + - waitForRos; sleep 3; rosbag play --clock ~/bag_files/datasets_08_2024/uav36_rosbags_rx/marlon/70_2024_08_20_14_22_18variant2/_2024-08-20-14-23-37.bag - blinkers-seen: layout: even-vertical panes: diff --git a/scripts/threshold_analysis_summary_adaptive.csv b/scripts/threshold_analysis_summary_adaptive.csv index 40f2eda..6af42d0 100644 --- a/scripts/threshold_analysis_summary_adaptive.csv +++ b/scripts/threshold_analysis_summary_adaptive.csv @@ -1,10 +1,2 @@ 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 +first_variant02,26239,5771,70347,48098,0.3729938732284248,0.11998419892719032,45.23180841264968,76.10684679152594,62.70061267715752,88.00158010728096,90.2834919660219,86.63801043905434,0.4929780721556151,0.3729938732284248,-0.2530096743012345,-67.83212606451825 diff --git a/src/detector.cpp b/src/detector.cpp index 1a2810c..8d439a0 100644 --- a/src/detector.cpp +++ b/src/detector.cpp @@ -58,6 +58,10 @@ class UVDARDetector : public nodelet::Nodelet{ 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); + param_loader.loadParam("sigma_x",_sigma_x_, 6.0); + param_loader.loadParam("sigma_y",_sigma_y_, 6.0); + param_loader.loadParam("grayscale_roi_weight",_grayscale_roi_weight_, 0.25); + param_loader.loadParam("sharped_roi_weight",_sharped_roi_weight_, 1.75); //Print adaptive threshold @@ -148,7 +152,11 @@ class UVDARDetector : public nodelet::Nodelet{ _adaptive_debug_, _contours_size_limit_, _contour_max_size_limit_, - _roi_detected_points_limit_ + _roi_detected_points_limit_, + _sigma_x_, + _sigma_y_, + _grayscale_roi_weight_, + _sharped_roi_weight_ )); if (!uvda_.back()){ ROS_ERROR("[UVDARDetector]: Failed to initialize ADAPTIVE-based marker detection!"); @@ -776,6 +784,10 @@ class UVDARDetector : public nodelet::Nodelet{ int _contours_size_limit_; int _contour_max_size_limit_; int _roi_detected_points_limit_; + double _sigma_x_; + double _sigma_y_; + double _grayscale_roi_weight_; + double _sharped_roi_weight_; double _initial_delay_ = 5.0;