add a break when calibration is done

test/arena_config.yml

@@ -1,8 +1,10 @@
 # filepath: /catkin_ws/arena_config.yml
 markers:
-  - id: 1
-    position: [0, 0]
-  - id: 2
-    position: [0, 1]
-  - id: 3
-    position: [0, 2]
+  - id: 5
+    position: [0, 0, 0]
+  - id: 6
+    position: [1, 0, 0]
+  - id: 7
+    position: [1, 1, 0]
+  - id: 8
+    position: [0, 1, 0]

test/src/arenaCalibration.cpp

@@ -76,7 +76,7 @@ cv::Mat enhanceImage(const cv::Mat& image)
     return enhanced;
 }
 
-std::map<int, cv::Point2f> fixedMarkerPositions;
+std::map<int, cv::Point3f> fixedMarkerPositions;
 
 void loadMarkerPositions(const std::string& filename)
 {
@@ -87,7 +87,7 @@ void loadMarkerPositions(const std::string& filename)
         {
             int id = marker["id"].as<int>();
             std::vector<float> pos = marker["position"].as<std::vector<float>>();
-            fixedMarkerPositions[id] = cv::Point2f(pos[0], pos[1]);
+            fixedMarkerPositions[id] = cv::Point3f(pos[0], pos[1], pos[2]);
         }
     }
     catch (const YAML::BadFile& e)
@@ -114,7 +114,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
@@ -125,7 +125,9 @@ void calibrateArena()
 
     loadMarkerPositions("arena_config.yml");
 
-    while (ros::ok())
+    bool calibrationDone = false;
+
+    while (ros::ok() && !calibrationDone)
     {
         cap >> frame; // Capture a new frame
         if (frame.empty())
@@ -183,7 +185,8 @@ void calibrateArena()
             // Compute the transformation matrix (example using homography)
             if (markerIds.size() >= 3) // Need at least 3 points to compute homography
             {
-                std::vector<cv::Point2f> srcPoints, dstPoints;
+                std::vector<cv::Point2f> srcPoints;
+                std::vector<cv::Point3f> dstPoints;
                 for (const auto& marker : markerPositions)
                 {
                     if (fixedMarkerPositions.find(marker.first) != fixedMarkerPositions.end())
@@ -195,6 +198,7 @@ void calibrateArena()
                 if (srcPoints.size() >= 3)
                 {
                     transformationMatrix = cv::findHomography(srcPoints, dstPoints);
+                    calibrationDone = true;
                 }
             }
         }
@@ -208,13 +212,16 @@ void calibrateArena()
         if (cv::waitKey(30) == 27) break; // Exit on ESC key
     }
 
-    // Save the marker positions to the configuration file
-    saveMarkerPositions("arena_config.yml", markerPositions);
-
-    // 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");
+    if (calibrationDone)
+    {
+        // 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");
+    }
+    else
+    {
+        ROS_ERROR("Calibration failed. Not enough markers detected.");
+    }
 }
 
 int main(int argc, char** argv)