相机/立体标定——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 <iostream>
#include <string>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/core/core.hpp>
#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<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, bool fisheye)
{
    vector<Point2f> 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<Point3f>& 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<Point3d>& 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<vector<Point2f> > imagePoints, vector<Mat>& rvecs, vector<Mat>& tvecs,
                            vector<float>& reprojErrs,  double& totalAvgErr)
{
    //! [fixed_aspect]
    cameraMatrix = Mat::eye(3, 3, CV_64F);
    if( s.flag & CALIB_FIX_ASPECT_RATIO )
    {
        cameraMatrix.at<double>(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<vector<Point3f> > 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<cv::Vec3f> _rvecs;
        std::vector<cv::Vec3f>  _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<float>(0,0) = vec[0];
            tempR.at<float>(1,1) = vec[1];
            tempR.at<float>(2,2) = vec[2];

            vec = _tvecs[i];
            tempT.at<float>(0,0) = vec[0];
            tempT.at<float>(1,1) = vec[1];
            tempT.at<float>(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<Mat>& rvecs, const vector<Mat>& tvecs,
                              const vector<float>& reprojErrs, const vector<vector<Point2f> >& 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 ? " +