opencv_c++单目相机标定和矫正

刚刚开始学习图像处理，拙作还请广大网友斧正

目的是：能每0.5秒截取视频画面进行保存，每20个画面进行一次标定，并对视频进行畸变矫正

#if 1

#include <opencv2/opencv.hpp>
#include <iostream>
#include <opencv2/calib3d/calib3d_c.h>
#include <chrono>
#include <thread>
#include <mutex>
#include <future>

using namespace std;
using namespace cv;

// 定义函数:计算重投影误差
double computeReprojectionErrors(const vector<vector<Point3f>>& objectPoints,
    const vector<vector<Point2f>>& imagePoints,
    const vector<Mat>& rvecs, const vector<Mat>& tvecs,
    const Mat& cameraMatrix, const Mat& distCoeffs) 
{
    vector<Point2f> imagePoints2;
    double totalError = 0;
    int totalPoints = 0;

    for (size_t i = 0; i < objectPoints.size(); ++i) 
    {
        projectPoints(objectPoints[i], rvecs[i], tvecs[i], cameraMatrix, distCoeffs, imagePoints2);
        double err = norm(imagePoints[i], imagePoints2, NORM_L2);
        totalError += err * err;
        totalPoints += objectPoints[i].size();
    }
    return sqrt(totalError / totalPoints);
}

int main() 
{
    int boardWidth = 9;
    int boardHeight = 6;
    Size boardSize(boardWidth, boardHeight);

    vector<vector<Point3f>> objpoints;
    vector<vector<Point2f>> imgpoints;

    vector<Point3f> objp;
    for (int i = 0; i < boardHeight; ++i)
        for (int j = 0; j < boardWidth; ++j)
            objp.push_back(Point3f(j, i, 0));

    VideoCapture cap(0);
    if (!cap.isOpened())
    {
        cerr << "Error: 无法打开摄像机" << endl;
        return -1;
    }

    Mat frame, gray;
    Mat cameraMatrix, distCoeffs;
    bool calibrated = false;
    mutex mtx;
    double rms = 0;
    auto last_time = chrono::steady_clock::now();
    int i = 1;

    auto calib_thread = async(launch::async, [&]() {
        while (true) {
            this_thread::sleep_for(chrono::seconds(2));
            lock_guard<mutex> lock(mtx);
            if (imgpoints.size() >= 20) {
                cout << "正在标定摄像机..." << endl;
                vector<Mat> rvecs, tvecs;
                rms = calibrateCamera(objpoints, imgpoints, frame.size(), cameraMatrix, distCoeffs, rvecs, tvecs);
                double reprojectionError = computeReprojectionErrors(objpoints, imgpoints, rvecs, tvecs, cameraMatrix, distCoeffs);
                calibrated = true;
                imgpoints.clear();
                objpoints.clear();
                cout << "标定完成 " << i << endl;
                i++;
                cout << "CameraMatrix:\n" << cameraMatrix << endl;
                cout << "DistCoeffs:\n" << distCoeffs << endl;
                cout << "RMS error reported by calibrateCamera: " << rms << endl;
                cout << "Average reprojection error: " << reprojectionError << endl;
            }
        }
        });

    auto capture_thread = async(launch::async, [&]() 
        {
        while (true) 
        {
            cap >> frame;
            if (frame.empty()) 
            {
                cerr << "Error: 无法捕捉摄像头画面" << endl;
                break;
            }

            imshow("摄像头视频流", frame);

            auto current_time = chrono::steady_clock::now();
            chrono::duration<double> elapsed = current_time - last_time;

            if (elapsed.count() >= 0.5) 
            {
                Mat frame1 = frame.clone();
                cvtColor(frame1, gray, COLOR_BGR2GRAY);
                vector<Point2f> corner_pts;
                bool success = findChessboardCorners(gray, Size(boardWidth, boardHeight), corner_pts, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
                //bool success = findChessboardCornersSB(gray, boardSize, corner_pts, CALIB_CB_EXHAUSTIVE | CALIB_CB_ACCURACY);
                if (success) 
                {
                    cornerSubPix(gray, corner_pts, Size(11, 11), Size(-1, -1), TermCriteria(TermCriteria::EPS + TermCriteria::COUNT, 30, 0.1));
                    drawChessboardCorners(frame1, boardSize, corner_pts, success);
                    lock_guard<mutex> lock(mtx);
                    imgpoints.push_back(corner_pts);
                    objpoints.push_back(objp);
                    imshow("捕捉当前画面", frame1);
                }
                else 
                {
                    cout << endl << "未检测到格点..." << endl;
                }

                last_time = current_time;
            }

            if (calibrated) {
                Mat undistorted;
                undistort(frame, undistorted, cameraMatrix, distCoeffs);
                imshow("消畸变图像", undistorted);
            }

            if (waitKey(1) == 27) 
            {
                break;
            }
        }
        //释放相机资源
        cap.release();
        });

    calib_thread.get();
    capture_thread.get();
    destroyAllWindows();

    system("pause");

    return 0;
}

#endif

欢迎各位前辈批评指正！

希望有前辈能够对我进行一些指导，感激不尽！