test

test/CMakeLists.txt

@@ -37,9 +37,11 @@ include_directories(
 ## Declare a C++ executable
 add_executable(qr_code_detector src/QRcode.cpp)
 add_executable(calibrationWebcam src/calibrationWebcam.cpp)
+add_executable(arenaCalibration src/arenaCalibration.cpp)
 
 ## Add cmake target dependencies of the executable
 add_dependencies(qr_code_detector ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(arenaCalibration ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 ## Specify libraries to link a library or executable target against
 target_link_libraries(qr_code_detector
@@ -50,6 +52,10 @@ target_link_libraries(calibrationWebcam
   ${catkin_LIBRARIES}
   ${OpenCV_LIBRARIES}
 )
+target_link_libraries(arenaCalibration
+  ${catkin_LIBRARIES}
+  ${OpenCV_LIBRARIES}
+)
 
 #############
 ## Install ##

test/arena_config.yml

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

test/src/QRcode.cpp

@@ -5,8 +5,21 @@
 #include <opencv2/imgproc/imgproc.hpp>
 #include <opencv2/calib3d.hpp> 
 #include <thread>
+#include <yaml-cpp/yaml.h>
 
 cv::Mat cameraMatrix, distCoeffs;
+std::map<int, cv::Point2f> markerPositions;
+
+void loadMarkerPositions(const std::string& filename)
+{
+    YAML::Node config = YAML::LoadFile(filename);
+    for (const auto& marker : config["markers"])
+    {
+        int id = marker["id"].as<int>();
+        std::vector<float> pos = marker["position"].as<std::vector<float>>();
+        markerPositions[id] = cv::Point2f(pos[0], pos[1]);
+    }
+}
 
 void processFrame(const cv::Mat& frame)
 {
@@ -54,6 +67,17 @@ void processFrame(const cv::Mat& frame)
             // Estimate the coordinates of the marker in the arena
             cv::Mat markerPosition = cv::Mat(tvecs[i]);
             ROS_INFO_STREAM("Marker Position in Arena: " << markerPosition);
+
+            // Retrieve the marker position from the configuration
+            if (markerPositions.find(markerIds[i]) != markerPositions.end())
+            {
+                cv::Point2f arenaPosition = markerPositions[markerIds[i]];
+                ROS_INFO_STREAM("Marker Position in Arena: " << arenaPosition);
+            }
+            else
+            {
+                ROS_WARN("Marker ID %d not found in configuration", markerIds[i]);
+            }
         }
     }
     else
@@ -112,6 +136,9 @@ int main(int argc, char** argv)
     fs["distortion_coefficients"] >> distCoeffs;
     fs.release();
 
+    // Load marker positions
+    loadMarkerPositions("/catkin_ws/arena_config.yml");
+
     // Start frame processing thread
     std::thread processingThread(frameProcessingThread);
 

test/src/arenaCalibration.cpp

@@ -6,9 +6,24 @@
 #include <cv_bridge/cv_bridge.h>
 #include <vector>
 #include <thread>
+#include <yaml-cpp/yaml.h>
 
 cv::Mat cameraMatrix, distCoeffs;
 
+void saveMarkerPositions(const std::string& filename, const std::map<int, cv::Point2f>& markerPositions)
+{
+    YAML::Node config;
+    for (const auto& marker : markerPositions)
+    {
+        YAML::Node markerNode;
+        markerNode["id"] = marker.first;
+        markerNode["position"] = std::vector<float>{marker.second.x, marker.second.y};
+        config["markers"].push_back(markerNode);
+    }
+    std::ofstream fout(filename);
+    fout << config;
+}
+
 void calibrateArena()
 {
     // Create ArUco marker detector
@@ -33,6 +48,8 @@ void calibrateArena()
     cap.set(cv::CAP_PROP_FPS, 15); // Set frame rate to 15 Hz
 
     cv::Mat frame;
+    std::map<int, cv::Point2f> markerPositions;
+
     while (ros::ok())
     {
         cap >> frame; // Capture a new frame
@@ -76,6 +93,9 @@ void calibrateArena()
                 eulerAngles[2] = atan2(rotationMatrix.at<double>(1, 0), rotationMatrix.at<double>(0, 0)); // Yaw
 
                 ROS_INFO_STREAM("Euler Angles (Roll, Pitch, Yaw): " << eulerAngles);
+
+                // Save the marker position
+                markerPositions[markerIds[i]] = cv::Point2f(tvecs[i][0], tvecs[i][1]);
             }
         }
         else
@@ -88,6 +108,9 @@ void calibrateArena()
         if (cv::waitKey(30) == 27) break; // Exit on ESC key
     }
 
+    // Save the marker positions to the configuration file
+    saveMarkerPositions("/catkin_ws/arena_config.yml", markerPositions);
+
     // Save the transformation to a configuration file
     cv::FileStorage fs("arena_transformation.yml", cv::FileStorage::WRITE);
     fs << "camera_matrix" << cameraMatrix;