单目+双目标定(模拟单目+投影仪)+基于PNP+RANSAC+BA的双目相对位姿求解

数以及相关文件：

链接: https://pan.baidu.com/s/1BPdMGlLzLJl5HuVpci840w 提取码: p2cc

 Note：这里标定“单目+投影仪”的算法并非调用 stereoCalibration，除非二者焦距、成像分辨率一致。

mian.cpp

#include"MonoCamCalibration.h"


// 构建PNP world -> right camera -> left camera -> left image plane
int computeRT( const vector<vector<Point3f> >& objectPoints,
    const vector<vector<Point2f> >& imagePoints,
    const vector<Mat>& rvecsR,
    const vector<Mat>& tvecsR,
    const Mat& cameraMatrixL,
    const Mat& distCoeffsL)
{
   /* if ( !(objectPoints.size() == imagePoints.size() == rvecsR.size() == tvecsR.size()) ||
         objectPoints.empty()  ||
         cameraMatrixL.empty() ||
         distCoeffsL.empty())
    {
        return -1;
    }*/

    vector<Point3f> objectPointsCam;
    vector<Point2f> imagePoints_;

    objectPointsCam.reserve(objectPoints.size() * objectPoints[0].size()); // resize is more efficient
    imagePoints_.reserve(imagePoints.size() * imagePoints[0].size());

    for (size_t i = 0; i < objectPoints.size(); i++)
    {
        Mat R;
        Rodrigues(rvecsR[i], R);
        Mat t = tvecsR[i];

        // T = (R|t) 这里只能用Affine3d 、 double， float会导致点溢出，可能是Mat 仅支持 double 的原因
        cv::Affine3d T(cv::Affine3d::Mat3(R),
            cv::Affine3d::Vec3(t.at<double>(0, 0), t.at<double>(1, 0), t.at<double>(2, 0)));

        for (size_t j = 0; j < objectPoints[0].size(); j++)
        {
            objectPointsCam.push_back(T * objectPoints[i][j]); // world ->  camera
            imagePoints_.push_back(imagePoints[i][j]);
        }
    }

    // optimize the R|t
    Mat rvec1, t1, inliers;
    float errThreshold = 0.f; // for getting the outliers and inliers
    float inliersRatio = 0.f; // num of points in line /num of all the points 
    for (; inliersRatio < 0.5f; errThreshold += 0.02f)
    {
        solvePnPRansac(objectPointsCam, imagePoints_, cameraMatrixL, distCoeffsL, rvec1, t1, false, 1000, errThreshold, 0.99, inliers);
        inliersRatio = (float)inliers.rows / (int)imagePoints_.size();
    }
    cout << "inliersRatio = " << inliersRatio << ";  num of samples = " << inliers.rows << endl;

    // copy the inliers 
    vector<Point3f> objectPointsCamInliers;
    vector<Point2f> imagePointsInliers;
    objectPointsCamInliers.resize((size_t)inliers.rows);
    imagePointsInliers.resize((size_t)inliers.rows);

    for (int i = 0; i < inliers.rows; i++)
    {
        size_t idx = (size_t)inliers.at<__int32>(i, 0);

        objectPointsCamInliers[i] = objectPointsCam[idx];
        imagePointsInliers[i]     = imagePoints_[idx];
    }

    solvePnPRefineLM(objectPointsCamInliers, imagePointsInliers, cameraMatrixL, distCoeffsL, rvec1, t1, TermCriteria(TermCriteria::EPS + TermCriteria::COUNT, 1000, FLT_EPSILON));
    
    Mat R1;
    Rodrigues(rvec1, R1);

    vector<Point2f> imagePoints_RePro;
    double errs = 0.;
    projectPoints(objectPointsCamInliers, rvec1, t1, cameraMatrixL, distCoeffsL, imagePoints_RePro);
    for (size_t i = 0; i < imagePointsInliers.size(); i++)
    {
        errs += norm(Mat(imagePointsInliers[i]), Mat(imagePoints_RePro[i]), NORM_L2);
    }
    
    cout << "the err of PNP:" << errs / (int)imagePointsInliers.size() << " ";

    return 0;
}

void test1()
{
    // 1、棋盘
    Size boardSize(9, 6);                                   //角点size(w, h)
    Pattern pattern = CHESSBOARD;                         // 默认棋盘
    int nframes = 14;                                       // num of images
    float squareSize = 0.02f;                            // 格子实际宽度(m)

    // Left
    string outputFilenameL = "parameters_imgs1_chessboard_left.yml";  // xml of parameters
    string inputFilenameL = "imgs1_chessboard_left.xml";              // 图片集路径
    MonoCamCalibration monoCaliChessBoardL(boardSize, pattern, nframes, squareSize, outputFilenameL, inputFilenameL);
    if (monoCaliChessBoardL.compute() == 0)
    {
        cout << " Left successfully!" << endl;
    }

    Mat cameraMatrixL = monoCaliChessBoardL.getCameraMatrix();
    Mat distCoeffsL = monoCaliChessBoardL.getDistCoeffs();
    vector<vector<Point2f> > imagePointsL = monoCaliChessBoardL.getImagePoints();
    vector<vector<Point3f> > objectPoints = monoCaliChessBoardL.getObjectPoints();
    vector<Mat> rvecsL = monoCaliChessBoardL.getVecs();
    vector<Mat> tvecsL = monoCaliChessBoardL.getTvecs();

    // Right
    string outputFilenameR = "parameters_imgs1_chessboard_right.yml";  // xml of parameters
    string inputFilenameR = "imgs1_chessboard_right.xml";              // 图片集路径
    MonoCamCalibration monoCaliChessBoardR(boardSize, pattern, nframes, squareSize, outputFilenameR, inputFilenameR);
    if (monoCaliChessBoardR.compute() == 0)
    {
        cout << "Right successfully!" << endl;
    }

    Mat cameraMatrixR = monoCaliChessBoardR.getCameraMatrix();
    Mat distCoeffsR = monoCaliChessBoardR.getDistCoeffs();
    vector<vector<Point2f> > imagePointsR = monoCaliChessBoardR.getImagePoints();
    /*vector<vector<Point3f> > objectPoints = monoCaliChessBoardR.getObjectPoints();*/
    vector<Mat> rvecsR = monoCaliChessBoardR.getVecs();
    vector<Mat> tvecsR = monoCaliChessBoardR.getTvecs();

    // 构建PNP world -> right camera -> left camera -> left image plane
   /* if (computeRT(objectPoints, imagePointsL, rvecsR, tvecsR, cameraMatrixL, distCoeffsL) == 0)
    {

    }*/

    if (computeRT(objectPoints, imagePointsR, rvecsL, tvecsL, cameraMatrixR, distCoeffsR) == 0)
    {

    }
    cv::Mat M1 = cv::Mat::eye(3, 3, CV_64F);
    cv::Mat M2 = cv::Mat::eye(3, 3, CV_64F);
    cv::Mat D1, D2, R, T, E, F;
    cout << "\nRunning stereo calibration ...\n";
    cout << cv::stereoCalibrate(
        objectPoints, imagePointsL, imagePointsR, cameraMatrixL, distCoeffsL, cameraMatrixR, distCoeffsR, Size(640, 480), R, T, E, F,
        cv::CALIB_FIX_ASPECT_RATIO,
        cv::TermCriteria(cv::TermCriteria::COUNT | cv::TermCriteria::EPS, 1000,
            1e-5)) << endl;
}

void test2()
{
    // 2、圆(左)
    Size boardSize(7, 7);                                   //角点size(w, h)
    Pattern pattern = CIRCLES_GRID;                         //
    int nframes = 15;                                       // num of images
    float squareSize = 0.00375f;                            // 格子实际宽度(m)

    string outputFilenameL = "parameters_imgs2_circle_left.yml";  // xml of parameters
    string inputFilenameL = "imgs2_circle_left.xml";              // 图片集路径
    MonoCamCalibration monoCaliCircleL(boardSize, pattern, nframes, squareSize, outputFilenameL, inputFilenameL);
    if (monoCaliCircleL.compute() == 0)
    {
        cout << "successfully!" << endl;
    }
    
    Mat cameraMatrixL = monoCaliCircleL.getCameraMatrix();
    Mat distCoeffsL = monoCaliCircleL.getDistCoeffs();
    vector<vector<Point2f> > imagePointsL = monoCaliCircleL.getImagePoints();
    vector<vector<Point3f> > objectPoints = monoCaliCircleL.getObjectPoints();
    vector<Mat> rvecsL = monoCaliCircleL.getVecs();
    vector<Mat> tvecsL = monoCaliCircleL.getTvecs();

    // 2、圆(右)

    string outputFilenameR = "parameters_imgs2_circle_right.yml";  // xml of parameters
    string inputFilenameR = "imgs2_circle_right.xml";              // 图片集路径
    MonoCamCalibration monoCaliCircleR(boardSize, pattern, nframes, squareSize, outputFilenameR, inputFilenameR);
    if (monoCaliCircleR.compute() == 0)
    {
        cout << "successfully!" << endl;
    }

    Mat cameraMatrixR = monoCaliCircleR.getCameraMatrix();
    Mat distCoeffsR = monoCaliCircleR.getDistCoeffs();
    vector<vector<Point2f> > imagePointsR = monoCaliCircleR.getImagePoints();
    /*vector<vector<Point3f> > objectPoints = monoCaliCircleR.getObjectPoints();*/
    vector<Mat> rvecsR = monoCaliCircleR.getVecs();
    vector<Mat> tvecsR = monoCaliCircleR.getTvecs();

    // 构建PNP world -> right camera -> left camera -> left image plane
    /*if (computeRT(objectPoints, imagePointsL, rvecsR, tvecsR, cameraMatrixL, distCoeffsL) == 0)
    {

    }*/
    if (computeRT(objectPoints, imagePointsR, rvecsL, tvecsL, cameraMatrixR, distCoeffsR) == 0)
    {

    }

    cv::Mat M1 = cv::Mat::eye(3, 3, CV_64F);
    cv::Mat M2 = cv::Mat::eye(3, 3, CV_64F);
    cv::Mat D1, D2, R, T, E, F;
    cout << "\nRunning stereo calibration ...\n";
    cout << cv::stereoCalibrate(
        objectPoints, imagePointsL, imagePointsR, M1, D1, M2, D2, Size(320, 240), R, T, E, F,
        cv::CALIB_FIX_ASPECT_RATIO ,
        cv::TermCriteria(cv::TermCriteria::COUNT | cv::TermCriteria::EPS, 1000,
            1e-5)) << endl;
}

void test3()
{
    // 3、圆(左)
    Size boardSize(7, 7);                                   //角点size(w, h)
    Pattern pattern = CIRCLES_GRID;                         //
    int nframes = 11;                                       // num of images
    float squareSize = 0.00375f;                            // 格子实际宽度(m)

    string outputFilenameL = "parameters_imgs3_circle_left.yml";  // xml of parameters
    string inputFilenameL = "imgs3_circle_left.xml";              // 图片集路径
    MonoCamCalibration monoCaliCircleL(boardSize, pattern, nframes, squareSize, outputFilenameL, inputFilenameL);
    if (monoCaliCircleL.compute() == 0)
    {
        cout << "successfully!" << endl;
    }

    Mat cameraMatrixL                     = monoCaliCircleL.getCameraMatrix();
    Mat distCoeffsL                       = monoCaliCircleL.getDistCoeffs();
    vector<vector<Point2f> > imagePointsL = monoCaliCircleL.getImagePoints();
    vector<vector<Point3f> > objectPoints = monoCaliCircleL.getObjectPoints();
    vector<Mat> rvecsL                    = monoCaliCircleL.getVecs();
    vector<Mat> tvecsL                    = monoCaliCircleL.getTvecs();

    // 2、圆(右)

    string outputFilenameR = "parameters_imgs3_circle_right.yml";  // xml of parameters
    string inputFilenameR = "imgs3_circle_right.xml";              // 图片集路径
    MonoCamCalibration monoCaliCircleR(boardSize, pattern, nframes, squareSize, outputFilenameR, inputFilenameR);
    if (monoCaliCircleR.compute() == 0)
    {
        cout << "successfully!" << endl;
    }  

    Mat cameraMatrixR                     = monoCaliCircleR.getCameraMatrix();
    Mat distCoeffsR                       = monoCaliCircleR.getDistCoeffs();
    vector<vector<Point2f> > imagePointsR = monoCaliCircleR.getImagePoints();
    /*vector<vector<Point3f> > objectPoints = monoCaliCircleR.getObjectPoints();*/
    vector<Mat> rvecsR                    = monoCaliCircleR.getVecs();
    vector<Mat> tvecsR                    = monoCaliCircleR.getTvecs();

    // 构建PNP world -> right camera -> left camera -> left image plane
   /* if (computeRT(objectPoints, imagePointsL, rvecsR, tvecsR, cameraMatrixL, distCoeffsL) == 0)
    {

    }*/
    if (computeRT(objectPoints, imagePointsR, rvecsL, tvecsL, cameraMatrixR, distCoeffsR) == 0)
    {

    }

     cv::Mat M1 = cv::Mat::eye(3, 3, CV_64F);
     cv::Mat M2 = cv::Mat::eye(3, 3, CV_64F);
     cv::Mat D1, D2, R, T, E, F;
     cout << "\nRunning stereo calibration ...\n";
     cout << cv::stereoCalibrate(
         objectPoints, imagePointsL, imagePointsR, cameraMatrixL, distCoeffsL, cameraMatrixR, distCoeffsR, Size(640, 480), R, T, E, F,
         cv::CALIB_FIX_ASPECT_RATIO,
         cv::TermCriteria(cv::TermCriteria::COUNT | cv::TermCriteria::EPS, 1000,
             1e-5)) << endl;
    /* cout << -1 * R.t() * T << endl;*/
}
// 单目标定
void test4()
{
    Size boardSize(8, 11);                                   //角点size(w, h)
    Pattern pattern = CHESSBOARD;                         //
    int nframes = 6;                                       // num of images
    float squareSize = 0.03f;                            // 格子实际宽度(m)

    string outputFilenameL = "parameters_imgs_chessBoard.yml";  // xml of parameters
    string inputFilenameL = "imgs_chessBoard.xml";              // 图片集路径
    MonoCamCalibration monoCaliCircleL(boardSize, pattern, nframes, squareSize, outputFilenameL, inputFilenameL);
    if (monoCaliCircleL.compute() == 0)
    {
        cout << "successfully!" << endl;
    }

    Mat cameraMatrixL = monoCaliCircleL.getCameraMatrix();
    Mat distCoeffsL = monoCaliCircleL.getDistCoeffs();
    vector<vector<Point2f> > imagePointsL = monoCaliCircleL.getImagePoints();
    vector<vector<Point3f> > objectPoints = monoCaliCircleL.getObjectPoints();
    vector<Mat> rvecsL = monoCaliCircleL.getVecs();
    vector<Mat> tvecsL = monoCaliCircleL.getTvecs();

    
}

int main(int argc, char** argv)
{
    test1();
    test2();
    test3();
    test4();
    return 0;
}

 

MonoCamCalibration.h

#pragma once
#define _CRT_SECURE_NO_WARNINGS

#ifndef MONOCAMCALIBRATION_H
#define MONOCAMCALIBRATION_H

#include "opencv2/core.hpp"
#include <opencv2/core/utility.hpp>
#include "opencv2/imgproc.hpp"
#include "opencv2/calib3d.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/videoio.hpp"
#include "opencv2/highgui.hpp"

#include<iostream>
#include <cctype>
#include <stdio.h>
#include <string.h>
#include <time.h>

using namespace cv;
using namespace std;

enum { DETECTION = 0, CAPTURING = 1, CALIBRATED = 2 };
enum Pattern { CHESSBOARD, CIRCLES_GRID, ASYMMETRIC_CIRCLES_GRID };

class MonoCamCalibration
{
public:
    MonoCamCalibration(const Size&    boardSize,     
                    const Pattern& pattern,        
                    const int&     nframes,        
                    const float&   squareSize,      
                    const string&  outputFilename,  
                    const string&  inputFilename);

    // 输出重投影误差
        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<float>& perViewErrors);
    // 标定板类型判定、设定3D角点
    void calcChessboardCorners(Size boardSize, float squareSize, vector<Point3f>& corners, Pattern patternType = CHESSBOARD);

    // 执行标定
    bool runCalibration(vector<vector<Point2f> > imagePoints,
        Size imageSize, Size boardSize, Pattern patternType,
        float squareSize, float aspectRatio,
        int flags, Mat& cameraMatrix, Mat& distCoeffs,
        vector<Mat>& rvecs, vector<Mat>& tvecs,
        vector<float>& reprojErrs,
        double& totalAvgErr);

    // 保存操作
    void saveCameraParams(const string& filename,
        Size imageSize, Size boardSize,
        float squareSize, float aspectRatio, int flags,
        const Mat& cameraMatrix, const Mat& distCoeffs,
        const vector<Mat>& rvecs, const vector<Mat>& tvecs,
        const vector<float>& reprojErrs,
        const vector<vector<Point2f> >& imagePoints,
        double totalAvgErr);

    // 从xml/yaml 文件读取文件路径
    bool readStringList(const string& filename, vector<string>& l);

    // 执行保存操作
    bool runAndSave(const string& outputFilename,
        const vector<vector<Point2f> >& imagePoints,
        Size imageSize, Size boardSize, Pattern patternType, float squareSize,
        float aspectRatio, int flags, Mat& cameraMatrix,
        Mat& distCoeffs, bool writeExtrinsics, bool writePoints);

    // 执行整个流程
    // 以下是输入参数
    //Size boardSize,  //角点size(w, h)
    //Pattern pattern, // 默认棋盘
    //int nframes,     // num of images
    //float squareSize,// 格子实际宽度(m)
    //string outputFilename,// xml of parameters              
    //string inputFilename
    int compute();

    // 
    Mat getCameraMatrix() const
    {
        return this->cameraMatrix;
    }
    Mat getDistCoeffs() const
    {
        return this->distCoeffs;
    }
    vector<Mat> getVecs() const
    {
        return this->rvecs;
    }

    vector<Mat> getTvecs() const
    {
        return this->tvecs;
    }
    vector<vector<Point2f> > getImagePoints() const
    {
        return this->imagePoints;
    }
    vector<vector<Point3f> > getObjectPoints() const
    {
        return this->objectPoints;
    }

    vector<size_t> getGoodPaternIdx() const
    {
        return this->goodPaternIdx;
    }

private:
    Size    boardSize;       // 角点size(w, h)
    Pattern pattern;         // 默认棋盘
    int     nframes;         // num of images
    float   squareSize;      // 格子实际宽度(m)
    string  outputFilename;  // xml of parameters
    string  inputFilename;   // 图片集路径

private:
    Mat cameraMatrix, distCoeffs;
    vector<Mat> rvecs, tvecs;
    vector<vector<Point2f> > imagePoints;
    vector<vector<Point3f> > objectPoints;
    vector<size_t> goodPaternIdx;
    
};

#endif

MonoCamCalibration.cpp

#include "MonoCamCalibration.h"


MonoCamCalibration::MonoCamCalibration(   const Size& boardSize,
                                    const Pattern& pattern,
                                    const int& nframes,
                                    const float& squareSize,
                                    const string& outputFilename,
                                    const string& inputFilename   ):
                                    boardSize(boardSize),
                                    pattern(pattern),
                                    nframes(nframes),
                                    squareSize(squareSize),
                                    outputFilename(outputFilename),
                                    inputFilename(inputFilename)
{
    objectPoints.resize(1);
}



// 输出重投影误差
double MonoCamCalibration::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<float>& perViewErrors)
{
    vector<Point2f> imagePoints2;
    int i, totalPoints = 0;
    double totalErr = 0, err;
    perViewErrors.resize(objectPoints.size());

    for (i = 0; i < (int)objectPoints.size(); i++)
    {
        projectPoints(Mat(objectPoints[i]), rvecs[i], tvecs[i], cameraMatrix, distCoeffs, imagePoints2);
        err = norm(Mat(imagePoints[i]), Mat(imagePoints2), NORM_L2);
        int n = (int)objectPoints[i].size();
        perViewErrors[i] = (float)std::sqrt(err * err / n);
        totalErr += err * err;
        totalPoints += n;
    }

    return std::sqrt(totalErr / totalPoints);
}

// 标定板类型判定、设定3D角点
void MonoCamCalibration::calcChessboardCorners(Size boardSize, float squareSize, vector<Point3f>& corners, Pattern patternType)
{
    corners.resize(0);

    switch (patternType)
    {
    case CHESSBOARD:
    case CIRCLES_GRID: // 对称圆环
      // 世界坐标系点
        for (int i = 0; i < boardSize.height; i++)
            for (int j = 0; j < boardSize.width; j++)
                corners.push_back(Point3f(float(j * squareSize), float(i * squareSize), 0));
        break;

    case ASYMMETRIC_CIRCLES_GRID: // 不对称圆环
      // 世界坐标系点
        for (int i = 0; i < boardSize.height; i++)
            for (int j = 0; j < boardSize.width; j++)
                corners.push_back(Point3f(float((2 * j + i % 2) * squareSize), float(i * squareSize), 0));
        break;

    default:
        CV_Error(Error::StsBadArg, "Unknown pattern type\n");
    }
}

// 执行标定流程
bool MonoCamCalibration::runCalibration(vector<vector<Point2f> > imagePoints,
    Size imageSize, Size boardSize, Pattern patternType,
    float squareSize, float aspectRatio,
    int flags, Mat& cameraMatrix, Mat& distCoeffs,
    vector<Mat>& rvecs, vector<Mat>& tvecs,
    vector<float>& reprojErrs,
    double& totalAvgErr)
{
    cameraMatrix = Mat::eye(3, 3, CV_64F);
    if (flags & CALIB_FIX_ASPECT_RATIO)
        cameraMatrix.at<double>(0, 0) = aspectRatio;

    distCoeffs = Mat::zeros(8, 1, CV_64F);

    
    calcChessboardCorners(boardSize, squareSize, objectPoints[0], patternType);

    objectPoints.resize(imagePoints.size(), objectPoints[0]);// 每一张棋盘对应世界坐标系角点3D坐标不变，直接拷贝

    double rms = calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs, flags | CALIB_FIX_K4 | CALIB_FIX_K5);
    ///*|CALIB_FIX_K3*/|CALIB_FIX_K4|CALIB_FIX_K5);
    printf("RMS error reported by calibrateCamera: %g\n", rms);

    bool ok = checkRange(cameraMatrix) && checkRange(distCoeffs); // 检测是否有无穷大 or 无穷小

    totalAvgErr = computeReprojectionErrors(objectPoints, imagePoints, rvecs, tvecs, cameraMatrix, distCoeffs, reprojErrs);

    return ok;
}

// 保存操作
void MonoCamCalibration::saveCameraParams(const string& filename,
    Size imageSize, Size boardSize,
    float squareSize, float aspectRatio, int flags,
    const Mat& cameraMatrix, const Mat& distCoeffs,
    const vector<Mat>& rvecs, const vector<Mat>& tvecs,
    const vector<float>& reprojErrs,
    const vector<vector<Point2f> >& imagePoints,
    double totalAvgErr)
{
    FileStorage fs(filename, FileStorage::WRITE);

    time_t tt;
    time(&tt);
    struct tm* t2 = localtime(&tt);
    char buf[1024];
    strftime(buf, sizeof(buf) - 1, "%c", t2);

    fs << "calibration_time" << buf;

    if (!rvecs.empty() || !reprojErrs.empty())
        fs << "nframes" << (int)std::max(rvecs.size(), reprojErrs.size());
    fs << "image_width" << imageSize.width;
    fs << "image_height" << imageSize.height;
    fs << "board_width" << boardSize.width;
    fs << "board_height" << boardSize.height;
    fs << "square_size" << squareSize;

    if (flags & CALIB_FIX_ASPECT_RATIO)
        fs << "aspectRatio" << aspectRatio;

    if (flags != 0)
    {
        sprintf(buf, "flags: %s%s%s%s",
            flags & CALIB_USE_INTRINSIC_GUESS ? "+use_intrinsic_guess" : "",
            flags & CALIB_FIX_ASPECT_RATIO ? "+fix_aspectRatio" : "",
            flags & CALIB_FIX_PRINCIPAL_POINT ? "+fix_principal_point" : "",
            flags & CALIB_ZERO_TANGENT_DIST ? "+zero_tangent_dist" : "");
        //cvWriteComment( *fs, buf, 0 );
    }

    fs << "flags" << flags;

    fs << "camera_matrix" << cameraMatrix;
    fs << "distortion_coefficients" << distCoeffs;

    fs << "avg_reprojection_error" << totalAvgErr;
    if (!reprojErrs.empty())
        fs << "per_view_reprojection_errors" << Mat(reprojErrs);

    if (!rvecs.empty() && !tvecs.empty())
    {
        CV_Assert(rvecs[0].type() == tvecs[0].type());
        Mat bigmat((int)rvecs.size(), 6, rvecs[0].type());
        for (int i = 0; i < (int)rvecs.size(); i++)
        {
            Mat r = bigmat(Range(i, i + 1), Range(0, 3));
            Mat t = bigmat(Range(i, i + 1), Range(3, 6));

            CV_Assert(rvecs[i].rows == 3 && rvecs[i].cols == 1);
            CV_Assert(tvecs[i].rows == 3 && tvecs[i].cols == 1);
            //*.t() is MatExpr (not Mat) so we can use assignment operator
            r = rvecs[i].t();
            t = tvecs[i].t();
        }
        //cvWriteComment( *fs, "a set of 6-tuples (rotation vector + translation vector) for each view", 0 );
        fs << "extrinsic_parameters" << bigmat;
    }

    if (!imagePoints.empty())
    {
        Mat imagePtMat((int)imagePoints.size(), (int)imagePoints[0].size(), CV_32FC2);
        for (int i = 0; i < (int)imagePoints.size(); i++)
        {
            Mat r = imagePtMat.row(i).reshape(2, imagePtMat.cols);
            Mat imgpti(imagePoints[i]);
            imgpti.copyTo(r);
        }
        fs << "image_points" << imagePtMat;
    }
}

// 从xml/yaml 文件读取文件路径
bool MonoCamCalibration::readStringList(const string& filename, vector<string>& l)
{
    l.resize(0);
    FileStorage fs(filename, FileStorage::READ);
    if (!fs.isOpened())
        return false;
    size_t dir_pos = filename.rfind('/');
    if (dir_pos == string::npos)
        dir_pos = filename.rfind('\\');
    FileNode n = fs.getFirstTopLevelNode();
    if (n.type() != FileNode::SEQ)
        return false;
    FileNodeIterator it = n.begin(), it_end = n.end();
    for (; it != it_end; ++it)
    {
        string fname = (string)*it;
        if (dir_pos != string::npos)
        {
            string fpath = samples::findFile(filename.substr(0, dir_pos + 1) + fname, false);
            if (fpath.empty())
            {
                fpath = samples::findFile(fname);
            }
            fname = fpath;
        }
        else
        {
            fname = samples::findFile(fname);
        }
        l.push_back(fname);
    }
    return true;
}

// 执行保存操作
bool MonoCamCalibration::runAndSave(const string& outputFilename,
    const vector<vector<Point2f> >& imagePoints,
    Size imageSize, Size boardSize, Pattern patternType, float squareSize,
    float aspectRatio, int flags, Mat& cameraMatrix,
    Mat& distCoeffs, bool writeExtrinsics, bool writePoints)
{
    vector<float> reprojErrs;
    double totalAvgErr = 0;

    bool ok = runCalibration(imagePoints, imageSize, boardSize, patternType, squareSize, aspectRatio, flags, cameraMatrix, distCoeffs, rvecs, tvecs, reprojErrs, totalAvgErr);
    printf("%s. avg reprojection error = %.2f\n", ok ? "Calibration succeeded" : "Calibration failed", totalAvgErr);
    return ok;
}

int MonoCamCalibration::compute()// 图片集路径
{
    // 优化参数设置
    // aspectRatio + flags !!!
    float aspectRatio = 1.0f;                               // fx/fy
    int flags = 2;                                              // 优化参数
    //flags |= CALIB_FIX_ASPECT_RATIO;                        // 固定fx/fy


    // <1>、参数检查
    if (boardSize.width <= 0)
        return fprintf(stderr, "Invalid board width\n"), -1;
    if (boardSize.height <= 0)
        return fprintf(stderr, "Invalid board height\n"), -1;
    if (nframes <= 0)
        return fprintf(stderr, "Invalid  nframes\n"), -1;
    if (squareSize <= 0)
        return fprintf(stderr, "Invalid squareSize\n"), -1;
    if (outputFilename.empty())
        return fprintf(stderr, "Invalid outputFilename\n"), -1;
    if (inputFilename.empty())
        return fprintf(stderr, "Invalid inputFilename\n"), -1;

    // <2>、读取数据
    int mode = DETECTION;
    vector<string> imageList;

    if (!inputFilename.empty())
    {
        if (readStringList(samples::findFile(inputFilename), imageList))
            mode = CAPTURING;
    }

    if (!imageList.empty())
    {
        nframes = (int)imageList.size();
    }
    else
    {
        return -2;
    }

    namedWindow("Image View", 1);
    // <3>、开始检测
    Size imageSize;
    int i;
    for (i = 0;; i++)
    {
        Mat view, viewGray;

        if (i < (int)imageList.size())
            view = imread(imageList[i], 1);

        if (view.empty())
        {
            cout << i << endl;
            if (imagePoints.size() > 0)
                runAndSave(outputFilename, imagePoints, imageSize, boardSize, pattern, squareSize, aspectRatio, flags, cameraMatrix, distCoeffs, false, false);
            break;

        }

        imageSize = view.size();
        vector<Point2f> pointbuf;
        cvtColor(view, viewGray, COLOR_BGR2GRAY);
        // 检测角点 -> pointbuf
        bool found;
        switch (pattern)
        {
        case CHESSBOARD:
            found = findChessboardCorners(view, boardSize, pointbuf, CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_FAST_CHECK | CALIB_CB_NORMALIZE_IMAGE);
            break;
        case CIRCLES_GRID:
            found = findCirclesGrid(view, boardSize, pointbuf);
            break;
        case ASYMMETRIC_CIRCLES_GRID:
            found = findCirclesGrid(view, boardSize, pointbuf, CALIB_CB_ASYMMETRIC_GRID);
            break;
        default:
            return fprintf(stderr, "Unknown pattern type\n"), -1;
        }

        // 提取棋盘亚像素角点
        if (pattern == CHESSBOARD && found)
            cornerSubPix(viewGray, pointbuf, Size(11, 11), Size(-1, -1), TermCriteria(TermCriteria::EPS + TermCriteria::COUNT, 30, 0.1));

        if (mode == CAPTURING && found)
        {
            imagePoints.push_back(pointbuf);
        }

        if (found)
            drawChessboardCorners(view, boardSize, Mat(pointbuf), found);

        string msg = mode == CAPTURING ? "100/100" : mode == CALIBRATED ? "Calibrated" : "Press 'g' to start";
        int baseLine = 0;
        Size textSize = getTextSize(msg, 1, 1, 1, &baseLine);
        Point textOrigin(view.cols - 2 * textSize.width - 10, view.rows - 2 * baseLine - 10);

        if (mode == CAPTURING)
        {
            msg = format("%d/%d", (int)imagePoints.size(), nframes);
        }

        putText(view, msg, textOrigin, 1, 1, mode != CALIBRATED ? Scalar(0, 0, 255) : Scalar(0, 255, 0));
        imshow("Image View", view);
        waitKey(10);
    }
    return 0;
}

 输出结果：

另：对于文件中丢了一堆图，如何获取每一张图的“绝对路径”？这里提供一个方法：

1、新建文本，拷贝以下脚本

DIR /S/B > fileList.txt

保存，修改后缀名为：“.bat”

2、双击该文件，会自动生成 包含所有图片路径的文本文件，如下图：

D:\VS2019_Project\外参数求解\MonoProjectorCalib\calibration2_stereo_circle\left\calib_l_01.png
D:\VS2019_Project\外参数求解\MonoProjectorCalib\calibration2_stereo_circle\left\calib_l_02.png
D:\VS2019_Project\外参数求解\MonoProjectorCalib\calibration2_stereo_circle\left\calib_l_03.png
D:\VS2019_Project\外参数求解\MonoProjectorCalib\calibration2_stereo_circle\left\calib_l_04.png
D:\VS2019_Project\外参数求解\MonoProjectorCalib\calibration2_stereo_circle\left\calib_l_05.png
D:\VS2019_Project\外参数求解\MonoProjectorCalib\calibration2_stereo_circle\left\calib_l_06.png
D:\VS2019_Project\外参数求解\MonoProjectorCalib\calibration2_stereo_circle\left\calib_l_07.png
D:\VS2019_Project\外参数求解\MonoProjectorCalib\calibration2_stereo_circle\left\calib_l_08.png
D:\VS2019_Project\外参数求解\MonoProjectorCalib\calibration2_stereo_circle\left\calib_l_09.png
D:\VS2019_Project\外参数求解\MonoProjectorCalib\calibration2_stereo_circle\left\calib_l_10.png
D:\VS2019_Project\外参数求解\MonoProjectorCalib\calibration2_stereo_circle\left\calib_l_11.png
D:\VS2019_Project\外参数求解\MonoProjectorCalib\calibration2_stereo_circle\left\calib_l_12.png
D:\VS2019_Project\外参数求解\MonoProjectorCalib\calibration2_stereo_circle\left\calib_l_13.png
D:\VS2019_Project\外参数求解\MonoProjectorCalib\calibration2_stereo_circle\left\calib_l_14.png
D:\VS2019_Project\外参数求解\MonoProjectorCalib\calibration2_stereo_circle\left\calib_l_15.png
D:\VS2019_Project\外参数求解\MonoProjectorCalib\calibration2_stereo_circle\left\fileList.txt
D:\VS2019_Project\外参数求解\MonoProjectorCalib\calibration2_stereo_circle\left\GetFileList.bat

【记得删除最后两行】

3、注意到上述路径为左斜杠，即：\ ，直接保存到 String中去汇报错，我们将文本文件丢到vs中去，再按Ctrl + F 查找并用“\\”替换 \