From 2369e6c85f06e2d7a757dcd65e2343f27e32f4ec Mon Sep 17 00:00:00 2001 From: Marlon Rivera Date: Sun, 20 Oct 2024 22:08:22 +0200 Subject: [PATCH] Processing adaptive only with valid feedback, positive values from blinkers callback --- include/detect/uv_led_detect_adaptive.cpp | 967 +++++++++++----------- include/detect/uv_led_detect_adaptive.h | 75 +- launch/rosbag.launch | 4 +- scripts/.tmuxinator.yml | 5 +- scripts/test_rosbag.yml | 5 +- src/detector.cpp | 45 +- 6 files changed, 555 insertions(+), 546 deletions(-) diff --git a/include/detect/uv_led_detect_adaptive.cpp b/include/detect/uv_led_detect_adaptive.cpp index 7420a71..797c077 100644 --- a/include/detect/uv_led_detect_adaptive.cpp +++ b/include/detect/uv_led_detect_adaptive.cpp @@ -6,166 +6,146 @@ #include #include -#include // Include for std::fixed and std::setprecision -#include // Include for std::tuple +#include // Include for std::fixed and std::setprecision +#include // Include for std::tuple #include #include #include - - namespace fs = std::filesystem; -struct PointComparator { - bool operator() (const cv::Point& a, const cv::Point& b) const { - return (a.x < b.x) || (a.x == b.x && a.y < b.y); - } +struct PointComparator +{ + bool operator()(const cv::Point& a, const cv::Point& b) const + { + return (a.x < b.x) || (a.x == b.x && a.y < b.y); + } }; -namespace uvdar { +namespace uvdar +{ -UVDARLedDetectAdaptive::UVDARLedDetectAdaptive(int neighborhoodSize, double point_similarity_threshold, std::string adaptive_method, bool adaptive_debug ) : neighborhoodSize_(neighborhoodSize), -point_similarity_threshold_(point_similarity_threshold), adaptive_method_(adaptive_method), adaptive_debug_(adaptive_debug){} + UVDARLedDetectAdaptive::UVDARLedDetectAdaptive(int neighborhoodSize, double point_similarity_threshold, std::string adaptive_method, bool adaptive_debug) + : neighborhoodSize_(neighborhoodSize), + point_similarity_threshold_(point_similarity_threshold), + adaptive_method_(adaptive_method), + adaptive_debug_(adaptive_debug) + { + } -UVDARLedDetectAdaptive::~UVDARLedDetectAdaptive() {} + UVDARLedDetectAdaptive::~UVDARLedDetectAdaptive() + { + } -/* processImageAdaptive //{ */ -bool UVDARLedDetectAdaptive::processImageAdaptive(const cv::Mat& inputImage, const std::vector& trackingPoints, std::vector& detectedPoints, -const std::vector& standardPoints) { + /* processImageAdaptive() //{ */ + bool UVDARLedDetectAdaptive::processImageAdaptive(const cv::Mat& inputImage, const std::vector& trackingPoints, + std::vector& detectedPoints, const std::vector& standardPoints) + { /** * @brief: This function processes the input image to detect UV-LEDs using adaptive thresholding - * + * * Args: * inputImage: The input image * trackingPoints: The points around which the adaptive thresholding is applied * detectedPoints: The detected points * standardPoints: The standard points - * + * * @returns: * True if the process is successful, False otherwise */ - - // Use a set to store unique points - std::set uniquePoints; - - std::string outputDir = "/home/rivermar/Desktop/MRS_Master_Project/pres_images"; - - // Ensure the output directory exists - fs::create_directories(outputDir); - - /* cv::imwrite(outputDir + "/im_" + std::to_string(index) + ".png", inputImage); */ - - index++; - // Reset detected points - detectedPoints.clear(); - lastProcessedBinaryROIs_.clear(); - lastProcessedROIs_.clear(); - //Reset the number of ROIs - numRois = 0; - numberDetectedPoints.clear(); - thresholdValue.clear(); - klDivergence.clear(); - validRoi.clear(); - + { + std::scoped_lock lock(mutex_viz_rois_); + // Reset detected points + detectedPoints.clear(); + lastProcessedBinaryROIs_.clear(); + lastProcessedROIs_.clear(); + } - //Print size of tracking points - //std::cout << "[UVDARLedDetectAdaptive]: TRACKING POINTS SIZE: " << trackingPoints.size() << std::endl; - + // Reset the number of ROIs + numRois_ = 0; + numberDetectedPoints_.clear(); + thresholdValues_.clear(); + klDivergences_.clear(); + validRois_.clear(); - if (trackingPoints.size() == 0 || trackingPoints.size() > 50) { - std::cout << "[UVDARLedDetectAdaptive]: INVALID NUMBER OF TRACKING POINTS" << std::endl; - return false; - } + // Avoid noisy images, to prevent adding overhead into the system if too many points are detected + if (trackingPoints.size() == 0 || trackingPoints.size() > 20) + { + std::cout << "[UVDARLedDetectAdaptive]: INVALID NUMBER OF TRACKING POINTS" << std::endl; + return false; + } std::vector rois; - for (const auto& point : trackingPoints) { - cv::Rect roi = calculateROI(inputImage, point, neighborhoodSize_); - rois.push_back(roi); + for (const auto& point : trackingPoints) + { + cv::Rect roi = calculateROI(inputImage, point, neighborhoodSize_); + rois.push_back(roi); } - if (adaptive_debug_){ - //Print the size of the rois - std::cout << "[UVDARLedDetectAdaptive]: SIZE OF ROIS: " << rois.size() << std::endl; + if (adaptive_debug_) + { + // Print the size of the rois + std::cout << "[UVDARLedDetectAdaptive]: SIZE OF ROIS: " << rois.size() << std::endl; } - std::vector mergedROIs = mergeOverlappingROIs(rois,inputImage.size(), 0.05); // Overlap threshold of 50% + const auto mergedROIs = mergeOverlappingROIs(rois, inputImage.size(), 0.05); // Overlap threshold of 5% - if (adaptive_debug_){ - //Print number of merged ROIs - std::cout << "[UVDARLedDetectAdaptive]: NUMBER OF MERGED ROIS: " << mergedROIs.size() << std::endl; + if (adaptive_debug_) + { + // Print number of merged ROIs + std::cout << "[UVDARLedDetectAdaptive]: NUMBER OF MERGED ROIS: " << mergedROIs.size() << std::endl; } - for (const auto& roi : mergedROIs) { - std::vector roiDetectedPoints = applyAdaptiveThreshold(inputImage, roi); - detectedPoints.insert(detectedPoints.end(), roiDetectedPoints.begin(), roiDetectedPoints.end()); + for (const auto& roi : mergedROIs) + { + auto roiDetectedPoints = applyAdaptiveThreshold(inputImage, roi); + // Save the detected points per ROI + detectedPoints.insert(detectedPoints.end(), roiDetectedPoints.begin(), roiDetectedPoints.end()); } - std::vector final_detected_points = mergePoints(detectedPoints, standardPoints, point_similarity_threshold_); - + const auto final_detected_points = mergePoints(detectedPoints, standardPoints, point_similarity_threshold_); + + // Updating the detected points detectedPoints = final_detected_points; return true; -} -//} + } + //} -/* applyAdaptiveThreshold //{ */ -std::vector UVDARLedDetectAdaptive::applyAdaptiveThreshold(const cv::Mat& inputImage, const cv::Rect& roi) { + /* applyAdaptiveThreshold() //{ */ + std::vector UVDARLedDetectAdaptive::applyAdaptiveThreshold(const cv::Mat& inputImage, const cv::Rect& roi) + { /** - * + * * @brief: This function applies adaptive thresholding to the neighborhood around a point and detects the points within the thresholded region - * + * * Args: * image: The input image * point: The point around which the adaptive thresholding is applied * neighborhoodSize: The size of the neighborhood around the point - * + * * @returns: * roiDetectedPoints: The points detected within the region of interest */ cv::Mat grayImage; - if (inputImage.channels() == 3) { - cv::cvtColor(inputImage(roi), grayImage, cv::COLOR_BGR2GRAY); - } else { - grayImage = inputImage(roi).clone(); - } - - 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); - } else { - grayImage = image.clone(); + if (inputImage.channels() == 3) + { + cv::cvtColor(inputImage(roi), grayImage, cv::COLOR_BGR2GRAY); + } else + { + grayImage = inputImage(roi).clone(); } - int x = std::max(0, point.x - neighborhoodSize); - int y = std::max(0, point.y - neighborhoodSize); - int width = std::min(neighborhoodSize * 2, image.cols - x); - int height = std::min(neighborhoodSize * 2, image.rows - y); - - cv::Rect roi(x, y, width, height); // Create a rectangle around the point - //lastProcessedROIs_.push_back(roi); // Store the ROI for visualization - //cv::Rect roi(point.x - neighborhoodSize, point.y - neighborhoodSize, 2 * neighborhoodSize, 2 * neighborhoodSize); - cv::Mat roiImage = grayImage(roi); // Extract the ROI from the grayscale image - //Copy the roiImage for comparison of threshold without blur - cv::Mat roiImageCopy = roiImage.clone(); - //saveRoiImage(roiImage, point, roiIndex_++, 0, 0.0); */ - - //Apply Gaussian blur to the ROI + // Apply Gaussian blur to the ROI cv::Mat blurred; double sigmaX = 6.0; double sigmaY = 6.0; @@ -178,92 +158,66 @@ std::vector UVDARLedDetectAdaptive::applyAdaptiveThreshold(const cv:: cv::Mat enhancedImage; cv::addWeighted(grayImage, 0.25, unsharpMask, 1.75, 0, enhancedImage); - - /* cv::Mat histEnhanced = plotHistogram(enhancedImage); */ - - // Specify the output directory - std::string outputDir = "/home/rivermar/Desktop/MRS_Master_Project/pres_images"; - - // Ensure the output directory exists - fs::create_directories(outputDir); - - + // BinaryRoi to save after applying the threshold cv::Mat binaryRoi; - 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; - auto [thresholdValue, minKLDivergence] = findOptimalThresholdUsingKL(enhancedImage); - thresholdValue_ = thresholdValue; - minKLDivergence_ = minKLDivergence; - cv::threshold(enhancedImage,binaryRoi, thresholdValue_, 255, cv::THRESH_BINARY); + /* cv::Mat binaryRoiOriginal; */ + if (adaptive_method_ == "Otsu" || adaptive_method_ == "otsu") + { + // Apply Otsu's thresholding with the enhanced ROI + double 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; + auto [thresholdValue, minKLDivergence] = findOptimalThresholdUsingKL(enhancedImage); + thresholdValue_ = thresholdValue; + 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> contours; cv::findContours(binaryRoi, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE); // Create a mask to draw the filtered contours cv::Mat mask = cv::Mat::zeros(binaryRoi.size(), CV_8UC1); - - if (adaptive_debug_){ - //Print the contours size - std::cout << "[UVDARLedDetectAdaptive]: NUMBER OF CONTOURS: " << contours.size() << std::endl; + if (adaptive_debug_) + { + // Print the contours size + std::cout << "[UVDARLedDetectAdaptive]: NUMBER OF CONTOURS: " << contours.size() << std::endl; } - if (contours.size() >= 4){ - //Return empty roiDetectedPoints - std::vector empty_roiDetectedPoints = {}; - return empty_roiDetectedPoints; - + // TODO find proper value for noisy ROI + if (contours.size() >= 6) + { + // Return empty roiDetectedPoints + std::vector empty_roiDetectedPoints = {}; + return empty_roiDetectedPoints; } - 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 (adaptive_debug_){ - std::cout << "[UVDARLedDetectAdaptive]: IN AREA: " << area << std::endl; - } - // Draw the contour on the mask - cv::drawContours(mask, std::vector>{contour}, -1, cv::Scalar(255), cv::FILLED); - }else{ - if (adaptive_debug_){ - std::cout << "[UVDARLedDetectAdaptive]: OUT OF AREA: " << area << std::endl; - } + // TODO find proper value for MAX_AREA + int MAX_AREA = 50; + + 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 (adaptive_debug_) + { + std::cout << "[UVDARLedDetectAdaptive]: IN AREA: " << area << std::endl; } + // Draw the contour on the mask + cv::drawContours(mask, std::vector>{contour}, -1, cv::Scalar(255), cv::FILLED); + } else if (adaptive_debug_) + { + std::cout << "[UVDARLedDetectAdaptive]: OUT OF AREA: " << area << std::endl; + } } // Apply the mask to the binary ROI @@ -271,119 +225,155 @@ std::vector UVDARLedDetectAdaptive::applyAdaptiveThreshold(const cv:: // Detect points within this ROI std::vector roiDetectedPoints = detectPointsFromRoi(binaryRoi, roi); - /* //Save the binary ROI */ - /* cv::imwrite(outputDir + "/im_" + std::to_string(index) + ".png", binaryRoi); */ - index++; - if (adaptive_debug_){ - //Print the number of detected points - std::cout << "[UVDARLedDetectAdaptive]: ROI NUMBER OF DETECTED POINTS: " << roiDetectedPoints.size() << std::endl; + if (adaptive_debug_) + { + // Print the number of detected points + std::cout << "[UVDARLedDetectAdaptive]: ROI NUMBER OF DETECTED POINTS: " << roiDetectedPoints.size() << std::endl; } - if (roiDetectedPoints.size() > 5){ + // TODO find the proper value of the detected points + // 5 - if (adaptive_debug_){ - //Print the number of detected points - std::cout << "[UVDARLedDetectAdaptive]: NOISY ROI: " << roiDetectedPoints.size() << std::endl; - } - //saveRoiImage(binaryRoi, point, roiIndex_++, thresholdValue, -1.0); - numberDetectedPoints.push_back(roiDetectedPoints.size()); - thresholdValue.push_back(thresholdValue_); - if(adaptive_method_ == "Otsu" || adaptive_method_ == "otsu"){ - klDivergence.push_back(0.0); + std::scoped_lock lock(mutex_viz_rois_); + { + if (roiDetectedPoints.size() > 4) + { + + if (adaptive_debug_) + { + // Print the number of detected points + std::cout << "[UVDARLedDetectAdaptive]: NOISY ROI: " << roiDetectedPoints.size() << std::endl; } - else{ - klDivergence.push_back(minKLDivergence_); + 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_); } - validRoi.push_back(0); - - //Return empty roiDetectedPoints + validRois_.push_back(0); + + // Return empty roiDetectedPoints std::vector empty_roiDetectedPoints = {}; return empty_roiDetectedPoints; - //Clear the lastProcessedROIs_ and lastProcessedBinaryROIs_ vectors + // Clear the lastProcessedROIs_ and lastProcessedBinaryROIs_ vectors lastProcessedROIs_.clear(); lastProcessedBinaryROIs_.clear(); - } - else{ - 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()); - thresholdValue.push_back(thresholdValue_); - if(adaptive_method_ == "Otsu" || adaptive_method_ == "otsu"){ - klDivergence.push_back(0.0); - } - else{ - klDivergence.push_back(minKLDivergence_); - } - validRoi.push_back(1); + } 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; + return roiDetectedPoints; + } } + } + //} -} -//} + /* adjustROI() //{ */ -cv::Rect UVDARLedDetectAdaptive::adjustROI(const cv::Rect& inputROI, const cv::Size& imageSize) { + cv::Rect UVDARLedDetectAdaptive::adjustROI(const cv::Rect& inputROI, const cv::Size& imageSize) + { int x = std::max(0, inputROI.x); int y = std::max(0, inputROI.y); int width = std::min(inputROI.width, imageSize.width - x); int height = std::min(inputROI.height, imageSize.height - y); return cv::Rect(x, y, width, height); -} + } + + //} -cv::Rect UVDARLedDetectAdaptive::calculateROI(const cv::Mat& image, const cv::Point& point, int neighborhoodSize) { + /* calculateROI() //{ */ + + cv::Rect UVDARLedDetectAdaptive::calculateROI(const cv::Mat& image, const cv::Point& point, int neighborhoodSize) + { int x = std::max(0, point.x - neighborhoodSize); int y = std::max(0, point.y - neighborhoodSize); + // To avoid going outside of the image bounds int width = std::min(neighborhoodSize * 2, image.cols - x); int height = std::min(neighborhoodSize * 2, image.rows - y); return cv::Rect(x, y, width, height); -} + } + + //} + + /* mergeOverlappingROIs() //{ */ -std::vector UVDARLedDetectAdaptive::mergeOverlappingROIs(const std::vector& rois, const cv::Size& imageSize, double overlapThreshold) { + std::vector UVDARLedDetectAdaptive::mergeOverlappingROIs(const std::vector& rois, const cv::Size& imageSize, double overlapThreshold) + { std::vector mergedROIs; std::vector merged(rois.size(), false); - for (int i = 0; i < rois.size(); i++) { - if (merged[i]) continue; - cv::Rect currentROI = rois[i]; - for (int j = i + 1; j < rois.size(); j++) { - if (merged[j]) continue; - if (isOverlapping(currentROI, rois[j], overlapThreshold)) { - std::vector corners; - corners.push_back(cv::Point(currentROI.x, currentROI.y)); - corners.push_back(cv::Point(currentROI.x + currentROI.width, currentROI.y + currentROI.height)); - corners.push_back(cv::Point(rois[j].x, rois[j].y)); - corners.push_back(cv::Point(rois[j].x + rois[j].width, rois[j].y + rois[j].height)); - // Calculate the bounding rectangle of these corners - currentROI = cv::boundingRect(corners); - merged[j] = true; - } + for (size_t i = 0; i < rois.size(); i++) + { + if (merged[i]) + continue; + cv::Rect currentROI = rois[i]; + for (size_t j = i + 1; j < rois.size(); j++) + { + if (merged[j]) + continue; + if (isOverlapping(currentROI, rois[j], overlapThreshold)) + { + std::vector corners; + corners.push_back(cv::Point(currentROI.x, currentROI.y)); + corners.push_back(cv::Point(currentROI.x + currentROI.width, currentROI.y + currentROI.height)); + corners.push_back(cv::Point(rois[j].x, rois[j].y)); + corners.push_back(cv::Point(rois[j].x + rois[j].width, rois[j].y + rois[j].height)); + // Calculate the bounding rectangle of the corners + currentROI = cv::boundingRect(corners); + merged[j] = true; } + } - currentROI = adjustROI(currentROI, imageSize); - mergedROIs.push_back(currentROI); - merged[i] = true; + // To check if it's not out of bounds + currentROI = adjustROI(currentROI, imageSize); + mergedROIs.push_back(currentROI); + merged[i] = true; } return mergedROIs; -} + } -bool UVDARLedDetectAdaptive::isOverlapping(const cv::Rect& roi1, const cv::Rect& roi2, double overlapThreshold) { + //} + + /* isOverlapping() //{ */ + + bool UVDARLedDetectAdaptive::isOverlapping(const cv::Rect& roi1, const cv::Rect& roi2, double overlapThreshold) + { cv::Rect intersection = roi1 & roi2; double iou = double(intersection.area()) / (roi1.area() + roi2.area() - intersection.area()); return iou > overlapThreshold; -} + } + //} -/* adjustNeighborhoodSizeBasedOnArea //{ */ -int UVDARLedDetectAdaptive::adjustNeighborhoodSizeBasedOnArea(const cv::Mat& roiImage, const std::vector>& contours, int currentSize) { + /* adjustNeighborhoodSizeBasedOnArea() //{ */ + int UVDARLedDetectAdaptive::adjustNeighborhoodSizeBasedOnArea(const cv::Mat& roiImage, const std::vector>& contours, int currentSize) + { double totalArea = 0; - for (const auto& contour : contours) { - totalArea += cv::contourArea(contour); + for (const auto& contour : contours) + { + totalArea += cv::contourArea(contour); } // Print the total area @@ -397,85 +387,90 @@ int UVDARLedDetectAdaptive::adjustNeighborhoodSizeBasedOnArea(const cv::Mat& roi std::cout << "[UVDARLedDetectAdaptive]: AREA RATIO: " << areaRatio << std::endl; const double targetAreaRatioSparse = 0.1, targetAreaRatioDense = 0.5; - double adjustmentFactor = 0.5; // Proportion of currentSize to adjust by, needs tuning + double adjustmentFactor = 0.5; // Proportion of currentSize to adjust by, needs tuning - if (areaRatio < targetAreaRatioSparse) { - // For sparse features, decrease size + if (areaRatio < targetAreaRatioSparse) + { + // For sparse features, decrease size - return std::max(static_cast(currentSize * (1 - adjustmentFactor)), MIN_SIZE); - } else if (areaRatio > targetAreaRatioDense) { + return std::max(static_cast(currentSize * (1 - adjustmentFactor)), MIN_SIZE); + } else if (areaRatio > targetAreaRatioDense) + { - // For dense features, increase size - return std::min(static_cast(currentSize * (1 + adjustmentFactor)), MAX_SIZE); + // For dense features, increase size + return std::min(static_cast(currentSize * (1 + adjustmentFactor)), MAX_SIZE); } - return currentSize; -} -//} + return currentSize; + } + //} -/* detectPointsFromRoi //{ */ -std::vector UVDARLedDetectAdaptive::detectPointsFromRoi(const cv::Mat& mask, const cv::Rect& roi) { + /* detectPointsFromRoi() //{ */ + std::vector UVDARLedDetectAdaptive::detectPointsFromRoi(const cv::Mat& mask, const cv::Rect& roi) + { /** - * @brief: This function detects points from the mask and returns the centroid of the detected points - * - * Args: - * mask: The binary mask of the ROI, where the blobs are expected to be detected. - * roi: The rectangle of interest, used to adjust the coordinates of the detected points to the full image coordinate system. - * - * @returns: - * points: The centroid of the detected points - */ + * @brief: This function detects points from the mask and returns the centroid of the detected points + * + * Args: + * mask: The binary mask of the ROI, where the blobs are expected to be detected. + * roi: The rectangle of interest, used to adjust the coordinates of the detected points to the full image coordinate system. + * + * @returns: + * points: The centroid of the detected points + */ std::vector points; - // Use connected components to find blobs in the mask cv::Mat labels; int numComponents = cv::connectedComponents(mask, labels, 8, CV_32S); // Iterate through components to calculate centroid - for (int i = 1; i < numComponents; i++) { // Start from 1 to ignore the background - cv::Mat componentMask = (labels == i); - // Calculate the centroid of the component - cv::Moments m = cv::moments(componentMask, true); - cv::Point centroid(static_cast(m.m10 / m.m00) + roi.x, static_cast(m.m01 / m.m00) + roi.y); - points.push_back(centroid); + for (int i = 1; i < numComponents; i++) + { // Start from 1 to ignore the background + cv::Mat componentMask = (labels == i); + // Calculate the centroid of the component + cv::Moments m = cv::moments(componentMask, true); + cv::Point centroid(static_cast(m.m10 / m.m00) + roi.x, static_cast(m.m01 / m.m00) + roi.y); + points.push_back(centroid); } return points; -} -//} - -/* isClose //{ */ -bool UVDARLedDetectAdaptive::isClose(const cv::Point& p1, const cv::Point& p2, double threshold) { -/** -* @brief: This function checks if two points are close to each other -* -* Args: -* p1: The first point -* p2: The second point -* threshold: The maximum distance between the two points for them to be considered close -* -* @returns: -* True if the points are close, False otherwise -*/ + } + //} + + /* isClose() //{ */ + bool UVDARLedDetectAdaptive::isClose(const cv::Point& p1, const cv::Point& p2, double threshold) + { + /** + * @brief: This function checks if two points are close to each other + * + * Args: + * p1: The first point + * p2: The second point + * threshold: The maximum distance between the two points for them to be considered close + * + * @returns: + * True if the points are close, False otherwise + */ double dist = cv::norm(p1 - p2); return dist < threshold; -} -//} + } + //} -/* mergePoints //{ */ -std::vector UVDARLedDetectAdaptive::mergePoints(const std::vector& adaptivePoints, - const std::vector& standardPoints, double threshold) { + /* mergePoints() //{ */ + std::vector UVDARLedDetectAdaptive::mergePoints(const std::vector& adaptivePoints, const std::vector& standardPoints, + double threshold) + { /** * @brief: This function merges the adaptive points with the standard points, ensuring that no two points are too close to each other - * + * * Args: * adaptivePoints: The points detected using adaptive thresholding * standardPoints: The standard points * threshold: The maximum distance between two points for them to be considered close - * + * * @returns: * combinedPoints: The merged points */ @@ -486,87 +481,88 @@ std::vector UVDARLedDetectAdaptive::mergePoints(const std::vector& segmentHist, - const std::vector& overallHist, - size_t limit) { // Add a 'limit' parameter to restrict calculations - - /** - * @brief: This function calculates the Kullback-Leibler divergence between two distributions - * - * Args: - * segmentHist: The histogram of the segment - * overallHist: The histogram of the overall image - * limit: The index up to which the KL divergence should be calculated - * - * @returns: - * klDivergence: The Kullback-Leibler divergence - */ - - double klDivergence = 0.0; - double epsilon = 1e-10; // Small value to avoid log(0) - for (size_t i = 0; i < overallHist.size(); ++i) { - // Make sure we only calculate where both distributions have positive values - if (segmentHist[i] > 0 && overallHist[i] > 0) { - klDivergence += overallHist[i] * log(overallHist[i] / segmentHist[i] + epsilon); - } - } - return klDivergence; + } + //} + + /* calculateKLDivergence() //{ */ + // Function to calculate KL divergence + double UVDARLedDetectAdaptive::calculateKLDivergence(const std::vector& segmentHist, const std::vector& overallHist, size_t limit) + { // Add a 'limit' parameter to restrict calculations + + /** + * @brief: This function calculates the Kullback-Leibler divergence between two distributions + * + * Args: + * segmentHist: The histogram of the segment + * overallHist: The histogram of the overall image + * limit: The index up to which the KL divergence should be calculated + * + * @returns: + * klDivergence: The Kullback-Leibler divergence + */ -} + double klDivergence = 0.0; + double epsilon = 1e-10; // Small value to avoid log(0) + for (size_t i = 0; i < overallHist.size(); ++i) + { + // Make sure we only calculate where both distributions have positive values + if (segmentHist[i] > 0 && overallHist[i] > 0) + { + klDivergence += overallHist[i] * log(overallHist[i] / segmentHist[i] + epsilon); + } + } + return klDivergence; + } -double UVDARLedDetectAdaptive::calculateEntropy(const std::vector& histSegment) { + double UVDARLedDetectAdaptive::calculateEntropy(const std::vector& histSegment) + { double entropy = 0.0; - for (double p : histSegment) { - if (p > 0) { // Only calculate for positive probabilities to avoid log(0) - entropy -= p * log(p); // Using natural log - } + for (double p : histSegment) + { + if (p > 0) + { // Only calculate for positive probabilities to avoid log(0) + entropy -= p * log(p); // Using natural log + } } return entropy; -} + } + -/* findOptimalThresholdUsingKL */ -std::tuple UVDARLedDetectAdaptive::findOptimalThresholdUsingKL(const cv::Mat& roiImage) { + //} + + /* findOptimalThresholdUsingKL() //{ */ + std::tuple UVDARLedDetectAdaptive::findOptimalThresholdUsingKL(const cv::Mat& roiImage) + { /** * @brief: This function finds the optimal threshold for adaptive thresholding using Kullback-Leibler divergence - * + * * Args: * roiImage: The region of interest - * + * * @returns: * optimalThreshold: The optimal threshold */ - - int countZeros = cv::countNonZero(roiImage); - int totalPixels = roiImage.total(); - int countOnes = totalPixels - countZeros; - - - // Calculate the histogram of the ROI image int histSize = 256; float range[] = {0, 256}; @@ -575,61 +571,59 @@ std::tuple UVDARLedDetectAdaptive::findOptimalThresholdUsingKL(cons cv::calcHist(&roiImage, 1, 0, cv::Mat(), hist, 1, &histSize, &histRange, true, false); cv::normalize(hist, hist, 1, 0, cv::NORM_L1, -1, cv::Mat()); - - double minKLDivergence = std::numeric_limits::max(); int optimalThreshold = 0; - + std::vector Q(histSize); - for (int i = 0; i < histSize; ++i) { - Q[i] = hist.at(i); + for (int i = 0; i < histSize; ++i) + { + Q[i] = hist.at(i); } - double epsilon = 1e-10; - - std::vector P_below(histSize, 0), P_above(Q); // Use the full Q for P_above initially + std::vector P_below(histSize, 0), P_above(Q); // Use the full Q for P_above initially double sumBelow = 0.0, sumAbove = std::accumulate(Q.begin(), Q.end(), 0.0); - for (int t = 1; t < histSize - 1; ++t) { - sumBelow += Q[t - 1]; - sumAbove -= Q[t - 1]; - P_below[t] = sumBelow; // Track cumulative sum below the threshold - - if (sumBelow == 0 || sumAbove == 0) continue; // Skip invalid cases - - // Normalize probabilities up to the current threshold - std::vector normalizedP_below(P_below.begin(), P_below.begin() + t + 1); - std::vector normalizedP_above(P_above.begin() + t + 1, P_above.end()); - - for (auto& val : normalizedP_below) val /= sumBelow; - for (auto& val : normalizedP_above) val /= sumAbove; - - - double klDivBelow = calculateKLDivergence(normalizedP_below, Q, t + 1); - double klDivAbove = calculateKLDivergence(normalizedP_above, Q, histSize - t - 1); - - - double totalKLDiv = klDivBelow + klDivAbove; - - if (totalKLDiv < minKLDivergence && totalKLDiv > 0.0) { - - - minKLDivergence = totalKLDiv; - optimalThreshold = t; - } - - + for (int t = 1; t < histSize - 1; ++t) + { + sumBelow += Q[t - 1]; + sumAbove -= Q[t - 1]; + P_below[t] = sumBelow; // Track cumulative sum below the threshold + + if (sumBelow == 0 || sumAbove == 0) + continue; // Skip invalid cases + + // Normalize probabilities up to the current threshold + std::vector normalizedP_below(P_below.begin(), P_below.begin() + t + 1); + std::vector normalizedP_above(P_above.begin() + t + 1, P_above.end()); + for (auto& val : normalizedP_below) + val /= sumBelow; + for (auto& val : normalizedP_above) + val /= sumAbove; + + double klDivBelow = calculateKLDivergence(normalizedP_below, Q, t + 1); + double klDivAbove = calculateKLDivergence(normalizedP_above, Q, histSize - t - 1); + double totalKLDiv = klDivBelow + klDivAbove; + + if (totalKLDiv < minKLDivergence && totalKLDiv > 0.0) + { + minKLDivergence = totalKLDiv; + optimalThreshold = t; + } } - return std::make_tuple(optimalThreshold, minKLDivergence); + } + + //} -} -//} + /* findOptimalThresholdUsingEntropy() //{ */ -std::tuple UVDARLedDetectAdaptive::findOptimalThresholdUsingEntropy(const cv::Mat& roiImage) { - if (roiImage.empty()) { - throw std::runtime_error("Input image is empty."); + + std::tuple UVDARLedDetectAdaptive::findOptimalThresholdUsingEntropy(const cv::Mat& roiImage) + { + if (roiImage.empty()) + { + throw std::runtime_error("Input image is empty."); } // Calculate the histogram as before @@ -640,103 +634,111 @@ std::tuple UVDARLedDetectAdaptive::findOptimalThresholdUsingEntropy cv::calcHist(&roiImage, 1, 0, cv::Mat(), hist, 1, &histSize, &histRange, true, false); cv::normalize(hist, hist, 1, 0, cv::NORM_L1, -1, cv::Mat()); - if (cv::sum(hist)[0] == 0) { - throw std::runtime_error("Histogram normalization failed."); + if (cv::sum(hist)[0] == 0) + { + throw std::runtime_error("Histogram normalization failed."); } double maxEntropy = std::numeric_limits::min(); int optimalThreshold = 0; std::vector Q(histSize); - for (int i = 0; i < histSize; ++i) { - Q[i] = hist.at(i); + for (int i = 0; i < histSize; ++i) + { + Q[i] = hist.at(i); } - for (int t = 1; t < histSize - 1; ++t) { - std::vector P_below(Q.begin(), Q.begin() + t); - std::vector P_above(Q.begin() + t, Q.end()); + for (int t = 1; t < histSize - 1; ++t) + { + std::vector P_below(Q.begin(), Q.begin() + t); + std::vector P_above(Q.begin() + t, Q.end()); - double entropyBelow = calculateEntropy(P_below); - double entropyAbove = calculateEntropy(P_above); - double totalEntropy = entropyBelow + entropyAbove; + double entropyBelow = calculateEntropy(P_below); + double entropyAbove = calculateEntropy(P_above); + double totalEntropy = entropyBelow + entropyAbove; - if (totalEntropy > maxEntropy) { - maxEntropy = totalEntropy; - optimalThreshold = t; - } + if (totalEntropy > maxEntropy) + { + maxEntropy = totalEntropy; + optimalThreshold = t; + } } return std::make_tuple(optimalThreshold, maxEntropy); -} + } + -/* generateVisualizationAdaptive //{ */ -void UVDARLedDetectAdaptive::generateVisualizationAdaptive(const cv::Mat& inputImage,cv::Mat& visualization_image, const std::vector& detectedPoints) { + //} + + /* generateVisualizationAdaptive() //{ */ + void UVDARLedDetectAdaptive::generateVisualizationAdaptive(const cv::Mat& inputImage, cv::Mat& visualization_image, + const std::vector& detectedPoints) + { /** * @brief: This function generates a visualization of the detected points - * + * * Args: * inputImage: The input image * visualization_image: The output visualization image - * + * * @returns: * None - */ + */ // Create a copy of the current image for visualization - //3the a black background with the same size as the input image - - visualization_image = inputImage.clone(); - + // 3the a black background with the same size as the input image + visualization_image = inputImage.clone(); // Check if the image is not empty - if (visualization_image.empty()) { - return; + if (visualization_image.empty()) + { + return; } + + std::scoped_lock lock(mutex_viz_rois_); // Overlay binary ROIs - for (size_t i = 0; i < lastProcessedBinaryROIs_.size(); i++) { - const auto& binaryRoi = lastProcessedBinaryROIs_[i]; - const auto& roi = lastProcessedROIs_[i]; + for (size_t i = 0; i < lastProcessedBinaryROIs_.size(); i++) + { + const auto& binaryRoi = lastProcessedBinaryROIs_[i]; + const auto& roi = lastProcessedROIs_[i]; - // Ensure the ROI is within the bounds of the image - cv::Rect imageBounds(0, 0, visualization_image.cols, visualization_image.rows); - if (imageBounds.contains(cv::Point(roi.x, roi.y)) && - imageBounds.contains(cv::Point(roi.x + roi.width, roi.y + roi.height))) { + // Ensure the ROI is within the bounds of the image + cv::Rect imageBounds(0, 0, visualization_image.cols, visualization_image.rows); + if (imageBounds.contains(cv::Point(roi.x, roi.y)) && imageBounds.contains(cv::Point(roi.x + roi.width, roi.y + roi.height))) + { - // Overlay the binary ROI - binaryRoi.copyTo(visualization_image(roi)); + // Overlay the binary ROI + binaryRoi.copyTo(visualization_image(roi)); - // Optionally draw a rectangle around the ROI - cv::rectangle(visualization_image, roi, cv::Scalar(0, 255, 0)); // Green rectangle - - } + // Optionally draw a rectangle around the ROI + cv::rectangle(visualization_image, roi, cv::Scalar(0, 255, 0)); // Green rectangle + } } - - + } + //} + /* plotHistogram() //{ */ -} -//} - - -cv::Mat UVDARLedDetectAdaptive::plotHistogram(const cv::Mat& image) { + cv::Mat UVDARLedDetectAdaptive::plotHistogram(const cv::Mat& image) + { // Compute the histogram int histSize = 256; - float range[] = { 0, 256 }; - const float* histRange = { range }; + 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) + // 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 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)); @@ -746,33 +748,33 @@ cv::Mat UVDARLedDetectAdaptive::plotHistogram(const cv::Mat& image) { cv::minMaxLoc(hist, 0, &maxVal); // Draw the histogram - for (int i = 0; i < histSize; i++) { - int binVal = std::round(hist.at(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); + for (int i = 0; i < histSize; i++) + { + int binVal = std::round(hist.at(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 //{ */ + /* saveRoiImage() //{ */ -void UVDARLedDetectAdaptive::saveRoiImage(const cv::Mat& binaryRoi, const cv::Point& center, int index, int thresholdValue = 0, double minKLDivergence = 0.0) { + void UVDARLedDetectAdaptive::saveRoiImage(const cv::Mat& binaryRoi, const cv::Point& center, int index, int thresholdValue = 0, double minKLDivergence = 0.0) + { /** * @brief: This function saves the binary ROI as an image - * + * * Args: * binaryRoi: The binary ROI * center: The center of the ROI * index: The index of the ROI * thresholdValue: The threshold value used for adaptive thresholding - * + * * @returns: * None */ @@ -782,41 +784,42 @@ void UVDARLedDetectAdaptive::saveRoiImage(const cv::Mat& binaryRoi, const cv::Po // Ensure the output directory exists fs::create_directories(outputDir); - // Convert minKLDivergence to string with fixed decimal places + // Convert minKLDivergence to string with fixed decimal places std::stringstream minKLDivStream; - minKLDivStream << std::fixed << std::setprecision(2) << minKLDivergence; // Adjust precision as needed + minKLDivStream << std::fixed << std::setprecision(2) << minKLDivergence; // Adjust precision as needed // Format the filename - std::string filename = "roi_" + std::to_string(center.x) + "_" + std::to_string(center.y) + "_" + std::to_string(index) + "_" + std::to_string(thresholdValue) + "_" + minKLDivStream.str() + ".png"; + std::string filename = "roi_" + std::to_string(center.x) + "_" + std::to_string(center.y) + "_" + std::to_string(index) + "_" + + std::to_string(thresholdValue) + "_" + minKLDivStream.str() + ".png"; std::string fullPath = fs::path(outputDir) / filename; // Save the image - /* cv::imwrite(fullPath, binaryRoi); */ -} -//} - + cv::imwrite(fullPath, binaryRoi); + } + //} -/* prepareAdaptiveDataForLogging //{ */ -ROIData UVDARLedDetectAdaptive::prepareAdaptiveDataForLogging() { + /* prepareAdaptiveDataForLogging() //{ */ + ROIData UVDARLedDetectAdaptive::prepareAdaptiveDataForLogging() + { /** * @brief: This function prepares the adaptive data for logging - * + * * Args: * None - * + * * @returns: * A tuple containing the number of detected points, the threshold value, the KL divergence, and the validity of the ROI */ - + ROIData adaptiveData; - adaptiveData.numRois = numRois; - adaptiveData.numberDetectedPoints = numberDetectedPoints; - adaptiveData.thresholdValue = thresholdValue; - adaptiveData.klDivergence = klDivergence; - adaptiveData.validRoi = validRoi; + adaptiveData.numRois = numRois_; + adaptiveData.numberDetectedPoints = numberDetectedPoints_; + adaptiveData.thresholdValues = thresholdValues_; + adaptiveData.klDivergences = klDivergences_; + adaptiveData.validRois = validRois_; return adaptiveData; -} + } -} // namespace uvdar +} // namespace uvdar diff --git a/include/detect/uv_led_detect_adaptive.h b/include/detect/uv_led_detect_adaptive.h index a28c3dc..f635089 100644 --- a/include/detect/uv_led_detect_adaptive.h +++ b/include/detect/uv_led_detect_adaptive.h @@ -6,48 +6,54 @@ #include #include -#include +#include #include #include -#include // Include for std::fixed and std::setprecision -#include // Include for std::tuple +#include // Include for std::fixed and std::setprecision +#include // Include for std::tuple #include #include #include -struct ROIData{ - int numRois; - std::vector numberDetectedPoints; - std::vector thresholdValue; - std::vector klDivergence; - std::vector validRoi; +struct ROIData +{ + int numRois; + std::vector numberDetectedPoints; + std::vector thresholdValues; + std::vector klDivergences; + std::vector validRois; }; -namespace uvdar { +namespace uvdar +{ -class UVDARLedDetectAdaptive{ -public: - UVDARLedDetectAdaptive(int neighborhoodSize = 25, double point_similarity_threshold = 5.0, std::string adaptive_method = "Otsu", bool adaptive_debug = false); + class UVDARLedDetectAdaptive + { + public: + UVDARLedDetectAdaptive(int neighborhoodSize = 25, double point_similarity_threshold = 5.0, std::string adaptive_method = "Otsu", + bool adaptive_debug = false); ~UVDARLedDetectAdaptive(); - bool processImageAdaptive(const cv::Mat& inputImage, const std::vector& trackingPoints, std::vector& detectedPoints, const std::vector& standardPoints); + bool processImageAdaptive(const cv::Mat& inputImage, const std::vector& trackingPoints, std::vector& detectedPoints, + const std::vector& standardPoints); - void generateVisualizationAdaptive(const cv::Mat& inputImage,cv::Mat& visualization_image,const std::vector& detectedPoints); + void generateVisualizationAdaptive(const cv::Mat& inputImage, cv::Mat& visualization_image, const std::vector& detectedPoints); ROIData prepareAdaptiveDataForLogging(); - const std::vector& getLastProcessedBinaryROIs() const { + const std::vector& getLastProcessedBinaryROIs() const + { return lastProcessedBinaryROIs_; } - const std::vector& getLastProcessedROIs() const { + const std::vector& getLastProcessedROIs() const + { return lastProcessedROIs_; } -private: - + private: std::vector applyAdaptiveThreshold(const cv::Mat& inputImage, const cv::Rect& roi); cv::Rect adjustROI(const cv::Rect& inputROI, const cv::Size& imageSize); @@ -63,7 +69,7 @@ class UVDARLedDetectAdaptive{ int adjustNeighborhoodSizeBasedOnArea(const cv::Mat& roiImage, const std::vector>& contours, int currentSize); - std::vector mergePoints(const std::vector& adaptivePoints,const std::vector& standardPoints, double threshold); + std::vector mergePoints(const std::vector& adaptivePoints, const std::vector& standardPoints, double threshold); double calculateKLDivergence(const std::vector& segmentHist, const std::vector& overallHist, size_t limit); double calculateEntropy(const std::vector& histSegment); @@ -76,9 +82,9 @@ class UVDARLedDetectAdaptive{ void saveRoiImage(const cv::Mat& binaryRoi, const cv::Point& center, int index, int thresholdValue, double klDivergence); int neighborhoodSize_; - int thresholdValue_; + double thresholdValue_; double minKLDivergence_; - + double point_similarity_threshold_; std::string adaptive_method_; bool adaptive_debug_; @@ -86,19 +92,18 @@ class UVDARLedDetectAdaptive{ int MAX_SIZE = 50; int MIN_SIZE = 5; std::vector lastProcessedBinaryROIs_; - std::vector lastProcessedROIs_; // For storing ROI positions - std::vector allRoiDetectedPoints;// For storing ROI positions + std::vector lastProcessedROIs_; // For storing ROI positions + std::vector allRoiDetectedPoints; // For storing ROI positions + std::mutex mutex_viz_rois_; - int numRois; - std::vector numberDetectedPoints; - std::vector thresholdValue; - std::vector klDivergence; - std::vector validRoi; - int index = 0; - int index_g = 0; - -}; + int numRois_; + std::vector numberDetectedPoints_; + std::vector thresholdValues_; + std::vector klDivergences_; + std::vector validRois_; + int index_ = 0; // index to label saved pictures + }; -} // namespace uvdar +} // namespace uvdar -#endif // UV_LED_DETECT_ADAPTIVE_H +#endif // UV_LED_DETECT_ADAPTIVE_H diff --git a/launch/rosbag.launch b/launch/rosbag.launch index 0337983..589001d 100644 --- a/launch/rosbag.launch +++ b/launch/rosbag.launch @@ -13,7 +13,7 @@ - + @@ -133,7 +133,7 @@ - + diff --git a/scripts/.tmuxinator.yml b/scripts/.tmuxinator.yml index 1685c88..bab718a 100644 --- a/scripts/.tmuxinator.yml +++ b/scripts/.tmuxinator.yml @@ -20,7 +20,7 @@ windows: layout: even-vertical panes: # - waitForOffBoard; 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 - - waitForRos; waitForOffBoard; ./record.sh + # - waitForRos; waitForOffBoard; ./record.sh #- 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" @@ -35,11 +35,10 @@ 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/71_2024_08_20_14_35_41variant3/_2024-08-20-14-37-28.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: layout: even-vertical panes: - # - waitForRos; sleep 3; rqt_image_view - waitForRos; sleep 3; rostopic echo /uav36/uvdar/blinkers_seen_left diff --git a/scripts/test_rosbag.yml b/scripts/test_rosbag.yml index 1685c88..bab718a 100644 --- a/scripts/test_rosbag.yml +++ b/scripts/test_rosbag.yml @@ -20,7 +20,7 @@ windows: layout: even-vertical panes: # - waitForOffBoard; 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 - - waitForRos; waitForOffBoard; ./record.sh + # - waitForRos; waitForOffBoard; ./record.sh #- 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" @@ -35,11 +35,10 @@ 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/71_2024_08_20_14_35_41variant3/_2024-08-20-14-37-28.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: layout: even-vertical panes: - # - waitForRos; sleep 3; rqt_image_view - waitForRos; sleep 3; rostopic echo /uav36/uvdar/blinkers_seen_left diff --git a/src/detector.cpp b/src/detector.cpp index db29231..fde3136 100644 --- a/src/detector.cpp +++ b/src/detector.cpp @@ -350,8 +350,12 @@ class UVDARDetector : public nodelet::Nodelet{ for (const auto& point : msg->points) { //cv::Point cvPoint(static_cast(point.x), static_cast(point.y)); //Without casting - cv::Point cvPoint(point.x, point.y); - trackingPointsPerCamera[image_index].push_back(cvPoint); + 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); + } } ROS_INFO_STREAM("[UVDARDetector]: Camera " << image_index << " Tracking points: " << trackingPointsPerCamera[image_index].size()); @@ -376,7 +380,6 @@ class UVDARDetector : public nodelet::Nodelet{ } //} - /* processStandard //{ */ void processStandard(const cv_bridge::CvImageConstPtr& image, int image_index){ /** @@ -467,15 +470,14 @@ class UVDARDetector : public nodelet::Nodelet{ msg_adaptive.stamp = image->header.stamp; msg_adaptive.num_rois = adaptiveData.numRois; msg_adaptive.roi_detected_points = adaptiveData.numberDetectedPoints; - msg_adaptive.roi_threshold_used = adaptiveData.thresholdValue; - msg_adaptive.roi_kl_divergence = adaptiveData.klDivergence; - msg_adaptive.roi_is_valid = adaptiveData.validRoi; + msg_adaptive.roi_threshold_used = adaptiveData.thresholdValues; + msg_adaptive.roi_kl_divergence = adaptiveData.klDivergences; + msg_adaptive.roi_is_valid = adaptiveData.validRois; pub_adaptive_logging_[image_index].publish(msg_adaptive); } //} - /* publishStandard //{ */ void publishStandard(const cv_bridge::CvImageConstPtr& image, int image_index, const std::vector& detected_points) { @@ -539,7 +541,6 @@ class UVDARDetector : public nodelet::Nodelet{ if( _adaptive_threshold_ && trackingPointsPerCamera[image_index].size() > 0){ ROS_INFO_STREAM("[UVDARDetector]: Tracking points per camera: " << trackingPointsPerCamera[image_index].size()); - received_tracking_points_ = true; { @@ -565,8 +566,9 @@ class UVDARDetector : public nodelet::Nodelet{ } else{ - - ROS_INFO_STREAM("[UVDARDetector]: No tracking points for camera " << 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 << "..."); */ @@ -589,7 +591,7 @@ class UVDARDetector : public nodelet::Nodelet{ processStandard(image, image_index); - } + } } @@ -633,21 +635,23 @@ class UVDARDetector : public nodelet::Nodelet{ void VisualizationThread([[maybe_unused]] const ros::TimerEvent& te) { if (initialized_){ cv::Mat visualization_image; - if(_adaptive_threshold_){ + + if (_adaptive_threshold_) { int image_index = 0; - 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); + 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); } - //generateVisualization(image_visualization_); - - if ((visualization_image.cols != 0) && (visualization_image.rows != 0)){ + + //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, visualization_image, true); + pub_visualization_->publish("uvdar_detection_visualization", 0.01, image_visualization_, true); } if (_gui_){ - cv::imshow("ocv_uvdar_detection_" + _uav_name_, visualization_image); + cv::imshow("ocv_uvdar_detection_" + _uav_name_, image_visualization_); cv::waitKey(25); } } @@ -780,7 +784,6 @@ class UVDARDetector : public nodelet::Nodelet{ std::vector> adaptive_detected_points_; std::vector> combinedPoints_; - bool received_tracking_points_ = false; std::vector ROI_data; };