相机/立体标定——Matlab/Opencv实现代码

1. Matlab资源

(1) Camera Calibration Toolbox for Matlab 工具箱

下载链接：http://www.vision.caltech.edu/bouguetj/calib_doc/

2. Matlab内建工具，如下图所示：

3.本人基于OpenCV实现了相机标定和立体标定的代码，如下（注意，本代码是在Opencv相机标定的Demo上进行修改，但是，Opencv的Demo已经和当前的3.1版本的不适应，特别是鱼眼相机标定方法，Opencv自带的Demo的配置文件不满足鱼眼相机标定相关的参数配置，详细见下面代码和说明）

3.1 Opencv相机标定Demo

/*
 * Author: HSW
 * Date: 2017-9-16
 * Conventional Camera / Fisheye Camera calibrate
*/

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include "singlecameracalib.h"

using namespace std;
using namespace cv;

// 单相机标定结果
typedef struct singleCameraCalibResult
{
    Mat cameraMatrix;
    Mat distCoeffs;

    void saveCameraParams(string savePath)
    {
        FileStorage fs(savePath, FileStorage::WRITE);
        if(fs.isOpened())
        {
            fs << "cameraMatrix" << cameraMatrix
               << "distCoeffs"   << distCoeffs;

            fs.release();
        }
        else
        {
            cout << "Save Camera Params. Failed !" << endl;
        }
    }
}singleCameraCalibResult;

//立体标定结果
typedef struct stereoCameraCalibResult
{
    singleCameraCalibResult left;
    singleCameraCalibResult right;
    Mat R;
    Mat T;

    void saveStereoParams(string savePath)
    {
        FileStorage fs(savePath, FileStorage::WRITE);
        if(fs.isOpened())
        {
            fs << "cameraMatrixL" << left.cameraMatrix
               << "distCoeffsL"   << left.distCoeffs
               << "cameraMatrixR" << right.cameraMatrix
               << "distCoeffsR"   << right.distCoeffs
               << "R"             << R
               << "T"             << T;
            fs.release();
        }
        else
        {
            cout << "Save Camera Params. Failed !" << endl;
        }
    }
}stereoCameraCalibResult;

enum { SINGLECAMERACALIB, STEREOCALIB};
enum { DETECTION = 0, CAPTURING = 1, CALIBRATED = 2 };

const int calibMode = STEREOCALIB; //标定的模式，单相机标定、立体标定

singleCameraCalibResult singleResult;
stereoCameraCalibResult stereoResult;

static inline void read(const FileNode& node, Settings& x, const Settings& default_value = Settings())
{
    if(node.empty())
        x = default_value;
    else
        x.read(node);
}

static inline void write(FileStorage& fs, const String&, const Settings& s )
{
    s.write(fs);
}

//! [compute_errors]
static double computeReprojectionErrors( const vector >& objectPoints,
                                         const vector >& imagePoints,
                                         const vector& rvecs, const vector& tvecs,
                                         const Mat& cameraMatrix , const Mat& distCoeffs,
                                         vector& perViewErrors, bool fisheye)
{
    vector imagePoints2;
    size_t totalPoints = 0;
    double totalErr = 0, err;
    perViewErrors.resize(objectPoints.size());

    for(size_t i = 0; i < objectPoints.size(); ++i )
    {
        if (fisheye)
        {
            fisheye::projectPoints(objectPoints[i], imagePoints2, rvecs[i], tvecs[i], cameraMatrix,
                                   distCoeffs);
        }
        else
        {
            projectPoints(objectPoints[i], rvecs[i], tvecs[i], cameraMatrix, distCoeffs, imagePoints2);
        }
        err = norm(imagePoints[i], imagePoints2, NORM_L2);

        size_t n = objectPoints[i].size();
        perViewErrors[i] = (float) std::sqrt(err*err/n);
        totalErr        += err*err;
        totalPoints     += n;
    }

    return std::sqrt(totalErr/totalPoints);
}
//! [compute_errors]
//! [board_corners]
static void calcBoardCornerPositions(Size boardSize, float squareSize, vector& corners,
                                     Settings::Pattern patternType /*= Settings::CHESSBOARD*/)
{
    corners.clear();

    switch(patternType)
    {
    case Settings::CHESSBOARD:
    case Settings::CIRCLES_GRID:
        for( int i = 0; i < boardSize.height; ++i )
            for( int j = 0; j < boardSize.width; ++j )
                corners.push_back(Point3f(j*squareSize, i*squareSize, 0));
        break;

    case Settings::ASYMMETRIC_CIRCLES_GRID:
        for( int i = 0; i < boardSize.height; i++ )
            for( int j = 0; j < boardSize.width; j++ )
                corners.push_back(Point3f((2*j + i % 2)*squareSize, i*squareSize, 0));
        break;
    default:
        break;
    }
}
//! [board_corners]

//! [board_corners 2]
static void calcBoardCornerPositions2(Size boardSize, float squareSize, vector& corners,
                                      Settings::Pattern patternType /*= Settings::CHESSBOARD*/)
{
    corners.clear();

    switch(patternType)
    {
    case Settings::CHESSBOARD:
    case Settings::CIRCLES_GRID:
        for( int i = 0; i < boardSize.height; ++i )
            for( int j = 0; j < boardSize.width; ++j )
                corners.push_back(Point3d(j*squareSize, i*squareSize, 0));
        break;

    case Settings::ASYMMETRIC_CIRCLES_GRID:
        for( int i = 0; i < boardSize.height; i++ )
            for( int j = 0; j < boardSize.width; j++ )
                corners.push_back(Point3d((2*j + i % 2)*squareSize, i*squareSize, 0));
        break;
    default:
        break;
    }
}
//! [board_corners 2]
static bool runCalibration( Settings& s, Size& imageSize, Mat& cameraMatrix, Mat& distCoeffs,
                            vector > imagePoints, vector& rvecs, vector& tvecs,
                            vector& reprojErrs,  double& totalAvgErr)
{
    //! [fixed_aspect]
    cameraMatrix = Mat::eye(3, 3, CV_64F);
    if( s.flag & CALIB_FIX_ASPECT_RATIO )
    {
        cameraMatrix.at(0,0) = s.aspectRatio;
    }
    //! [fixed_aspect]
    if (s.useFisheye)
    {
        distCoeffs = Mat::zeros(4, 1, CV_64F);
    }
    else
    {
        distCoeffs = Mat::zeros(8, 1, CV_64F);
    }

    vector > objectPoints(1);
    calcBoardCornerPositions(s.boardSize, s.squareSize, objectPoints[0], s.calibrationPattern);

    objectPoints.resize(imagePoints.size(),objectPoints[0]);

    //Find intrinsic and extrinsic camera parameters
    double rms;

    if (s.useFisheye)
    {
        //Mat _rvecs, _tvecs;
        std::vector _rvecs;
        std::vector  _tvecs;
        rms = fisheye::calibrate(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, _rvecs,
                                 _tvecs, s.flag, cv::TermCriteria(3, 20, 1e-6));

        for(int i = 0; i < (int)(objectPoints.size()); ++i)
        {
            Mat tempR = Mat::zeros(Size(1,3), CV_32FC1);
            Mat tempT = Mat::zeros(Size(1,3), CV_32FC1);
            Vec3f vec = _rvecs[i];
            tempR.at(0,0) = vec[0];
            tempR.at(1,1) = vec[1];
            tempR.at(2,2) = vec[2];

            vec = _tvecs[i];
            tempT.at(0,0) = vec[0];
            tempT.at(1,1) = vec[1];
            tempT.at(2,2) = vec[2];

            rvecs.push_back(tempR);
            tvecs.push_back(tempT);
        }
    }
    else
    {
        rms = calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs,
                              s.flag);
    }

    cout << "Re-projection error reported by calibrateCamera: "<< rms << endl;

    bool ok = checkRange(cameraMatrix) && checkRange(distCoeffs);

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

    return ok;
}

// Print camera parameters to the output file
static void saveCameraParams( Settings& s, Size& imageSize, Mat& cameraMatrix, Mat& distCoeffs,
                              const vector& rvecs, const vector& tvecs,
                              const vector& reprojErrs, const vector >& imagePoints,
                              double totalAvgErr )
{
    FileStorage fs( s.outputFileName, FileStorage::WRITE );

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

     fs << "calibration_time" << buf;

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

     if( s.flag & CALIB_FIX_ASPECT_RATIO )
         fs << "fix_aspect_ratio" << s.aspectRatio;

     if (s.flag)
     {
         std::stringstream flagsStringStream;
         if (s.useFisheye)
         {
             flagsStringStream << "flags:"
                 << (s.flag & fisheye::CALIB_FIX_SKEW ? " +fix_skew" : "")
                 << (s.flag & fisheye::CALIB_FIX_K1 ? " +fix_k1" : "")
                 << (s.flag & fisheye::CALIB_FIX_K2 ? " +fix_k2" : "")
                 << (s.flag & fisheye::CALIB_FIX_K3 ? " +fix_k3" : "")
                 << (s.flag & fisheye::CALIB_FIX_K4 ? " +fix_k4" : "")
                 << (s.flag & fisheye::CALIB_RECOMPUTE_EXTRINSIC ? " +recompute_extrinsic" : "");
         }
         else
         {
             flagsStringStream << "flags:"
                 << (s.flag & CALIB_USE_INTRINSIC_GUESS ? " +use_intrinsic_guess" : "")
                 << (s.flag & CALIB_FIX_ASPECT_RATIO ? " +fix_aspectRatio" : "")
                 << (s.flag & CALIB_FIX_PRINCIPAL_POINT ? " +fix_principal_point" : "")
                 << (s.flag & CALIB_ZERO_TANGENT_DIST ? " +zero_tangent_dist" : "")
                 << (s.flag & CALIB_FIX_K1 ? " +fix_k1" : "")
                 << (s.flag & CALIB_FIX_K2 ? " +fix_k2" : "")
                 << (s.flag & CALIB_FIX_K3 ? " +fix_k3" : "")
                 << (s.flag & CALIB_FIX_K4 ? " +fix_k4" : "")
                 << (s.flag & CALIB_FIX_K5 ? " +fix_k5" : "");
         }
         //fs.writeComment(flagsStringStream.str());
     }

     fs << "flags" << s.flag;

     fs << "fisheye_model" << s.useFisheye;

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

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

     if(s.writeExtrinsics && !rvecs.empty() && !tvecs.empty() )
     {
         CV_Assert(rvecs[0].type() == tvecs[0].type());
         Mat bigmat((int)rvecs.size(), 6, CV_MAKETYPE(rvecs[0].type(), 1));
         bool needReshapeR = rvecs[0].depth() != 1 ? true : false;
         bool needReshapeT = tvecs[0].depth() != 1 ? true : false;

         for( size_t i = 0; i < rvecs.size(); i++ )
         {
             Mat r = bigmat(Range(int(i), int(i+1)), Range(0,3));
             Mat t = bigmat(Range(int(i), int(i+1)), Range(3,6));

             if(needReshapeR)
                 rvecs[i].reshape(1, 1).copyTo(r);
             else
             {
                 //*.t() is MatExpr (not Mat) so we can use assignment operator
                 CV_Assert(rvecs[i].rows == 3 && rvecs[i].cols == 1);
                 r = rvecs[i].t();
             }

             if(needReshapeT)
                 tvecs[i].reshape(1, 1).copyTo(t);
             else
             {
                 CV_Assert(tvecs[i].rows == 3 && tvecs[i].cols == 1);
                 t = tvecs[i].t();
             }
         }
         //fs.writeComment("a set of 6-tuples (rotation vector + translation vector) for each view");
         fs << "extrinsic_parameters" << bigmat;
     }

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

//! [run_and_save]
bool runCalibrationAndSave(Settings& s, Size imageSize, Mat& cameraMatrix, Mat& distCoeffs,
                           vector > imagePoints)
{
    vector rvecs, tvecs;
    rvecs.reserve(100);
    tvecs.reserve(100);
    vector reprojErrs;
    double totalAvgErr = 0;

    bool ok = runCalibration(s, imageSize, cameraMatrix, distCoeffs, imagePoints, rvecs, tvecs, reprojErrs,
                             totalAvgErr);
    cout << (ok ? "Calibration succeeded" : "Calibration failed")
         << ". avg re projection error = " << totalAvgErr << endl;

    if (ok)
    {
        // 保存到对象中
        cameraMatrix.copyTo(s.cameraMatrix);
        distCoeffs.copyTo(s.distCoeffs);
        s.reprojErrs = reprojErrs;
        s.totalAvgErr = totalAvgErr;
        s.imagePoints = imagePoints;
        // 保存到文件
        saveCameraParams(s, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs, reprojErrs, imagePoints, totalAvgErr);
    }
    return ok;
}
//! [run_and_save]

int singleCameraCalib(const string inputSettingsFile, Settings& s)
{
    //! [file_read]
    FileStorage fs(inputSettingsFile, FileStorage::READ); // Read the settings
    if (!fs.isOpened())
    {
        cout << "Could not open the configuration file: \"" << inputSettingsFile << "\"" << endl;
        return -1;
    }
    fs["Settings"] >> s;
    fs.release();                                         // close Settings file
    //! [file_read]

    if (!s.goodInput)
    {
        cout << "Invalid input detected. Application stopping. " << endl;
        return -1;
    }

    vector > imagePoints;
    Mat cameraMatrix, distCoeffs;
    Size imageSize;
    int mode = s.inputType == Settings::IMAGE_LIST ? CAPTURING : DETECTION;
    clock_t prevTimestamp = 0;
    const Scalar RED(0,0,255), GREEN(0,255,0);
    const char ESC_KEY = 27;

    Mat view;
    //! [get_input]
    for(;;)
    {
        bool blinkOutput = false;

        view = s.nextImage();

        //-----  If no more image, or got enough, then stop calibration and show result -------------
        if( mode == CAPTURING && imagePoints.size() >= (size_t)s.nrFrames )
        {
            if( runCalibrationAndSave(s, imageSize,  cameraMatrix, distCoeffs, imagePoints))
                mode = CALIBRATED;
            else
                mode = DETECTION;
        }
        if(view.empty())          // If there are no more images stop the loop
        {
            // if calibration threshold was not reached yet, calibrate now
            if( mode != CALIBRATED && !imagePoints.empty() )
            {
                runCalibrationAndSave(s, imageSize,  cameraMatrix, distCoeffs, imagePoints);
            }
            break;
        }
        //! [get_input]

        imageSize = view.size();  // Format input image.

        if( s.flipVertical )
        {
            flip( view, view, 0 );
        }

        //! [find_pattern]
        vector pointBuf;

        bool found;

        int chessBoardFlags = CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE;

        if(!s.useFisheye)
        {
            // fast check erroneously fails with high distortions like fisheye
            chessBoardFlags |= CALIB_CB_FAST_CHECK;
        }

        switch( s.calibrationPattern ) // Find feature points on the input format
        {
        case Settings::CHESSBOARD:
            found = findChessboardCorners( view, s.boardSize, pointBuf, chessBoardFlags);
            break;
        case Settings::CIRCLES_GRID:
            found = findCirclesGrid( view, s.boardSize, pointBuf );
            break;
        case Settings::ASYMMETRIC_CIRCLES_GRID:
            found = findCirclesGrid( view, s.boardSize, pointBuf, CALIB_CB_ASYMMETRIC_GRID );
            break;
        default:
            found = false;
            break;
        }
        //! [find_pattern]
        //! [pattern_found]
        if ( found)                // If done with success,
        {
            // improve the found corners' coordinate accuracy for chessboard
            if( s.calibrationPattern == Settings::CHESSBOARD)
            {
                Mat viewGray;
                if(view.channels() == 3)
                {
                    cvtColor(view, viewGray, COLOR_BGR2GRAY);
                }
                else
                {
                    view.copyTo(viewGray);
                }
                cornerSubPix( viewGray, pointBuf, Size(11,11),
                              Size(-1,-1), TermCriteria( TermCriteria::EPS+TermCriteria::COUNT, 30, 0.1 ));
            }

            // For camera only take new samples after delay time
            if( mode == CAPTURING &&
                    (!s.inputCapture.isOpened() || clock() - prevTimestamp > s.delay*1e-3*CLOCKS_PER_SEC) )
            {
                imagePoints.push_back(pointBuf);
                prevTimestamp = clock();
                blinkOutput = s.inputCapture.isOpened();
            }

            // Draw the corners.
            drawChessboardCorners( view, s.boardSize, Mat(pointBuf), found );
        }
        //! [pattern_found]
        //----------------------------- Output Text ------------------------------------------------
        //! [output_text]
        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 )
        {
            if(s.showUndistorsed)
                msg = format( "%d/%d Undist", (int)imagePoints.size(), s.nrFrames );
            else
                msg = format( "%d/%d", (int)imagePoints.size(), s.nrFrames );
        }

        putText( view, msg, textOrigin, 1, 1, mode == CALIBRATED ?  GREEN : RED);

        if( blinkOutput )
        {
            bitwise_not(view, view);
        }
        //! [output_text]
        //------------------------- Video capture  output  undistorted ------------------------------
        //! [output_undistorted]
        if( mode == CALIBRATED && s.showUndistorsed )
        {
            Mat temp = view.clone();
            undistort(temp, view, cameraMatrix, distCoeffs);
        }
        //! [output_undistorted]
        //------------------------------ Show image and check for input commands -------------------
        //! [await_input]
        imshow("Image View", view);
        char key = (char)waitKey(s.inputCapture.isOpened() ? 50 : s.delay);
        if( key  == ESC_KEY )
            break;

        if( key == 'u' && mode == CALIBRATED )
        {
            s.showUndistorsed = !s.showUndistorsed;
        }

        if( s.inputCapture.isOpened() && key == 'g' )
        {
            mode = CAPTURING;
            imagePoints.clear();
        }
        //! [await_input]
    }

    // -----------------------Show the undistorted image for the image list ------------------------
    //! [show_results]
    if( s.inputType == Settings::IMAGE_LIST && s.showUndistorsed )
    {
        Mat view, rview, map1, map2;

        if (s.useFisheye)
        {
            Mat newCamMat;
            fisheye::estimateNewCameraMatrixForUndistortRectify(cameraMatrix, distCoeffs, imageSize,
                                                                Matx33d::eye(), newCamMat, 1);
            fisheye::initUndistortRectifyMap(cameraMatrix, distCoeffs, Matx33d::eye(), newCamMat, imageSize,
                                             CV_16SC2, map1, map2);
        }
        else
        {
            initUndistortRectifyMap(
                        cameraMatrix, distCoeffs, Mat(),
                        getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 1, imageSize, 0), imageSize,
                        CV_16SC2, map1, map2);
        }

        for(size_t i = 0; i < s.imageList.size(); i++ )
        {
            view = imread(s.imageList[i], 1);
            if(view.empty())
                continue;
            remap(view, rview, map1, map2, INTER_LINEAR);
            imshow("Image View", rview);
            char c = (char)waitKey(1000);
            if( c  == ESC_KEY || c == 'q' || c == 'Q' )
                break;
        }
    }
    //! [show_results]

    return 0;
}

int sameSettings(Settings settingsLeft, Settings settingsRight)
{
    //左右相机标定板的物理特性配置不同时，不进行立体标定
    if((settingsLeft.boardSize.width != settingsRight.boardSize.width)
            ||(settingsLeft.boardSize.height != settingsRight.boardSize.height)
            ||(settingsLeft.squareSize != settingsRight.squareSize)
            ||(settingsLeft.calibrationPattern != settingsRight.calibrationPattern)
            || (settingsLeft.useFisheye ^ settingsRight.useFisheye))
    {
        return -1;
    }
    return 0;
}

int main()
{
    if(calibMode == SINGLECAMERACALIB)
    {
        const string inputSettingsFile = "singleCalib\\inputSettingFile.xml";
        const string calibResultSaveFile = "singleCalib\\calibResult.yml";
        Settings s;
        if(singleCameraCalib(inputSettingsFile, s))
        {
            cout << "Single Camera Calibrate Failed!" << endl;
        }
        else
        {
            cout << "Camera Calib Params Details Have Saved to File: " << s.outputFileName << endl;
        }
        // 保存标定结果
        singleResult.cameraMatrix = s.cameraMatrix;
        singleResult.distCoeffs = s.distCoeffs;
        singleResult.saveCameraParams(calibResultSaveFile);
    }
    else if(calibMode == STEREOCALIB)
    {
        // 立体标定只支持图片序列
        const string inputSettingFileLeft = "stereoCalib\\inputSettingFileLeft.xml";
        const string inputSettingFileRight = "stereoCalib\\inputSettingFileRight.xml";
        const string calibResultSaveFile = "stereoCalib\\calibResult.yml";
        Settings settingsLeft;
        Settings settingsRight;

        int leftOK = singleCameraCalib(inputSettingFileLeft, settingsLeft);
        if(leftOK)
        {
            cout << "Left Camera Calibrate Failed! " << endl;
            return -1;
        }

        int rightOK = singleCameraCalib(inputSettingFileRight, settingsRight);
        if(rightOK)
        {
            cout << "Right Camera Calibrate Failed! " << endl;
            return -1;
        }

        if(sameSettings(settingsLeft, settingsRight))
        {
            cout << "Left Calibrate Board is not same with Right Calibrate Board !" << endl;
            return -1;
        }

        vector::iterator iterLeft;
        vector::iterator iterRight;
        Mat viewLeft, viewRight, viewGrayLeft, viewGrayRight;
        Mat unDistortViewLeft, unDistortViewRight, rectifyViewLeft, rectifyViewRight;
        Size imageSize;
        Size rectifyImageSize;
        int chessBoardFlags = CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE;

        if(!settingsLeft.useFisheye)
        {
            // fast check erroneously fails with high distortions like fisheye
            chessBoardFlags |= CALIB_CB_FAST_CHECK;
        }
        int goodFrameCount = 0;
        vector imagePointBufferLeft, imagePointBufferRight;
        vector< vector > imagesPointBufferLeft, imagesPointBufferRight;

        // 获取匹配点
        for(iterLeft = settingsLeft.imageList.begin(), iterRight = settingsRight.imageList.begin();
            iterLeft != settingsLeft.imageList.end() && iterRight != settingsRight.imageList.end();
            ++iterLeft, ++iterRight)
        {
            viewLeft = imread(*iterLeft, CV_LOAD_IMAGE_UNCHANGED);
            viewRight = imread(*iterRight, CV_LOAD_IMAGE_UNCHANGED);

            if(!viewLeft.data || !viewRight.data)
            {
                cout << "read Image Pair:(" << *iterLeft << ", " << *iterRight <<") Failed !" << endl;
                continue;
            }

            if(viewLeft.channels() == 3)
            {
                cvtColor(viewLeft, viewGrayLeft, CV_BGR2GRAY);
            }
            else
            {
                viewLeft.copyTo(viewGrayLeft);
            }

            if(viewRight.channels() == 3)
            {
                cvtColor(viewRight, viewGrayRight, CV_BGR2GRAY);
            }
            else
            {
                viewRight.copyTo(viewGrayRight);
            }

            bool foundLeft = false;
            bool foundRight = false;

            switch(settingsLeft.calibrationPattern)
            {
            case Settings::CHESSBOARD:
                foundLeft = findChessboardCorners( viewGrayLeft, settingsLeft.boardSize, imagePointBufferLeft, chessBoardFlags);
                foundRight = findChessboardCorners( viewGrayRight, settingsRight.boardSize, imagePointBufferRight, chessBoardFlags);
                break;
            case Settings::CIRCLES_GRID:
                foundLeft = findCirclesGrid( viewGrayLeft, settingsLeft.boardSize, imagePointBufferLeft );
                foundRight = findCirclesGrid(viewGrayRight, settingsRight.boardSize, imagePointBufferRight);
                break;
            case Settings::ASYMMETRIC_CIRCLES_GRID:
                foundLeft = findCirclesGrid( viewGrayLeft, settingsLeft.boardSize, imagePointBufferLeft, CALIB_CB_ASYMMETRIC_GRID );
                foundRight = findCirclesGrid(viewGrayRight, settingsRight.boardSize, imagePointBufferRight, CALIB_CB_ASYMMETRIC_GRID);
                break;
            default:
                foundLeft = false;
                foundRight = false;
                break;
            }

            if(foundLeft && foundRight)
            {
                if( settingsLeft.calibrationPattern == Settings::CHESSBOARD)
                {
                    cornerSubPix( viewGrayLeft, imagePointBufferLeft, Size(11,11),
                                  Size(-1,-1), TermCriteria( TermCriteria::EPS+TermCriteria::COUNT, 30, 0.1 ));
                    cornerSubPix( viewGrayRight, imagePointBufferRight, Size(11,11),
                                  Size(-1,-1), TermCriteria( TermCriteria::EPS+TermCriteria::COUNT, 30, 0.1 ));
                }
                imagesPointBufferLeft.push_back(imagePointBufferLeft);
                imagesPointBufferRight.push_back(imagePointBufferRight);
                //获取图像的大小
                imageSize.height = viewGrayLeft.rows;
                imageSize.width = viewGrayLeft.cols;
                goodFrameCount++;
            }
        }//for iterLeft && iterRight

        // 获取标定板的实际3维坐标
        vector > imagesRealPoint(1);
        calcBoardCornerPositions(settingsLeft.boardSize, settingsLeft.squareSize, imagesRealPoint[0], settingsLeft.calibrationPattern);
        imagesRealPoint.resize(goodFrameCount,imagesRealPoint[0]);

        Mat cameraMatrixLeft = settingsLeft.cameraMatrix;
        Mat distCoeffsLeft  = settingsLeft.distCoeffs;

        Mat cameraMatrixRight = settingsRight.cameraMatrix;
        Mat distCoeffsRight = settingsRight.distCoeffs;

        double stereoError = 0.0;
        Mat R, T;                         // 旋转矩阵和平移矩阵
        Mat E, F;                         // 本征矩阵和基础矩阵
        Mat R1, R2, P1, P2, Q;            // 左右相机旋转矩阵、左右单相机投影矩阵、视差-深度变换矩阵
        Rect validRIOLeft, validRIORight; // 普通相机校正后的左右视图的有效区域
        rectifyImageSize = imageSize;
        Mat mapLx, mapLy, mapRx, mapRy;   // remap的网格
        if(settingsLeft.useFisheye)
        {
            int iter, iter2;
            vector > imagesRealPoint3d;
            vector > imagesPointBufferLeft2d;
            vector > imagesPointBufferRight2d;
            for (iter = 0; iter < imagesRealPoint.size(); ++iter)
            {
                vector imageRealPoint3d;
                vector imagePointBufferLeft2d;
                vector imagePointBufferRight2d;
                Point3f point3f;
                Point2f pointLeft2f;
                Point2f pointRight2f;
                Point3d point3d;
                Point2d pointLeft2d;
                Point2d pointRight2d;
                for (iter2 = 0; iter2 

3.2 相机标定实现：

#ifndef SINGLENARROWANGLECAMERACALIB_H
#define SINGLENARROWANGLECAMERACALIB_H
#include 
#include 
#include 
#include 

#include 
#include 
#include 
#include 
#include 
#include 
#include 

#ifndef _CRT_SECURE_NO_WARNINGS
# define _CRT_SECURE_NO_WARNINGS
#endif

using namespace cv;
using namespace std;

class Settings
{
public:
    Settings() : goodInput(false) {}
    enum Pattern { NOT_EXISTING, CHESSBOARD, CIRCLES_GRID, ASYMMETRIC_CIRCLES_GRID };
    enum InputType { INVALID, CAMERA, VIDEO_FILE, IMAGE_LIST };

    void write(FileStorage& fs) const;                         //Write serialization for this class
    void read(const FileNode& node);                           //Read serialization for this class
    void validate();
    Mat nextImage();

    static bool readStringList( const string& filename, vector& l );
public:
    Size boardSize;              // The size of the board -> Number of items by width and height
    Pattern calibrationPattern;  // One of the Chessboard, circles, or asymmetric circle pattern
    float squareSize;            // The size of a square in your defined unit (point, millimeter,etc).
    int nrFrames;                // The number of frames to use from the input for calibration
    float aspectRatio;           // The aspect ratio
    int delay;                   // In case of a video input
    bool writePoints;            // Write detected feature points
    bool writeExtrinsics;        // Write extrinsic parameters
    bool calibZeroTangentDist;   // Assume zero tangential distortion
    bool calibFixPrincipalPoint; // Fix the principal point at the center
    bool flipVertical;           // Flip the captured images around the horizontal axis
    string outputFileName;       // The name of the file where to write
    bool showUndistorsed;        // Show undistorted(去畸变的图像) images after calibration
    string input;                // The input ->
    bool useFisheye;             // use fisheye camera model for calibration
    bool fixK1;                  // fix K1 distortion coefficient
    bool fixK2;                  // fix K2 distortion coefficient
    bool fixK3;                  // fix K3 distortion coefficient
    bool fixK4;                  // fix K4 distortion coefficient
    bool fixK5;                  // fix K5 distortion coefficient

    int cameraID;
    vector imageList;
    size_t atImageList;
    VideoCapture inputCapture;
    InputType inputType;
    bool goodInput;
    int flag;

    Mat cameraMatrix;
    Mat distCoeffs;
    vector reprojErrs;
    vector > imagePoints;
    double totalAvgErr;
private:
    string patternToUse;
};

#endif // SINGLENARROWANGLECAMERACALIB_H

#include "singlecameracalib.h"

void Settings::write(FileStorage& fs) const                        //Write serialization for this class
{
    fs << "{"
       << "BoardSize_Width"  << boardSize.width
       << "BoardSize_Height" << boardSize.height
       << "Square_Size"         << squareSize
       << "Calibrate_Pattern" << patternToUse
       << "Calibrate_NrOfFrameToUse" << nrFrames
       << "Calibrate_FixAspectRatio" << aspectRatio
       << "Calibrate_AssumeZeroTangentialDistortion" << calibZeroTangentDist
       << "Calibrate_FixPrincipalPointAtTheCenter" << calibFixPrincipalPoint

       << "Write_DetectedFeaturePoints" << writePoints
       << "Write_extrinsicParameters"   << writeExtrinsics
       << "Write_outputFileName"  << outputFileName

       << "Show_UndistortedImage" << showUndistorsed

       << "Input_FlipAroundHorizontalAxis" << flipVertical
       << "Input_Delay" << delay
       << "Input" << input
       << "}";
}

void Settings::read(const FileNode& node)                          //Read serialization for this class
{
    node["BoardSize_Width" ] >> boardSize.width;
    node["BoardSize_Height"] >> boardSize.height;
    node["Calibrate_Pattern"] >> patternToUse;
    node["Square_Size"]  >> squareSize;
    node["Calibrate_NrOfFrameToUse"] >> nrFrames;
    node["Calibrate_FixAspectRatio"] >> aspectRatio;
    node["Write_DetectedFeaturePoints"] >> writePoints;
    node["Write_extrinsicParameters"] >> writeExtrinsics;
    node["Write_outputFileName"] >> outputFileName;
    node["Calibrate_AssumeZeroTangentialDistortion"] >> calibZeroTangentDist;
    node["Calibrate_FixPrincipalPointAtTheCenter"] >> calibFixPrincipalPoint;
    node["Calibrate_UseFisheyeModel"] >> useFisheye;
    node["Input_FlipAroundHorizontalAxis"] >> flipVertical;
    node["Show_UndistortedImage"] >> showUndistorsed;
    node["Input"] >> input;
    node["Input_Delay"] >> delay;
    node["Fix_K1"] >> fixK1;
    node["Fix_K2"] >> fixK2;
    node["Fix_K3"] >> fixK3;
    node["Fix_K4"] >> fixK4;
    node["Fix_K5"] >> fixK5;

    validate();
}

void Settings::validate()
{
    goodInput = true;
      if (boardSize.width <= 0 || boardSize.height <= 0)
      {
          cerr << "Invalid Board size: " << boardSize.width << " " << boardSize.height << endl;
          goodInput = false;
      }
      if (squareSize <= 10e-6)
      {
          cerr << "Invalid square size " << squareSize << endl;
          goodInput = false;
      }
      if (nrFrames <= 0)
      {
          cerr << "Invalid number of frames " << nrFrames << endl;
          goodInput = false;
      }

      if (input.empty())      // Check for valid input
              inputType = INVALID;
      else
      {
          if (input[0] >= '0' && input[0] <= '9')
          {
              stringstream ss(input);
              ss >> cameraID;
              inputType = CAMERA;
          }
          else
          {
              if (readStringList(input, imageList))
              {
                  inputType = IMAGE_LIST;
                  nrFrames = (nrFrames < (int)imageList.size()) ? nrFrames : (int)imageList.size();
              }
              else
                  inputType = VIDEO_FILE;
          }
          if (inputType == CAMERA)
              inputCapture.open(cameraID);
          if (inputType == VIDEO_FILE)
              inputCapture.open(input);
          if (inputType != IMAGE_LIST && !inputCapture.isOpened())
                  inputType = INVALID;
      }
      if (inputType == INVALID)
      {
          cerr << " Input does not exist: " << input;
          goodInput = false;
      }

      flag = 0;
      if(calibFixPrincipalPoint) flag |= CALIB_FIX_PRINCIPAL_POINT;
      if(calibZeroTangentDist)   flag |= CALIB_ZERO_TANGENT_DIST;
      if(aspectRatio)            flag |= CALIB_FIX_ASPECT_RATIO;
      if(fixK1)                  flag |= CALIB_FIX_K1;
      if(fixK2)                  flag |= CALIB_FIX_K2;
      if(fixK3)                  flag |= CALIB_FIX_K3;
      if(fixK4)                  flag |= CALIB_FIX_K4;
      if(fixK5)                  flag |= CALIB_FIX_K5;

      if (useFisheye) {
          // the fisheye model has its own enum, so overwrite the flags
          flag = fisheye::CALIB_FIX_SKEW | fisheye::CALIB_RECOMPUTE_EXTRINSIC;
          if(fixK1)                  flag |= fisheye::CALIB_FIX_K1;
          if(fixK2)                  flag |= fisheye::CALIB_FIX_K2;
          if(fixK3)                  flag |= fisheye::CALIB_FIX_K3;
          if(fixK4)                  flag |= fisheye::CALIB_FIX_K4;
      }

      calibrationPattern = NOT_EXISTING;
      if (!patternToUse.compare("CHESSBOARD")) calibrationPattern = CHESSBOARD;
      if (!patternToUse.compare("CIRCLES_GRID")) calibrationPattern = CIRCLES_GRID;
      if (!patternToUse.compare("ASYMMETRIC_CIRCLES_GRID")) calibrationPattern = ASYMMETRIC_CIRCLES_GRID;
      if (calibrationPattern == NOT_EXISTING)
      {
          cerr << " Camera calibration mode does not exist: " << patternToUse << endl;
          goodInput = false;
      }
      atImageList = 0;

}

Mat Settings::nextImage()
{
    Mat result;
    if( inputCapture.isOpened() )
    {
        Mat view0;
        inputCapture >> view0;
        view0.copyTo(result);
    }
    else if( atImageList < imageList.size() )
        result = imread(imageList[atImageList++], IMREAD_COLOR);

    return result;
}

bool Settings::readStringList( const string& filename, vector& l )
{
    l.clear();
    FileStorage fs(filename, FileStorage::READ);
    if( !fs.isOpened() )
        return false;
    FileNode n = fs.getFirstTopLevelNode();
    if( n.type() != FileNode::SEQ )
        return false;
    FileNodeIterator it = n.begin(), it_end = n.end();
    for( ; it != it_end; ++it )
        l.push_back((string)*it);
    return true;
}

3.3 相机标定配置文件说明：

3.3.1 相机标定

singleCalibImages内容：

imagesFileName.xml内容：

singleCalib\\singleCalibImages\\20170808175704.jpg
singleCalib\\singleCalibImages\\20170808175708.jpg
singleCalib\\singleCalibImages\\20170808175714.jpg
singleCalib\\singleCalibImages\\20170808175718.jpg
singleCalib\\singleCalibImages\\20170808175724.jpg
singleCalib\\singleCalibImages\\20170808175729.jpg
singleCalib\\singleCalibImages\\20170808175736.jpg
singleCalib\\singleCalibImages\\20170808175742.jpg
singleCalib\\singleCalibImages\\20170808175754.jpg
singleCalib\\singleCalibImages\\20170808175801.jpg
singleCalib\\singleCalibImages\\20170808175807.jpg
singleCalib\\singleCalibImages\\20170808175809.jpg
singleCalib\\singleCalibImages\\20170808175812.jpg
singleCalib\\singleCalibImages\\20170808175815.jpg
singleCalib\\singleCalibImages\\20170808175826.jpg
singleCalib\\singleCalibImages\\20170808175833.jpg
singleCalib\\singleCalibImages\\20170808175839.jpg
singleCalib\\singleCalibImages\\20170808175845.jpg
singleCalib\\singleCalibImages\\20170808175850.jpg
singleCalib\\singleCalibImages\\20170808175857.jpg

inputSettingFile.xml文件内容

  
  8
  6
  
  
  67
  
   
  "CHESSBOARD"
  
  
  "singleCalib\\imagesFileName.xml"
  
  0
  
  
  100	
  
  
  13
  
  1
  
  0
  
  0
  
  "singleCalib\\outputCameraParams.xml"
  
  1
  
  1
  
  1
   
  1
  
  
  0
  
  0
  
  0
  
  0
  
  1

3.3.2 立体相机标定

其中outputCameraParamsLeft.xml和outputCameraParamsRight.xml由程序生成

3.4. 运行部分截图