SLAM之路－OpenCV3张正友相机标定

版权声明：Davidwang原创文章，严禁用于任何商业途径，授权后方可转载。

  由于相机使用的镜头制造工艺及安装工艺存在精度误差，造成投影后的像素发生畸变，这会给以图像输入为主的视觉分析带来干扰，严重时导致算法完全失效。通常的解决方案是通过相机标定获取到相机的内参数据，然后用这些参数校正输入的图像。相机内参是对相机制造、安装环节出现偏差的参数化表达，有的相机会在出厂时提供内参数据，有的则不提供，对不提供内参数数据的相机，我们需要自己通过算法拿到这些数据，这就是本节所要讲述的相机标定内容。

  标定相机通常采用张正友棋盘格标定法，通过相机拍摄打印好的棋盘格图像，利用OpenCV3可以通过调用其内置算法拿到相机内参，理想情况下只需要3张棋盘格图像即可，一般为提高鲁棒性，会使用10余张图像，其流程如下图所示。

  下面以上面的流程图为准介绍各步骤及其所涉及到的OpenCV3函数。

（一）提取角点

  提取角点即是提取图像的特征，以便在各图片间匹配特征点，用到的函数为findChessboardCorners()，该函数原型为：

CV_EXPORTS_W bool findChessboardCorners( InputArray image, Size patternSize,
                                         OutputArray corners,
                                         int flags=CALIB_CB_ADAPTIVE_THRESH+CALIB_CB_NORMALIZE_IMAGE );

  其各参数意义如下表所示：

参数	描述
image	输入的棋盘图Mat图像，必须是8位的灰度或者彩色图像
patternSize	每个棋盘图上内角点的行列数，行列数尽量不要相同，便于后续标定程序识别标定板的方向
corners	用于存储检测到的内角点图像坐标位置，一般用元素是Point2f的向量来表示：vector image_points_buf
flags	用于定义棋盘图上内角点查找的处理方式，通常使用默认值即可

（二）提取亚像素点

  为降低相机标定偏差，提高精度，通常还需要在初步提取的角点信息上进一步提取亚像素信息。提取亚像素信息有两种方法：cornerSubPix()、find4QuadCornerSubpix()，这两种方法都可以提取到图像亚像素信息，本节我们使用find4QuadCornerSubpix()方法，该方法原型如下：

CV_EXPORTS bool find4QuadCornerSubpix(InputArray img, InputOutputArray corners, Size region_size);

  其各参数意义如下表所示：

参数	描述
img	输入的棋盘图Mat图像，必须是8位的灰度或者彩色图像
corners	初始的角点坐标向量，同时作为亚像素坐标位置的输出，浮点型数据，一般用元素是Pointf2f/Point2d的向量来表示：vector iamgePointsBuf
region_size	角点搜索窗口尺寸

（三）标定相机

  经过上述处理，获取到棋盘标定图的内角点图像坐标之后，就可以使用calibrateCamera()函数进行标定，计算相机内参数据，calibrateCamera()函数原型：

CV_EXPORTS_W double calibrateCamera( InputArrayOfArrays objectPoints,
                                     InputArrayOfArrays imagePoints,
                                     Size imageSize,
                                     CV_OUT InputOutputArray cameraMatrix,
                                     CV_OUT InputOutputArray distCoeffs,
                                     OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs,
                                     int flags=0, TermCriteria criteria = TermCriteria(
                                         TermCriteria::COUNT+TermCriteria::EPS, 30, DBL_EPSILON) );

  其各参数意义如下表所示：

参数	描述
objectPoints	相机坐标系中的角点三维坐标。在使用时，输入一个三维坐标点的向量的向量，即vector，每一个内层vector表示一张图像上的内角点坐标对应的相机坐标
imagePoints	为每一个内角点对应的图像坐标点。在使用时，输入一个二维坐标点的向量的向量，即vector，每一个内层vector表示一张图像上的所有内角点坐标
imageSize	为图像的像素尺寸大小，在计算相机的内参和畸变矩阵时需要使用到该参数
cameraMatrix	相机的内参矩阵。输入一个Mat cameraMatrix即可，如Mat cameraMatrix=Mat(3,3,CV_32FC1,Scalar::all(0))
distCoeffs	distCoeffs为畸变矩阵。摄像机的5个畸变系数：k1,k2,p1,p2,k3，使用如Mat distCoeffs=Mat(1,5,CV_32FC1,Scalar::all(0))
rvecs	相机外参旋转向量，应该输入一个Mat类型的vector，如vector
tvecs	相机外参平移向量，应该输入一个Mat类型的vector，如vector
flags	标定时所采用的算法。有如下几个参数：CV_CALIB_USE_INTRINSIC_GUESS：使用该参数时，在cameraMatrix矩阵中应该有fx,fy,u0,v0的估计值。否则的话，将初始化(u0,v0）图像的中心点，使用最小二乘估算出fx，fy；CV_CALIB_FIX_PRINCIPAL_POINT：在进行优化时会固定光轴点。当CV_CALIB_USE_INTRINSIC_GUESS参数被设置，光轴点将保持在中心或者某个输入的值；CV_CALIB_FIX_ASPECT_RATIO：固定fx/fy的比值，只将fy作为可变量，进行优化计算。当CV_CALIB_USE_INTRINSIC_GUESS没有被设置，fx和fy将会被忽略。只有fx/fy的比值在计算中会被用到；CV_CALIB_ZERO_TANGENT_DIST：设定切向畸变参数（p1,p2）为零；CV_CALIB_FIX_K1,…,CV_CALIB_FIX_K6：对应的径向畸变在优化中保持不变；CV_CALIB_RATIONAL_MODEL：计算k4，k5，k6三个畸变参数。如果没有设置，则只计算其它5个畸变参数。
criteria	迭代终止条件，通常使用默认值即可

  可以看到，该函数中的参数有的是用作输入，有的是用作输出，如果没有错误，该函数会返回相机的内参数矩阵和外参数旋转向量和平移向量。

（四）验证

  为验证每一个步骤中的流程是否正确，可以通过查看每一步骤执行的结果，如可以通过drawChessboardCorners()函数绘制被成功标定的角点，还可以通过重投影函数projectPoints()，该函数对空间的三维点进行重新投影计算，得到空间三维点在图像上新的投影点的坐标，计算投影坐标和亚像素角点坐标之间的偏差，评定标定结果，偏差越小越好。projectPoints()函数原型如下：

CV_EXPORTS_W void projectPoints( InputArray objectPoints,
                                 InputArray rvec, InputArray tvec,
                                 InputArray cameraMatrix, InputArray distCoeffs,
                                 OutputArray imagePoints,
                                 OutputArray jacobian=noArray(),
                                 double aspectRatio=0 );

  其各参数意义如下表所示：

参数	描述
objectPoints	从图像中提出计算的相机坐标系中的三维点坐标
rvec	旋转向量
tvec	平移向量
cameraMatrix	相机内参矩阵
distCoeffs	相机畸变参数
jacobian	雅可比矩阵
aspectRatio	相机传感器的感光单元有关的可选参数，如果设置为非0，则函数默认感光单元的dx/dy是固定的，会依此对雅可比矩阵进行调整

  通过projectPoints()重投影方法可以得到理性的误差数据。

（五）矫正

  通过上述方法得到相机内参数据和外参数据后，就可以对图像进行畸变的矫正，通常有两种矫正方法，下面分别予以说明。

  方法一
  方法一使用initUndistortRectifyMap()和remap()两个函数配合实现，initUndistortRectifyMap()函数用于计算畸变映射，remap()函数将映射结果应用到图像上，initUndistortRectifyMap()函数原型如下：

CV_EXPORTS_W void initUndistortRectifyMap( InputArray cameraMatrix, InputArray distCoeffs,
                           InputArray R, InputArray newCameraMatrix,
                           Size size, int m1type, OutputArray map1, OutputArray map2 );

  其各参数意义如下表所示：

参数	描述
cameraMatrix	相机内参矩阵
distCoeffs	相机畸变因子
R	旋转矩阵
newCameraMatrix	校正后的3X3摄像机矩阵，默认跟cameraMatrix保持一致
size	图像尺寸
m1type	定义map1的数据类型，可以是CV_32FC1或者CV_16SC2
map1	输出的X坐标重映射参数
map2	输出的Y坐标重映射参数

  remap()函数原型如下：

CV_EXPORTS_W void remap( InputArray src, OutputArray dst,
                         InputArray map1, InputArray map2,
                         int interpolation, int borderMode=BORDER_CONSTANT,
                         const Scalar& borderValue=Scalar());

  其各参数意义如下表所示：

参数	描述
src	原始图像
dst	矫正后的输出图像
map1	X坐标重映射参数
map2	Y坐标重映射参数
interpolation	像素插值方式
borderMode	边界填充方式
Scalar	边界填充值，默认为黑色

  方法二
  使用undistort()函数实现，该方法更简洁，内部处理了重映射相关操作，但通常方法一比方法二效率更好一些，undistort()函数原型如下：

CV_EXPORTS_W void undistort( InputArray src, OutputArray dst,
                             InputArray cameraMatrix,
                             InputArray distCoeffs,
                             InputArray newCameraMatrix=noArray() );

  其各参数意义如下表所示：

参数	描述
src	原始图像
dst	矫正后的输出图像
cameraMatrix	相机内参矩阵
distCoeffs	相机畸变因子
newCameraMatrix	校正后的3X3摄像机矩阵，默认跟cameraMatrix保持一致

（六）原代码

  通过上述基础知识，我们就可以很轻松的写出相机标定程序，具体的代码如下，在有基础知识及详细的注释，相信代码很容易看明白。

/*
* Calibration camera
* author:Davidwang
* date  :2020.08.21
*/
#include "opencv2/core/core.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include 
#include 

using namespace cv;
using namespace std;

Size const cornerSize = Size(7, 9);                       //定义标定板每行、每列角点数
Size squareSize = Size(20, 20);                           //实际测量得到的标定板上每个棋盘格的尺寸，单位mm
String const calibrationFileName = "calibrationData.txt"; //标定所用图像文件的路径
String const resultFileName = "calibrationResult.txt";    //保存标定结果的文件
String const rectifyFileName = "images/001.jpg";          //待校正的图像文件
String const rectifiedFileName = "images/rectified.jpg";  //校正后的图像文件

struct calibrationCache
{
    vector<Point2f> cornerPointBuffer;          //缓存每幅图像上检测到的角点
    vector<vector<Point2f>> cornerPointSequece; //保存检测到的所有角点
    vector<vector<Point3f>> cornerPostion;      //保存标定板上角点的三维坐标
    int imageCount = 0;                         //采样图像数量
    int cornerCount = 0;                        //检测到的角点数量
    Size imageSize;                             //输入图像的像素尺寸

    //----结果部分---------//
    Mat intrinsicMatrix = Mat(3, 3, CV_32FC1, Scalar::all(0)); //摄像机内参数矩阵
    Mat distCoeffs = Mat(1, 5, CV_32FC1, Scalar::all(0));      //摄像机的5个畸变系数：k1,k2,p1,p2,k3
    vector<Mat> tvecsMat;                                      //每幅图像的旋转向量
    vector<Mat> rvecsMat;                                      //每幅图像的平移向量
};
//----函数声明---------//
void drawIMage(Mat img, vector<Point2f> cornerBuffer);
void outPutCornerInfo(calibrationCache &cache);
void calibrationCamera(calibrationCache &cache);
void caculateErrorAndSaveResult(calibrationCache &cache);
void unDistortRectifyImage(calibrationCache &cache);

int main()
{
    ifstream fin(calibrationFileName);
    if (!fin)
    {
        cerr << "请在有calibrationFileName定义的文件目录下运行此程序" << endl;
        return 1;
    }
    calibrationCache calibCache;
    string imageName;

    cout << "开始读取图像..." << endl;
    while (getline(fin, imageName))
    {
        calibCache.imageCount++;
        cout << "imageCount = " << calibCache.imageCount << endl;
        Mat imageInput = imread(imageName);
        if (calibCache.imageCount == 1) //读入第一张图片时获取图像宽高信息
        {
            calibCache.imageSize.width = imageInput.cols;
            calibCache.imageSize.height = imageInput.rows;
            cout << "image size width  = " << calibCache.imageSize.width << endl;
            cout << "image size height = " << calibCache.imageSize.height << endl;
        }

        cout << "开始提取第" << calibCache.imageCount << "张图像的角点..." << endl;
        if (0 == findChessboardCorners(imageInput, cornerSize, calibCache.cornerPointBuffer))
        {
            cout << "无法提取第" << calibCache.imageCount << "张图像的角点，程序将跳过该图片！" << endl;
            calibCache.imageCount--;
            continue;
        }
        else
        {
            Mat grayImage;
            cvtColor(imageInput, grayImage, CV_RGB2GRAY);

            find4QuadCornerSubpix(grayImage, calibCache.cornerPointBuffer, Size(5, 5)); //对提取的角点进行精细化
            calibCache.cornerPointSequece.push_back(calibCache.cornerPointBuffer);      //保存亚像素角点

            drawIMage(grayImage, calibCache.cornerPointBuffer); //在图像上显示角点位置
        }
    }
    cout << "角点提取完成！" << endl
         << endl;
    outPutCornerInfo(calibCache);

    cout << "开始标定..." << endl;
    calibrationCamera(calibCache);

    cout << "开始评价标定结果..." << endl;
    caculateErrorAndSaveResult(calibCache);

    cout << "验证效果..." << endl;
    unDistortRectifyImage(calibCache);
    return 0;
}

///显示图像
void drawIMage(Mat img, vector<Point2f> cornerBuffer)
{
    drawChessboardCorners(img, cornerSize, cornerBuffer, false); //用于在图片中标记角点
    imshow("Camera Calibration", img);                           //显示图片
    waitKey(1000);                                               //暂停1
}
///输出所有角点信息
void outPutCornerInfo(calibrationCache &cache)
{
    int SampleImageCount = cache.cornerPointSequece.size();
    cout << "采样图像数= " << SampleImageCount << endl;
    if (SampleImageCount < 1)
        exit(1);
    for (int j = 0; j < cache.cornerPointSequece.size(); j++)
    {
        cout << endl;
        cout << "第" << j + 1 << "张图片的角点数据: " << endl;
        for (int i = 0; i < cache.cornerPointSequece[j].size(); i++)
        {
            cout << "(X:" << cache.cornerPointSequece[j][i].x << ",Y:" << cache.cornerPointSequece[j][i].y << ")"
                 << "      ";
            if (0 == (i + 1) % 4) // 格式化输出，便于控制台查看
            {
                cout << endl;
            }
        }
        cout << endl;
    }
    cout << endl;
}
///标定相机
void calibrationCamera(calibrationCache &cache)
{
    int i, j, t;
    for (t = 0; t < cache.imageCount; t++)
    {
        vector<Point3f> tempCornerPosition;
        for (i = 0; i < cornerSize.height; i++)
        {
            for (j = 0; j < cornerSize.width; j++)
            {
                Point3f cornerPos;
                cornerPos.x = i * squareSize.width;
                cornerPos.y = j * squareSize.height;
                cornerPos.z = 0; //假设标定板放在世界坐标系中z=0的平面上
                tempCornerPosition.push_back(cornerPos);
            }
        }
        cache.cornerPostion.push_back(tempCornerPosition);
    }
    //开始标定相机
    calibrateCamera(cache.cornerPostion, cache.cornerPointSequece, cache.imageSize, cache.intrinsicMatrix, cache.distCoeffs, cache.rvecsMat, cache.tvecsMat, 0);
    cout << "标定完成！" << endl
         << endl;
}
//评估误差并保存结果
void caculateErrorAndSaveResult(calibrationCache &cache)
{
    int cornerCount = cornerSize.width * cornerSize.height; //每幅图像中角点数量，假定每幅图像中都可以看到完整的标定板
    double totalError = 0.0;                                //所有图像的平均误差的总和
    double sigleError = 0.0;                                //每幅图像的平均误差
    vector<Point2f> reprojectPoints;                        //保存重新计算得到的投影点
    ofstream fout(resultFileName);
    cout << "每幅图像的标定误差：" << endl;
    fout << "每幅图像的标定误差：\n";
    for (int i = 0; i < cache.imageCount; i++)
    {
        vector<Point3f> tempPointSet = cache.cornerPostion[i];
        // 通过得到的摄像机内外参数，对空间的三维点进行重新投影计算，得到新的投影点
        projectPoints(tempPointSet, cache.rvecsMat[i], cache.tvecsMat[i], cache.intrinsicMatrix, cache.distCoeffs, reprojectPoints);
        // 计算新的投影点和旧的投影点之间的误差
        vector<Point2f> oldImagePoint = cache.cornerPointSequece[i];
        Mat oldImagePointMatrix = Mat(1, oldImagePoint.size(), CV_32FC2);
        Mat reprojectPointsMatrix = Mat(1, reprojectPoints.size(), CV_32FC2);
        for (int j = 0; j < oldImagePoint.size(); j++)
        {
            reprojectPointsMatrix.at<Vec2f>(0, j) = Vec2f(reprojectPoints[j].x, reprojectPoints[j].y);
            oldImagePointMatrix.at<Vec2f>(0, j) = Vec2f(oldImagePoint[j].x, oldImagePoint[j].y);
        }
        sigleError = norm(reprojectPointsMatrix, oldImagePointMatrix, NORM_L2);
        totalError += (sigleError /= cornerCount);
        cout << "第" << i + 1 << "幅图像的平均误差：" << sigleError << "像素" << endl;
        fout << "第" << i + 1 << "幅图像的平均误差：" << sigleError << "像素" << endl;
    }
    cout << "总体平均误差：" << totalError / cache.imageCount << "像素" << endl;
    fout << "总体平均误差：" << totalError / cache.imageCount << "像素" << endl
         << endl;
    cout << "评价完成！" << endl
         << endl;

    cout << "开始保存定标结果..." << endl;
    Mat rotationMatrix = Mat(3, 3, CV_32FC1, Scalar::all(0)); //保存每幅图像的旋转矩阵
    fout << "相机内参数矩阵：" << endl;
    cout << "相机内参数矩阵：" << endl;
    fout << cache.intrinsicMatrix << endl
         << endl;
    cout << cache.intrinsicMatrix << endl
         << endl;
    fout << "畸变系数：\n";
    cout << "畸变系数：" << endl;
    fout << cache.distCoeffs << endl
         << endl
         << endl;
    cout << cache.distCoeffs << endl
         << endl
         << endl;
    for (int i = 0; i < cache.imageCount; i++)
    {
        fout << "第" << i + 1 << "幅图像的旋转向量：" << endl;
        cout << "第" << i + 1 << "幅图像的旋转向量：" << endl;
        fout << cache.rvecsMat[i] << endl;
        cout << cache.rvecsMat[i] << endl;
        /* 罗德里格斯公式，将旋转向量转换为旋转矩阵 */
        Rodrigues(cache.rvecsMat[i], rotationMatrix);
        fout << "第" << i + 1 << "幅图像的旋转矩阵：" << endl;
        cout << "第" << i + 1 << "幅图像的旋转矩阵：" << endl;
        fout << rotationMatrix << endl;
        cout << rotationMatrix << endl;
        fout << "第" << i + 1 << "幅图像的平移向量：" << endl;
        cout << "第" << i + 1 << "幅图像的平移向量：" << endl;
        fout << cache.tvecsMat[i] << endl
             << endl;
        cout << cache.tvecsMat[i] << endl
             << endl;
    }
    cout << "完成保存" << endl
         << endl;
    fout << endl;
}
///校正图像
void unDistortRectifyImage(calibrationCache &cache)
{
    Mat sourceImage = imread(rectifyFileName);
    Mat rectifyImage = sourceImage.clone();
    Size imageSize = Size(sourceImage.cols, sourceImage.rows);
    Mat firstMap = Mat(imageSize, CV_32FC1);
    Mat secondMap = Mat(imageSize, CV_32FC1);
    Mat I = Mat::eye(3, 3, CV_32F);
    cout << "获取校正图..." << endl;
    initUndistortRectifyMap(cache.intrinsicMatrix, cache.distCoeffs, I, cache.intrinsicMatrix, imageSize, CV_32FC1, firstMap, secondMap);

    remap(sourceImage, rectifyImage, firstMap, secondMap, INTER_LINEAR);
    //undistort(sourceImage, rectifyImage, cache.intrinsicMatrix, cache.distCoeffs); //另一种不需要转换矩阵的方式
    imwrite(rectifiedFileName, rectifyImage);
    std::cout << "校正结束" << endl;
    imshow("rectified image", rectifyImage); //显示校正后图片
    waitKey(1000);                           //暂停1秒
}

  CMakeLists如下

# 声明要求的 cmake 最低版本
cmake_minimum_required(VERSION 2.8)
# 声明一个 cmake 工程
project(Calibration)
# 设置编译模式
set(CMAKE_BUILD_TYPE "Debug")
find_package(OpenCV REQUIRED)
# 添加可执行程序
add_executable(calibration Calibration.cpp)
# 将库文件链接到可执行程序上
target_link_libraries(calibration ${OpenCV_LIBS})

  代码在Ubuntu18.04，OpenCV3.4.11环境下编译通过。

（七）效果

  经过我们测试，上述代码能比较好的实现相机内参与外内的标定，相关效果图如下：

（八）相关资源

  相关资源包括：标定图纸、测试图像、矫正验证图像。标定图纸可直接A4打印使用，不用另行处理 ，测试图像与验证图像本人亲自拍摄，验证图像可供矫正验证。请从[这里]下载。

（九）参考文献

  1、张正友相机标定Opencv实现以及标定流程&&标定结果评价&&图像矫正流程解析（附标定程序和棋盘图）
  2、Camera Calibration and 3D Reconstruction
  3、calibrateCamera() 原理