相机标定程序详解<2>

#ifndef _CAMERACALIBRATE_H_
#define _CAMERACALIBRATE_H_

#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <iostream>

using namespace cv;
using namespace std;
class CameraCalibrate
{
private:
    vector<vector<Point3f>> objectPoints;    // 角点的世界坐标系坐标
    vector<vector<Point2f>> imagePoints;     // 角点的像素坐标系坐标
    Mat cameraMatrix;                        // 内参矩阵
    Mat distCoeffs;                          // 畸变矩阵
    vector<Mat> rvecs, tvecs;                // 旋转矩阵队列和平移矩阵队列，每一幅标定图像都有对应的一个旋转矩阵和平移矩阵
    vector<double> calibrateErrs;            // 保存矫正偏差

    int flag;
    cv::Mat map1, map2;
    bool mustInitUndistort;

public:
    CameraCalibrate() :flag(0), mustInitUndistort(true) {};
    int addChessboardPoints(const vector<string>& filelist, Size& boardSize);
    void addPoints(const std::vector<cv::Point2f>& imageCorners, const std::vector<cv::Point3f>& objectCorners);
    double CalibCamera(Size imageSize);
    Mat remap(Mat & image);
    void setCalibrationFlag(bool radial8CoeffEnabled = false, bool tangentialParamEnabled = false);
    void computeCalibrateError();
    Mat getCameraMatrix() { return cameraMatrix; }
    Mat getDistCoeffs() { return distCoeffs; }
    vector<double>getCalibrateErrs() { return calibrateErrs; }
    ~CameraCalibrate() {};
};

#endif

#include "CameraCalibrate.h"
int CameraCalibrate::addChessboardPoints(const vector<string>& filelist, Size& boardSize)
{
    vector<Point2f> imageCorners;
    vector<Point3f> objectCorners;

    for (int i = 0; i < boardSize.height; i++)
    {
        for (int j = 0; j < boardSize.width; j++)
        {
            objectCorners.push_back(Point3f(i, j, 0.0f));// 保存世界坐标系坐标
        }
    }

    Mat image, grayImage;
    int success = 0;
    for (int i = 0; i < filelist.size(); i++)
    {
        image = imread(filelist[i]);
        cvtColor(image, grayImage, COLOR_BGR2GRAY);
        bool found = findChessboardCorners(grayImage, boardSize, imageCorners);
        cornerSubPix(grayImage, imageCorners, Size(5, 5), Size(-1, -1), TermCriteria(TermCriteria::MAX_ITER + TermCriteria::EPS, 30, 0.1));// 计算亚像素级别角点信息
        //如果角点数目满足要求，那么将它加入数据
        if (imageCorners.size() == boardSize.area())
        {
            addPoints(imageCorners, objectCorners);// 保存像素坐标系坐标
            success++;
        }

        drawChessboardCorners(image, boardSize, imageCorners, found);
        imshow("Corners on Chessboard", image);// 显示角点信息
        cv::waitKey(100);
    }    
    return success;
}

void CameraCalibrate::addPoints(const vector<Point2f>& imageCorners, const vector<Point3f>& objectCorners)
{ 
    imagePoints.push_back(imageCorners);    
    objectPoints.push_back(objectCorners);
}

double CameraCalibrate::CalibCamera(Size imageSize)
{
    mustInitUndistort = true;
    double ret = calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs, flag);
    return ret;
}

Mat CameraCalibrate::remap(Mat & image)
{
    Mat undistorted;
    if (mustInitUndistort)//每次标定只需要初始化一次
    {
        //计算无畸变和修正转换映射。 
        initUndistortRectifyMap(cameraMatrix,distCoeffs, cv::Mat(), cv::Mat(), image.size(), CV_32FC1, map1, map2);// 首先计算得到用于矫正计算的两个矩阵map1和map2
        mustInitUndistort = false;
    }
    cv::remap(image, undistorted, map1, map2, cv::INTER_LINEAR);// 矫正运算
    return undistorted;
}

void CameraCalibrate::setCalibrationFlag(bool radial8CoeffEnabled , bool tangentialParamEnabled )
{
    flag = 0;
    if (!tangentialParamEnabled)
        flag += CALIB_ZERO_TANGENT_DIST;
    if (radial8CoeffEnabled)
        flag += CALIB_RATIONAL_MODEL;
}

void CameraCalibrate::computeCalibrateError()
{
    int image_count = rvecs.size();    
    for (int i = 0; i < image_count; i++) 
    {
        std::vector<cv::Point2f> result_image_points;// 保存反向映射后的像素坐标系坐标        
        cv::projectPoints(objectPoints[i], rvecs[i], tvecs[i],cameraMatrix, distCoeffs, result_image_points);// 将世界坐标系坐标反向投影到像素坐标系中        
        cv::Mat original_image_points_Mat = cv::Mat(1, objectPoints[i].size(), CV_32FC2);        
        cv::Mat result_image_points_Mat = cv::Mat(1, result_image_points.size(), CV_32FC2);         /* 使用二阶笵数计算矩阵间的差异作为矫正偏差 */        
        for(int j = 0; j < objectPoints[i].size(); j++)        
        {            
            original_image_points_Mat.at<cv::Vec2f>(0,j) = cv::Vec2f(imagePoints[i][j].x, imagePoints[i][j].y);            
            result_image_points_Mat.at<cv::Vec2f>(0,j) = cv::Vec2f(result_image_points[j].x, result_image_points[j].y);     
        }        
        double err = cv::norm(original_image_points_Mat, result_image_points_Mat, cv::NORM_L2);        
        err /= result_image_points.size();        
        calibrateErrs.push_back(err);    
    }
}

#include <iostream>
#include "CameraCalibrate.h"
#include <fstream>  
#include <io.h>     
#include <string>
#include <direct.h> 
#include <vector> 
void getFiles(std::string path, std::vector<std::string>& files)
{

    intptr_t hFile = 0;//win10使用
    struct _finddata_t fileinfo;
    string p;
    if ((hFile = _findfirst(p.assign(path).append("\*").c_str(), &fileinfo)) != -1)
    {
        do 
        { 
            if ((fileinfo.attrib & _A_SUBDIR)) 
            { 
                if (strcmp(fileinfo.name, ".") != 0 && strcmp(fileinfo.name, "..") != 0)        
                    getFiles(p.assign(path).append("\").append(fileinfo.name), files); 
            }
            else 
            { 
                files.push_back(p.assign(path).append("\").append(fileinfo.name));
            }
        } while (_findnext(hFile, &fileinfo) == 0);
        _findclose(hFile);
    }
}

std::string path = "E:\欣奕华\项目\视觉\标定\chess";
void main()
{
    vector<string>files;
    getFiles(path, files);

    Mat image;
    CameraCalibrate cameraCali;
    Size boardSize(6, 4);// 标定图像中每行、列中内角点的数量
    image = imread(files[7], 0);

    cameraCali.addChessboardPoints(files, boardSize);
    cameraCali.CalibCamera(image.size());
    Mat uImage = cameraCali.remap(image);// 对图像进行矫正操作
    imshow("Original Image", image);
    imshow("Undistorted Image", uImage);

    cout << "相机内参矩阵：" << endl;
    Mat cameraMatrix = cameraCali.getCameraMatrix();
    for (int i = 0; i < cameraMatrix.rows; i++)
    {
        for (int j = 0; j < cameraMatrix.cols; j++)
        {
            cout << cameraMatrix.at<double>(i, j) << "   ";
        }
        cout << endl;
    }
    cout << "相机畸变矩阵：" << endl;
    Mat distCoeffs = cameraCali.getDistCoeffs();
    for (int i = 0; i < distCoeffs.rows; i++)
    {
        for (int j = 0; j < distCoeffs.cols; j++)
        {
            cout << distCoeffs.at<double>(i, j) << "   ";
        }
        cout << endl;
    }

    cout << "标定结果：" << endl;
    cameraCali.computeCalibrateError();
    vector<double> errs = cameraCali.getCalibrateErrs();
    for (int i = 0; i < errs.size(); i++)
    {
        std::cout << "第" << i + 1 << "幅图像的平均矫正偏差： " << errs[i] << "像素大小" << std::endl;
    }
    waitKey(0);

相机标定程序解读：

1. 读取文件图片；

2.初始化图片角点在世界坐标系的位置：

for (int i = 0; i < boardSize.height; i++)
    {
        for (int j = 0; j < boardSize.width; j++)
        {
            objectCorners.push_back(Point3f(i, j, 0.0f));// 保存世界坐标系坐标
        }
    }

3.遍历文件夹中所有图片，并将其转化为灰度图：

cvtColor(image, grayImage, COLOR_BGR2GRAY);

4. 寻找棋盘图的内角点位置

 bool found = findChessboardCorners(grayImage, boardSize, imageCorners);

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<Point2f> image_points_buf;
第四个参数flage：用于定义棋盘图上内角点查找的不同处理方式，有默认值。

返回值：如果找到角点返回1，没有找到返回：0；

解释：a regular chessboard has 8 x 8 squares and 7 x 7 internal corners, that is, points where the black squares touch each other. The detected coordinates are approximate, and to determine their positions more accurately, the function calls cornerSubPix. You also may use the function cornerSubPix with different parameters if returned coordinates are not accurate enough.

5. 精确确定棋盘格内角点位置

cornerSubPix(grayImage, imageCorners, Size(5, 5), Size(-1, -1), TermCriteria(TermCriteria::MAX_ITER + TermCriteria::EPS, 30, 0.1));// 计算亚像素级别角点信息

cornerSubPix(InputArray image, InputOutputArray corners, Size winSize, Size zeroZone, TermCriteria criteria);

image：输入图像
corners：输入角点的初始坐标以及精准化后的坐标用于输出
winSize：搜索窗口边长的一半，例如如果winSize=Size(5,5)，则一个大小为：的搜索窗口将被使用。
zeroZone：搜索区域中间的dead region边长的一半，有时用于避免自相关矩阵的奇异性。如果值设为(-1,-1)则表示没有这个区域。
criteria：角点精准化迭代过程的终止条件。也就是当迭代次数超过criteria.maxCount，或者角点位置变化小于criteria.epsilon时，停止迭代过程。
CvTermCriteria 类：迭代算法的终止准则

原型：
`typedef struct CvTermCriteria
{
int    type;  /* CV_TERMCRIT_ITER 和CV_TERMCRIT_EPS二值之一，或者二者的组合 */
int    max_iter; /* 最大迭代次数 */
double epsilon; /* 结果的精确性 */
宏定义：
CV_TERMCRIT_ITER：代终止条件为达到最大迭代次数终止
CV_TERMCRIT_EPS：迭代到阈值终止

6.保存角点在图像坐标系中坐标以及在世界坐标系的坐标

 角点的图像坐标系是二维空间，定义vector<Point2f>imageCorner

 角点的世界坐标系是三维空间，定义vector<Point3f>projectCorner

 判断条件判： 如果角点数目满足要求，那么将它加入数据

 最后调用 addPoints(const vector<Point2f>& imageCorners, const vector<Point3f>& objectCorners)；

7. 求出角点在两坐标系中坐标后，调用标定函数

calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs, flag);

计算相机内参和外参系数

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<Point3f>> object_points。需要依据棋盘上单个黑白矩阵的大小，计算出（初始化）每一个内角点的世界坐标。
第二个参数imagePoints，为每一个内角点对应的图像坐标点。和objectPoints一样，应该输入vector<vector<Point2f>>image_points_seq形式的变量；
第三个参数imageSize，   为图像的像素尺寸大小，在计算相机的内参和畸变矩阵时需要使用到该参数；
第四个参数cameraMatrix为相机的内参矩阵。输入一个Mat cameraMatrix即可，如Mat cameraMatrix=Mat(3,3,CV_32FC1,Scalar::all(0));
第五个参数distCoeffs为畸变矩阵。输入一个Mat distCoeffs=Mat(1,5,CV_32FC1,Scalar::all(0))即可；
第六个参数rvecs为旋转向量；应该输入一个Mat类型的vector，即vector<Mat>rvecs;
第七个参数tvecs为位移向量，和rvecs一样，应该为vector<Mat> tvecs；
第八个参数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是最优迭代终止条件设定。
在使用该函数进行标定运算之前，需要对棋盘上每一个内角点的空间坐标系的位置坐标进行初始化，标定的结果是生成相机的内参矩阵cameraMatrix、相机的5个畸变系数distCoeffs，另外每张图像都会生成属于自己的平移向量和旋转向量。

8. 查看标定效果——利用标定结果对棋盘图进行矫正

initUndistortRectifyMap(cameraMatrix,distCoeffs, cv::Mat(), cv::Mat(), image.size(), CV_32FC1, map1, map2);// 首先计算得到用于矫正计算的两个矩阵map1和ma

利用求得的相机的内参和外参数据，可以对图像进行畸变的矫正，这里有两种方法可以达到矫正的目的，分别说明一下。

方法一：使用initUndistortRectifyMap和remap两个函数配合实现。
initUndistortRectifyMap用来计算畸变映射，remap把求得的映射应用到图像上。
方法二：使用undistort函数实现
正向矫正的流程为：畸变像素坐标→畸变物理坐标→标准物理坐标→标准像素坐标
逆向矫正的流程为：标准像素坐标→标准物理坐标→畸变物理坐标→畸变像素坐标
UndistortPoints就是执行的正向矫正过程，而initUndistortRectifyMap执行的是逆向矫正过程。
initUndistortRectifyMap
cv::initUndistortRectifyMap( InputArray cameraMatrix, InputArray distCoeffs,
                              InputArray matR, InputArray newCameraMatrix,
                              Size size, int m1type, OutputArray map1, OutputArray map2 )

1.cameraMatrix：输入相机矩阵
2.distCoeffs：输入参数，相机的畸变系数：，有4，5,8,12或14个元素。如果这个向量是空的，就认为是零畸变系数。
3.matR：可选的修正变换矩阵，是个3*3的矩阵。通过stereoRectify计算得来的R1或R2可以放在这里。如果这个矩阵是空的，就假设为单位矩阵。在cvInitUndistortMap中，R被认为是单位矩阵。
4.newCameraMatrix：新的相机矩阵
5.size：未畸变的图像尺寸。
6.m1type：第一个输出的映射的类型，可以为 CV_32FC1, CV_32FC2或CV_16SC2，参见cv::convertMaps。
7.map1：第一个输出映射。
8.map2：第二个输出映射。

函数说明：

这个函数用于计算无畸变和修正转换关系，为了重映射，将结果以映射的形式表达。无畸变的图像看起来就想原始的图像，就像这个图像是用内参为newCameraMatrix的且无畸变的相机采集得到的。
在单目相机例子中，newCameraMatrix一般和cameraMatrix相等，或者可以用cv::getOptimalNewCameraMatrix来计算，获得一个更好的有尺度的控制结果。
在双目相机例子中，newCameraMatrix一般是用cv::stereoRectify计算而来的，设置为P1或P2。
此外，根据R，新的相机在坐标空间中的取向是不同的。例如，它帮助配准双目相机的两个相机方向，从而使得两个图像的极线是水平的，且y坐标相同（在双目相机的两个相机谁水平放置的情况下）。
该函数实际上为反向映射算法构建映射，供反向映射使用。也就是，对于在已经修正畸变的图像中的每个像素，该函数计算原来图像（从相机中获得的原始图像）中对应的坐标系。这个过程是这样的： 

其中，是畸变系数。
在双目相机的例子中，这个函数调用两次：一次是为了每个相机的朝向，经过stereoRectify之后，依次调用cv::stereoCalibrate。但是如果这个双目相机没有标定，依然可以使用cv::stereoRectifyUncalibrated

直接从单应性矩阵H中计算修正变换。对每个相机，函数计算像素域中的单应性矩阵H作为修正变换，而不是3D空间中的旋转矩阵R。R可以通过H矩阵计算得来：

 9.重映射

cv::remap(image, undistorted, map1, map2, INTER_LINEAR)

第一个参数，InputArray类型的src，输入图像，即源图像，填Mat类的对象即可，且需为单通道8位或者浮点型图像。

第二个参数，OutputArray类型的dst，函数调用后的运算结果存在这里，即这个参数用于存放函数调用后的输出结果，需和源图片有一样的尺寸和类型。

第三个参数，InputArray类型的map1，它有两种可能的表示对象。表示点（x，y）的第一个映射。表示CV_16SC2 , CV_32FC1 或CV_32FC2类型的X值。

第四个参数，InputArray类型的map2，同样，它也有两种可能的表示对象，而且他是根据map1来确定表示那种对象。若map1表示点（x，y）时。这个参数不代表任何值。表示CV_16UC1 , CV_32FC1类型的Y值（第二个值）。

第五个参数，int类型的interpolation,插值方式，之前的resize( )函数中有讲到，需要注意，resize( )函数中提到的INTER_AREA插值方式在这里是不支持的，所以可选的插值方式如下：INTER_NEAREST - 最近邻插值INTER_LINEAR – 双线性插                      值（默认值）INTER_CUBIC – 双三次样条插值（逾4×4像素邻域内的双三次插值）INTER_LANCZOS4 -Lanczos插值（逾8×8像素邻域的Lanczos插值）

第六个参数，int类型的borderMode，边界模式，有默认值BORDER_CONSTANT，表示目标图像中“离群点（outliers）”的像素值不会被此函数修改.

第七个参数，const Scalar&类型的borderValue，当有常数边界时使用的值，其有默认值Scalar( )，即默认值为0。

这个函数可参考：https://blog.csdn.net/qq_20823641/article/details/52232046，讲述映射以及原理。

10.对标定结果进行评价。

对标定结果进行评价的方法是通过得到的摄像机内外参数，对空间的三维点进行重新投影计算，得到空间三维点在图像上新的投影点的坐标，计算投影坐标和亚像素角点坐标之间的偏差，偏差越小，标定结果越好。

对空间三维坐标点进行反向投影的函数是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为相机的畸变矩阵；

第六个参数iamgePoints为每一个内角点对应的图像上的坐标点；

第七个参数jacobian是雅可比行列式；

第八个参数aspectRatio是跟相机传感器的感光单元有关的可选参数，如果设置为非0，则函数默认感光单元的dx/dy是固定的，会依此对雅可比矩阵进行调整；

 

备注：关于畸变矫正仍需要看看书中详细介绍，还有网址讲述了矫正函数的重定义。