From 61be3dd2f89d6e4576b05512ebbdd16cb60b4b30 Mon Sep 17 00:00:00 2001 From: Thomas Bonnier Date: Fri, 13 Dec 2024 09:03:08 +0100 Subject: [PATCH] test --- README.md | 52 ++++++++++++++++++++--- test/src/QRcode.cpp | 12 +++++- test/src/arenaCalibration.cpp | 78 +++++++++++++++++++++++++++++++--- test/src/calibrationWebcam.cpp | 3 ++ 4 files changed, 133 insertions(+), 12 deletions(-) diff --git a/README.md b/README.md index 2847bd1..63598df 100644 --- a/README.md +++ b/README.md @@ -1,10 +1,52 @@ -This is the README for the EU_Robot of Group D. - -// ...existing content... - ## Complete Calibration and Detection Procedure ### Step 1: Setup Configuration Files 1. Create `arena_config.yml` with fixed marker positions: - \ No newline at end of file + ```yaml + markers: + - id: 1 + position: [0, 0] + - id: 2 + position: [0, 1] + - id: 3 + position: [0, 2] + ``` + +2. Create `arena_transformation.yml` with an initial transformation matrix (example identity matrix): + ```yaml + transformation_matrix: !!opencv-matrix + rows: 3 + cols: 3 + dt: d + data: [1, 0, 0, 0, 1, 0, 0, 0, 1] + ``` + +### Step 2: Calibrate the Camera + +1. Run the `calibration_webcam` node to capture frames and calibrate the camera: + ```bash + rosrun your_package calibration_webcam + ``` + +2. Follow the on-screen instructions to capture at least 10 frames of the chessboard pattern from different angles and positions. + +3. The calibration parameters will be saved to `camera_parameters.yml`. + +### Step 3: Calibrate the Arena + +1. Run the `arena_calibration` node to detect ArUco markers and calibrate the arena: + ```bash + rosrun your_package arena_calibration + ``` + +2. The detected marker positions and transformation matrix will be saved to `arena_config.yml` and `arena_transformation.yml` respectively. + +### Step 4: Run the Main Node + +1. Run the `aruco_detector` node to detect ArUco markers and display their positions in the arena: + ```bash + rosrun your_package aruco_detector + ``` + +2. The node will process frames from the camera, detect ArUco markers, and display their positions in the arena frame. diff --git a/test/src/QRcode.cpp b/test/src/QRcode.cpp index a8e7181..5c70839 100644 --- a/test/src/QRcode.cpp +++ b/test/src/QRcode.cpp @@ -87,6 +87,15 @@ cv::Mat gammaCorrection(const cv::Mat& image, double gamma) return adjusted; } +cv::Mat enhanceImage(const cv::Mat& image) +{ + cv::Mat enhanced; + cv::Mat kernel = (cv::Mat_(3, 3) << 1, 1, 1, 1, -7, 1, 1, 1, 1); + cv::filter2D(image, enhanced, CV_32F, kernel); + cv::normalize(enhanced, enhanced, 0, 255, cv::NORM_MINMAX, CV_8UC1); + return enhanced; +} + void processFrame(const cv::Mat& frame) { // Apply image processing steps @@ -94,6 +103,7 @@ void processFrame(const cv::Mat& frame) processedFrame = sharpenImage(processedFrame); processedFrame = upscaleImage(processedFrame, 3.0); processedFrame = reduceNoise(processedFrame); + processedFrame = enhanceImage(processedFrame); // Create ArUco marker detector cv::Ptr dictionary = @@ -144,7 +154,7 @@ void processFrame(const cv::Mat& frame) std::vector cameraPoints = {cv::Point2f(tvecs[i][0], tvecs[i][1])}; std::vector arenaPoints; cv::perspectiveTransform(cameraPoints, arenaPoints, transformationMatrix); - ROS_INFO_STREAM("Marker Position in Arena Frame: " << arenaPoints[0]); + ROS_INFO_STREAM("Marker ID: " << markerIds[i] << " Position in Arena Frame: [" << arenaPoints[0].x << ", " << arenaPoints[0].y << "]"); } } else diff --git a/test/src/arenaCalibration.cpp b/test/src/arenaCalibration.cpp index a69cc01..1bb9290 100644 --- a/test/src/arenaCalibration.cpp +++ b/test/src/arenaCalibration.cpp @@ -32,6 +32,70 @@ void saveTransformationMatrix(const std::string& filename, const cv::Mat& transf fs.release(); } +cv::Mat upscaleImage(const cv::Mat& image, double scaleFactor) +{ + cv::Mat upscaled; + cv::resize(image, upscaled, cv::Size(), scaleFactor, scaleFactor, cv::INTER_LINEAR); + return upscaled; +} + +cv::Mat sharpenImage(const cv::Mat& image) +{ + cv::Mat sharpened; + cv::Mat sharpeningKernel = (cv::Mat_(3, 3) << 0, -1, 0, -1, 5, -1, 0, -1, 0); + cv::filter2D(image, sharpened, -1, sharpeningKernel); + return sharpened; +} + +cv::Mat reduceNoise(const cv::Mat& image) +{ + cv::Mat denoised; + cv::GaussianBlur(image, denoised, cv::Size(5, 5), 0); + return denoised; +} + +cv::Mat gammaCorrection(const cv::Mat& image, double gamma) +{ + cv::Mat adjusted; + cv::Mat lookupTable(1, 256, CV_8U); + uchar* p = lookupTable.ptr(); + for (int i = 0; i < 256; ++i) + { + p[i] = cv::saturate_cast(pow(i / 255.0, gamma) * 255.0); + } + cv::LUT(image, lookupTable, adjusted); + return adjusted; +} + +cv::Mat enhanceImage(const cv::Mat& image) +{ + cv::Mat enhanced; + cv::Mat kernel = (cv::Mat_(3, 3) << 1, 1, 1, 1, -7, 1, 1, 1, 1); + cv::filter2D(image, enhanced, CV_32F, kernel); + cv::normalize(enhanced, enhanced, 0, 255, cv::NORM_MINMAX, CV_8UC1); + return enhanced; +} + +std::map fixedMarkerPositions; + +void loadMarkerPositions(const std::string& filename) +{ + try + { + YAML::Node config = YAML::LoadFile(filename); + for (const auto& marker : config["markers"]) + { + int id = marker["id"].as(); + std::vector pos = marker["position"].as>(); + fixedMarkerPositions[id] = cv::Point2f(pos[0], pos[1]); + } + } + catch (const YAML::BadFile& e) + { + ROS_ERROR("Failed to load marker positions: %s", e.what()); + } +} + void calibrateArena() { // Create ArUco marker detector @@ -59,12 +123,7 @@ void calibrateArena() std::map markerPositions; cv::Mat transformationMatrix; - std::map fixedMarkerPositions = { - {1, cv::Point2f(0, 0)}, - {2, cv::Point2f(0, 1)}, - {3, cv::Point2f(1, 0)}, - {4, cv::Point2f(1, 1)} - }; + loadMarkerPositions("arena_config.yml"); while (ros::ok()) { @@ -75,6 +134,13 @@ void calibrateArena() continue; } + // Apply image processing steps + frame = gammaCorrection(frame, 0.4); + frame = sharpenImage(frame); + frame = upscaleImage(frame, 3.0); + frame = reduceNoise(frame); + frame = enhanceImage(frame); + // Detect markers in the image cv::aruco::detectMarkers(frame, dictionary, markerCorners, markerIds); diff --git a/test/src/calibrationWebcam.cpp b/test/src/calibrationWebcam.cpp index 3cf5d8c..fbcb09e 100644 --- a/test/src/calibrationWebcam.cpp +++ b/test/src/calibrationWebcam.cpp @@ -26,6 +26,7 @@ int main(int argc, char** argv) cv::Mat frame, gray; int frameCount = 0; + ros::Rate rate(1); // Capture 1 frame per second while (ros::ok()) { cap >> frame; @@ -53,6 +54,8 @@ int main(int argc, char** argv) cv::imshow("Calibration", frame); if (cv::waitKey(30) == 27) break; // Exit on ESC key + + rate.sleep(); // Sleep to maintain 1 frame per second } if (imagePoints.size() < 10)