From 55939f7f9c07f19b9d3fb33de037d93892e0c874 Mon Sep 17 00:00:00 2001
From: Thomas Bonnier <thomas.bonnier@ecam.fr>
Date: Fri, 29 Nov 2024 13:53:09 +0100
Subject: [PATCH] calibration

---
 test/CMakeLists.txt            |  5 +++
 test/src/calibrationWebcam.cpp | 73 ++++++++++++++++++++++++++++++++++
 2 files changed, 78 insertions(+)
 create mode 100644 test/src/calibrationWebcam.cpp

diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index ee6c6b4..d93f8cd 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -36,6 +36,7 @@ include_directories(
 
 ## Declare a C++ executable
 add_executable(qr_code_detector src/QRcode.cpp)
+add_executable(calibrationWebcam src/calibrationWebcam.cpp)
 
 ## Add cmake target dependencies of the executable
 add_dependencies(qr_code_detector ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
@@ -45,6 +46,10 @@ target_link_libraries(qr_code_detector
   ${catkin_LIBRARIES}
   ${OpenCV_LIBRARIES}
 )
+target_link_libraries(calibrationWebcam
+  ${catkin_LIBRARIES}
+  ${OpenCV_LIBRARIES}
+)
 
 #############
 ## Install ##
diff --git a/test/src/calibrationWebcam.cpp b/test/src/calibrationWebcam.cpp
new file mode 100644
index 0000000..015fde6
--- /dev/null
+++ b/test/src/calibrationWebcam.cpp
@@ -0,0 +1,73 @@
+#include <opencv2/opencv.hpp>
+#include <opencv2/calib3d.hpp>
+#include <opencv2/highgui.hpp>
+#include <opencv2/imgproc.hpp>
+#include <vector>
+#include <iostream>
+
+int main()
+{
+    // Define the number of inner corners per chessboard row and column
+    int boardWidth = 9;
+    int boardHeight = 6;
+    cv::Size boardSize(boardWidth, boardHeight);
+
+    // Prepare object points (0,0,0), (1,0,0), (2,0,0), ..., (boardWidth-1, boardHeight-1, 0)
+    std::vector<cv::Point3f> obj;
+    for (int i = 0; i < boardHeight; ++i)
+        for (int j = 0; j < boardWidth; ++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
+
+    if (!cap.isOpened())
+    {
+        std::cerr << "Unable to open camera" << std::endl;
+        return -1;
+    }
+
+    int numFrames = 0;
+    while (numFrames < 20) // Capture 20 frames
+    {
+        cap >> frame;
+        cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY);
+
+        bool found = cv::findChessboardCorners(gray, boardSize, corners,
+                                               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::drawChessboardCorners(frame, boardSize, corners, found);
+            imagePoints.push_back(corners);
+            objectPoints.push_back(obj);
+            numFrames++;
+        }
+
+        cv::imshow("Chessboard", frame);
+        if (cv::waitKey(30) == 27) // Exit on ESC key
+            break;
+    }
+
+    cv::destroyAllWindows();
+
+    // Calibrate the camera
+    cv::Mat cameraMatrix, distCoeffs, rvecs, tvecs;
+    cv::calibrateCamera(objectPoints, imagePoints, gray.size(), cameraMatrix, distCoeffs, rvecs, tvecs);
+
+    // Save the camera parameters
+    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