#include <opencv2/opencv.hpp>
#include <opencv2/calib3d/calib3d.hpp>

int main() {
    // 设置圆形标定板的参数
    cv::Size boardSize(7, 6);  // 圆形标定板的行数和列数
    float squareSize = 0.02;    // 圆心之间的距离（米）

    // 读取左右摄像头的图像
    cv::Mat imgLeft = cv::imread("left_image.jpg", cv::IMREAD_GRAYSCALE);
    cv::Mat imgRight = cv::imread("right_image.jpg", cv::IMREAD_GRAYSCALE);

    // 检测圆形标定板的角点
    std::vector<cv::Point2f> centersLeft, centersRight;
    bool foundLeft = cv::findCirclesGrid(imgLeft, boardSize, centersLeft);
    bool foundRight = cv::findCirclesGrid(imgRight, boardSize, centersRight);

    if (foundLeft && foundRight) {
        // 生成圆形标定板的三维坐标
        std::vector<cv::Point3f> objectPoints;
        for (int i = 0; i < boardSize.height; ++i) {
            for (int j = 0; j < boardSize.width; ++j) {
                objectPoints.push_back(cv::Point3f(j * squareSize, i * squareSize, 0.0f));
            }
        }

        // 保存左右摄像头的图像坐标和对应的世界坐标
        std::vector<std::vector<cv::Point2f>> imagePointsLeft(1, centersLeft);
        std::vector<std::vector<cv::Point2f>> imagePointsRight(1, centersRight);
        std::vector<std::vector<cv::Point3f>> objectPointsVec(1, objectPoints);

        // 双目标定
        cv::Mat cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F;
        double rms = cv::stereoCalibrate(objectPointsVec, imagePointsLeft, imagePointsRight,
                                         cameraMatrix1, distCoeffs1,
                                         cameraMatrix2, distCoeffs2,
                                         imgLeft.size(), R, T, E, F,
                                         cv::CALIB_FIX_INTRINSIC);

        // 保存标定结果
        cv::FileStorage fs("stereo_calibration.yml", cv::FileStorage::WRITE);
        fs << "cameraMatrix1" << cameraMatrix1;
        fs << "distCoeffs1" << distCoeffs1;
        fs << "cameraMatrix2" << cameraMatrix2;
        fs << "distCoeffs2" << distCoeffs2;
        fs << "R" << R;
        fs << "T" << T;
        fs.release();

        std::cout << "Stereo calibration done. RMS error: " << rms << std::endl;
    } else {
        std::cerr << "Circles grid not found in one or both images." << std::endl;
    }

    return 0;
}