From ee764361f57bf824f8eaaa2cf4ae6f7054f8f3d3 Mon Sep 17 00:00:00 2001
From: Thomas Bonnier <thomas.bonnier@ecam.fr>
Date: Fri, 29 Nov 2024 14:23:10 +0100
Subject: [PATCH] calib2

---
 README.md                      | 12 +++++-
 test/src/calibrationWebcam.cpp | 75 ++++++++++++++++++----------------
 2 files changed, 51 insertions(+), 36 deletions(-)

diff --git a/README.md b/README.md
index 0819f8a..57d609e 100644
--- a/README.md
+++ b/README.md
@@ -1 +1,11 @@
-This is the README for the EU_Robot of Group D.
\ No newline at end of file
+This is the README for the EU_Robot of Group D.
+
+// ...existing content...
+
+## Calibration Instructions
+
+To calibrate the webcam using a chessboard pattern:
+
+1. Ensure you have a printed chessboard pattern with 9x6 inner corners.
+2. Run the `calibrationWebcam` node:
+   
\ No newline at end of file
diff --git a/test/src/calibrationWebcam.cpp b/test/src/calibrationWebcam.cpp
index 015fde6..ebef83a 100644
--- a/test/src/calibrationWebcam.cpp
+++ b/test/src/calibrationWebcam.cpp
@@ -1,73 +1,78 @@
+#include <ros/ros.h>
 #include <opencv2/opencv.hpp>
-#include <opencv2/calib3d.hpp>
-#include <opencv2/highgui.hpp>
-#include <opencv2/imgproc.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/calib3d/calib3d.hpp>
 #include <vector>
-#include <iostream>
 
-int main()
+int main(int argc, char** argv)
 {
-    // Define the number of inner corners per chessboard row and column
-    int boardWidth = 9;
-    int boardHeight = 6;
-    cv::Size boardSize(boardWidth, boardHeight);
+    ros::init(argc, argv, "calibration_webcam");
+    ros::NodeHandle nh;
 
-    // Prepare object points (0,0,0), (1,0,0), (2,0,0), ..., (boardWidth-1, boardHeight-1, 0)
+    cv::Size boardSize(9, 6); // Chessboard size
+    std::vector<std::vector<cv::Point2f>> imagePoints;
+    std::vector<std::vector<cv::Point3f>> objectPoints;
     std::vector<cv::Point3f> obj;
-    for (int i = 0; i < boardHeight; ++i)
-        for (int j = 0; j < boardWidth; ++j)
+    for (int i = 0; i < boardSize.height; ++i)
+        for (int j = 0; j < boardSize.width; ++j)
             obj.push_back(cv::Point3f(j, i, 0));
 
-    std::vector<std::vector<cv::Point3f>> objectPoints;
-    std::vector<std::vector<cv::Point2f>> imagePoints;
-    std::vector<cv::Point2f> corners;
-
-    cv::Mat frame, gray;
-    cv::VideoCapture cap(0); // Open the default camera
-
+    cv::VideoCapture cap(0);
     if (!cap.isOpened())
     {
-        std::cerr << "Unable to open camera" << std::endl;
+        ROS_ERROR("Unable to open camera");
         return -1;
     }
 
-    int numFrames = 0;
-    while (numFrames < 20) // Capture 20 frames
+    cv::Mat frame, gray;
+    int frameCount = 0;
+    while (ros::ok())
     {
         cap >> frame;
-        cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY);
+        if (frame.empty())
+        {
+            ROS_ERROR("Captured empty frame");
+            continue;
+        }
 
+        cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY);
+        std::vector<cv::Point2f> corners;
         bool found = cv::findChessboardCorners(gray, boardSize, corners,
-                                               cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_FAST_CHECK | cv::CALIB_CB_NORMALIZE_IMAGE);
+            cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_FAST_CHECK | cv::CALIB_CB_NORMALIZE_IMAGE);
 
         if (found)
         {
             cv::cornerSubPix(gray, corners, cv::Size(11, 11), cv::Size(-1, -1),
-                             cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::COUNT, 30, 0.1));
+                cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::COUNT, 30, 0.1));
             cv::drawChessboardCorners(frame, boardSize, corners, found);
             imagePoints.push_back(corners);
             objectPoints.push_back(obj);
-            numFrames++;
+            frameCount++;
+            ROS_INFO("Frame %d captured", frameCount);
         }
 
-        cv::imshow("Chessboard", frame);
-        if (cv::waitKey(30) == 27) // Exit on ESC key
-            break;
+        cv::imshow("Calibration", frame);
+        if (cv::waitKey(30) == 27) break; // Exit on ESC key
     }
 
-    cv::destroyAllWindows();
+    if (imagePoints.size() < 10)
+    {
+        ROS_ERROR("Not enough frames captured for calibration");
+        return -1;
+    }
 
-    // Calibrate the camera
-    cv::Mat cameraMatrix, distCoeffs, rvecs, tvecs;
+    cv::Mat cameraMatrix, distCoeffs;
+    std::vector<cv::Mat> rvecs, tvecs;
     cv::calibrateCamera(objectPoints, imagePoints, gray.size(), cameraMatrix, distCoeffs, rvecs, tvecs);
 
-    // Save the camera parameters
+    ROS_INFO("Calibration successful");
+    ROS_INFO_STREAM("Camera Matrix: " << cameraMatrix);
+    ROS_INFO_STREAM("Distortion Coefficients: " << distCoeffs);
+
     cv::FileStorage fs("camera_parameters.yml", cv::FileStorage::WRITE);
     fs << "camera_matrix" << cameraMatrix;
     fs << "distortion_coefficients" << distCoeffs;
     fs.release();
 
-    std::cout << "Camera calibration completed and parameters saved to camera_parameters.yml" << std::endl;
-
     return 0;
 }
\ No newline at end of file