test for next session

README.md

@@ -4,6 +4,8 @@ This is the README for the EU_Robot of Group D.
 
 ## Calibration Instructions
 
+### Webcam Calibration
+
 To calibrate the webcam using a chessboard pattern:
 
 1. Ensure you have a printed chessboard pattern with 9x6 inner corners.

test/arena_config.yml

@@ -0,0 +1,8 @@
+# filepath: /c:/Users/thoma/Nextcloud/Cours/EENG4/Coupe_de_France/EU_Robot_Group-D/arena_config.yml
+markers:
+  - id: 1
+    position: [0, 0]
+  - id: 2
+    position: [0, 1]
+  - id: 3
+    position: [0, 2]

test/src/QRcode.cpp

@@ -50,6 +50,10 @@ void processFrame(const cv::Mat& frame)
             eulerAngles[2] = atan2(rotationMatrix.at<double>(1, 0), rotationMatrix.at<double>(0, 0)); // Yaw
 
             ROS_INFO_STREAM("Euler Angles (Roll, Pitch, Yaw): " << eulerAngles);
+
+            // Estimate the coordinates of the marker in the arena
+            cv::Mat markerPosition = cv::Mat(tvecs[i]);
+            ROS_INFO_STREAM("Marker Position in Arena: " << markerPosition);
         }
     }
     else

test/src/arenaCalibration.cpp

@@ -0,0 +1,119 @@
+#include <ros/ros.h>
+#include <opencv2/aruco.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/calib3d.hpp>
+#include <cv_bridge/cv_bridge.h>
+#include <vector>
+#include <thread>
+
+cv::Mat cameraMatrix, distCoeffs;
+
+void calibrateArena()
+{
+    // Create ArUco marker detector
+    cv::Ptr<cv::aruco::Dictionary> dictionary = 
+        cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50); 
+    std::vector<int> markerIds;
+    std::vector<std::vector<cv::Point2f>> markerCorners;
+    std::vector<cv::Vec3d> rvecs, tvecs;
+
+    cv::VideoCapture cap(0, cv::CAP_V4L2); // Open the default camera with V4L2 backend
+    if (!cap.isOpened())
+    {
+        ROS_ERROR("Unable to open camera");
+        return;
+    }
+
+    // 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
+
+    // Set the desired frame rate
+    cap.set(cv::CAP_PROP_FPS, 15); // Set frame rate to 15 Hz
+
+    cv::Mat frame;
+    while (ros::ok())
+    {
+        cap >> frame; // Capture a new frame
+        if (frame.empty())
+        {
+            ROS_ERROR("Captured empty frame");
+            continue;
+        }
+
+        // Detect markers in the image
+        cv::aruco::detectMarkers(frame, dictionary, markerCorners, markerIds);
+
+        // Draw detected markers
+        if (!markerIds.empty())
+        {
+            cv::aruco::drawDetectedMarkers(frame, markerCorners, markerIds);
+            cv::aruco::estimatePoseSingleMarkers(markerCorners, 0.05, cameraMatrix, distCoeffs, rvecs, tvecs);
+            ROS_INFO("Detected %zu ArUco markers", markerIds.size());
+
+            for (size_t i = 0; i < markerIds.size(); ++i)
+            {
+                ROS_INFO("Marker ID: %d", markerIds[i]);
+                cv::aruco::drawAxis(frame, cameraMatrix, distCoeffs, rvecs[i], tvecs[i], 0.1);
+
+                // Print the rotation and translation vectors
+                ROS_INFO_STREAM("Rotation Vector: " << rvecs[i]);
+                ROS_INFO_STREAM("Translation Vector: " << tvecs[i]);
+
+                // Calculate the distance from the camera to the marker
+                double distance = cv::norm(tvecs[i]);
+                ROS_INFO("Distance from camera: %f meters", distance);
+
+                // Convert rotation vector to rotation matrix
+                cv::Mat rotationMatrix;
+                cv::Rodrigues(rvecs[i], rotationMatrix);
+
+                // Extract Euler angles from the rotation matrix
+                cv::Vec3d eulerAngles;
+                eulerAngles[0] = atan2(rotationMatrix.at<double>(2, 1), rotationMatrix.at<double>(2, 2)); // Roll
+                eulerAngles[1] = atan2(-rotationMatrix.at<double>(2, 0), sqrt(pow(rotationMatrix.at<double>(2, 1), 2) + pow(rotationMatrix.at<double>(2, 2), 2))); // Pitch
+                eulerAngles[2] = atan2(rotationMatrix.at<double>(1, 0), rotationMatrix.at<double>(0, 0)); // Yaw
+
+                ROS_INFO_STREAM("Euler Angles (Roll, Pitch, Yaw): " << eulerAngles);
+            }
+        }
+        else
+        {
+            ROS_INFO("No ArUco markers detected");
+        }
+
+        // Display the result
+        cv::imshow("Arena Calibration", frame);
+        if (cv::waitKey(30) == 27) break; // Exit on ESC key
+    }
+
+    // Save the transformation to a configuration file
+    cv::FileStorage fs("arena_transformation.yml", cv::FileStorage::WRITE);
+    fs << "camera_matrix" << cameraMatrix;
+    fs << "distortion_coefficients" << distCoeffs;
+    fs.release();
+
+    ROS_INFO("Arena calibration completed and transformation saved to arena_transformation.yml");
+}
+
+int main(int argc, char** argv)
+{
+    ros::init(argc, argv, "arena_calibration");
+    ros::NodeHandle nh;
+
+    // Load camera parameters
+    cv::FileStorage fs("camera_parameters.yml", cv::FileStorage::READ);
+    if (!fs.isOpened())
+    {
+        ROS_ERROR("Unable to open camera parameters file");
+        return -1;
+    }
+    fs["camera_matrix"] >> cameraMatrix;
+    fs["distortion_coefficients"] >> distCoeffs;
+    fs.release();
+
+    calibrateArena();
+
+    return 0;
+}