bug correction

test/src/QRcode.cpp

@@ -96,6 +96,24 @@ cv::Mat enhanceImage(const cv::Mat& image)
     return enhanced;
 }
 
+cv::Mat equalizeHistogram(const cv::Mat& image)
+{
+    cv::Mat equalized;
+    cv::cvtColor(image, equalized, cv::COLOR_BGR2GRAY);
+    cv::equalizeHist(equalized, equalized);
+    cv::cvtColor(equalized, equalized, cv::COLOR_GRAY2BGR);
+    return equalized;
+}
+
+cv::Mat adaptiveThresholding(const cv::Mat& image)
+{
+    cv::Mat gray, thresholded;
+    cv::cvtColor(image, gray, cv::COLOR_BGR2GRAY);
+    cv::adaptiveThreshold(gray, thresholded, 255, cv::ADAPTIVE_THRESH_GAUSSIAN_C, cv::THRESH_BINARY, 11, 2);
+    cv::cvtColor(thresholded, thresholded, cv::COLOR_GRAY2BGR);
+    return thresholded;
+}
+
 void processFrame(const cv::Mat& frame)
 {
     // Apply image processing steps
@@ -104,6 +122,8 @@ void processFrame(const cv::Mat& frame)
     processedFrame = upscaleImage(processedFrame, 3.0);
     processedFrame = reduceNoise(processedFrame);
     processedFrame = enhanceImage(processedFrame);
+    processedFrame = equalizeHistogram(processedFrame);
+    processedFrame = adaptiveThresholding(processedFrame);
 
     // Create ArUco marker detector
     cv::Ptr<cv::aruco::Dictionary> dictionary = 

test/src/arenaCalibration.cpp

@@ -76,6 +76,24 @@ cv::Mat enhanceImage(const cv::Mat& image)
     return enhanced;
 }
 
+cv::Mat equalizeHistogram(const cv::Mat& image)
+{
+    cv::Mat equalized;
+    cv::cvtColor(image, equalized, cv::COLOR_BGR2GRAY);
+    cv::equalizeHist(equalized, equalized);
+    cv::cvtColor(equalized, equalized, cv::COLOR_GRAY2BGR);
+    return equalized;
+}
+
+cv::Mat adaptiveThresholding(const cv::Mat& image)
+{
+    cv::Mat gray, thresholded;
+    cv::cvtColor(image, gray, cv::COLOR_BGR2GRAY);
+    cv::adaptiveThreshold(gray, thresholded, 255, cv::ADAPTIVE_THRESH_GAUSSIAN_C, cv::THRESH_BINARY, 11, 2);
+    cv::cvtColor(thresholded, thresholded, cv::COLOR_GRAY2BGR);
+    return thresholded;
+}
+
 std::map<int, cv::Point3f> fixedMarkerPositions;
 
 void loadMarkerPositions(const std::string& filename)
@@ -83,6 +101,10 @@ void loadMarkerPositions(const std::string& filename)
     try
     {
         YAML::Node config = YAML::LoadFile(filename);
+        if (!config["markers"])
+        {
+            throw std::runtime_error("No 'markers' node found in the configuration file.");
+        }
         for (const auto& marker : config["markers"])
         {
             int id = marker["id"].as<int>();
@@ -94,6 +116,14 @@ void loadMarkerPositions(const std::string& filename)
     {
         ROS_ERROR("Failed to load marker positions: %s", e.what());
     }
+    catch (const std::runtime_error& e)
+    {
+        ROS_ERROR("Error in configuration file: %s", e.what());
+    }
+    catch (const std::exception& e)
+    {
+        ROS_ERROR("Unexpected error: %s", e.what());
+    }
 }
 
 void calibrateArena()
@@ -114,7 +144,7 @@ void calibrateArena()
 
     // Set the desired resolution
     cap.set(cv::CAP_PROP_FRAME_WIDTH, 1920); // Set width to 1920 pixels
-    cap.set<cv::CAP_PROP_FRAME_HEIGHT, 1080); // Set height to 1080 pixels
+    cap.set(cv::CAP_PROP_FRAME_HEIGHT, 1080); // Set height to 1080 pixels
 
     // Set the desired frame rate
     cap.set(cv::CAP_PROP_FPS, 15); // Set frame rate to 15 Hz
@@ -142,6 +172,8 @@ void calibrateArena()
         frame = upscaleImage(frame, 3.0);
         frame = reduceNoise(frame);
         frame = enhanceImage(frame);
+        frame = equalizeHistogram(frame);
+        frame = adaptiveThresholding(frame);
 
         // Detect markers in the image
         cv::aruco::detectMarkers(frame, dictionary, markerCorners, markerIds);
@@ -240,6 +272,9 @@ int main(int argc, char** argv)
     fs["distortion_coefficients"] >> distCoeffs;
     fs.release();
 
+    // Load marker positions
+    loadMarkerPositions("arena_config.yml");
+
     calibrateArena();
 
     return 0;