test3

test/arena_transformation.yml

@@ -0,0 +1,5 @@
+transformation_matrix: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [1, 0, 0, 0, 1, 0, 0, 0, 1] # Example identity matrix, replace with actual data

test/src/QRcode.cpp

@@ -9,16 +9,36 @@
 
 cv::Mat cameraMatrix, distCoeffs;
 std::map<int, cv::Point2f> markerPositions;
+cv::Mat transformationMatrix;
 
 void loadMarkerPositions(const std::string& filename)
 {
-    YAML::Node config = YAML::LoadFile(filename);
-    for (const auto& marker : config["markers"])
+    try
     {
-        int id = marker["id"].as<int>();
-        std::vector<float> pos = marker["position"].as<std::vector<float>>();
-        markerPositions[id] = cv::Point2f(pos[0], pos[1]);
+        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]);
+        }
     }
+    catch (const YAML::BadFile& e)
+    {
+        ROS_ERROR("Failed to load marker positions: %s", e.what());
+    }
+}
+
+void loadTransformationMatrix(const std::string& filename)
+{
+    cv::FileStorage fs(filename, cv::FileStorage::READ);
+    if (!fs.isOpened())
+    {
+        ROS_ERROR("Unable to open transformation matrix file");
+        return;
+    }
+    fs["transformation_matrix"] >> transformationMatrix;
+    fs.release();
 }
 
 void processFrame(const cv::Mat& frame)
@@ -71,8 +91,11 @@ void processFrame(const cv::Mat& frame)
             // 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);
+                cv::Point2f cameraPosition = markerPositions[markerIds[i]];
+                std::vector<cv::Point2f> cameraPoints = {cameraPosition};
+                std::vector<cv::Point2f> arenaPoints;
+                cv::perspectiveTransform(cameraPoints, arenaPoints, transformationMatrix);
+                ROS_INFO_STREAM("Marker Position in Arena: " << arenaPoints[0]);
             }
             else
             {
@@ -137,7 +160,10 @@ int main(int argc, char** argv)
     fs.release();
 
     // Load marker positions
-    loadMarkerPositions("/catkin_ws/arena_config.yml");
+    loadMarkerPositions("arena_config.yml");
+
+    // Load transformation matrix
+    loadTransformationMatrix("arena_transformation.yml");
 
     // Start frame processing thread
     std::thread processingThread(frameProcessingThread);

test/src/arenaCalibration.cpp

@@ -25,6 +25,13 @@ void saveMarkerPositions(const std::string& filename, const std::map<int, cv::Po
     fout << config;
 }
 
+void saveTransformationMatrix(const std::string& filename, const cv::Mat& transformationMatrix)
+{
+    cv::FileStorage fs(filename, cv::FileStorage::WRITE);
+    fs << "transformation_matrix" << transformationMatrix;
+    fs.release();
+}
+
 void calibrateArena()
 {
     // Create ArUco marker detector
@@ -50,6 +57,7 @@ void calibrateArena()
 
     cv::Mat frame;
     std::map<int, cv::Point2f> markerPositions;
+    cv::Mat transformationMatrix;
 
     while (ros::ok())
     {
@@ -98,6 +106,18 @@ void calibrateArena()
                 // Save the marker position
                 markerPositions[markerIds[i]] = cv::Point2f(tvecs[i][0], tvecs[i][1]);
             }
+
+            // Compute the transformation matrix (example using homography)
+            if (markerIds.size() >= 4) // Need at least 4 points to compute homography
+            {
+                std::vector<cv::Point2f> srcPoints, dstPoints;
+                for (const auto& marker : markerPositions)
+                {
+                    srcPoints.push_back(marker.second);
+                    dstPoints.push_back(cv::Point2f(marker.second.x, marker.second.y)); // Replace with actual arena coordinates
+                }
+                transformationMatrix = cv::findHomography(srcPoints, dstPoints);
+            }
         }
         else
         {
@@ -110,13 +130,10 @@ void calibrateArena()
     }
 
     // Save the marker positions to the configuration file
-    saveMarkerPositions("/catkin_ws/arena_config.yml", markerPositions);
+    saveMarkerPositions("arena_config.yml", markerPositions);
 
-    // 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();
+    // Save the transformation matrix to a configuration file
+    saveTransformationMatrix("arena_transformation.yml", transformationMatrix);
 
     ROS_INFO("Arena calibration completed and transformation saved to arena_transformation.yml");
 }