相机模型与相机标定

针孔相机模型

主要参考：OpenCV: Camera Calibration and 3D Reconstruction

四个坐标系

世界坐标系$P_w(m):(x_w,y_w,z_w)$

世界坐标系可任意选择，为假想坐标系，在被指定后不变且唯一。

相机坐标系$P_c(m):(x_c,y_c,z_c)$

相机坐标系原点在光心，x轴为相机平面的水平方向，y轴为相机平面的竖直方向，z轴指向相机观察方向，其随相机移动而变化，即为相对坐标系。

图像坐标系$P(mm):(x,y)$

图像坐标系原点位于透镜光轴与成像平面的交点，x轴与y轴分别平行于相机坐标系的x轴与y轴，是二维平面直角坐标系，单位为毫米，依托相机坐标系，也为相对坐标系。

像素坐标系$p(pixel):(u,v)$

像素坐标系为固定在图像上的以像素为单位的平面直角坐标系，原点位于图像左上角，x轴与y轴分别平行于图像坐标系的x轴与y轴，依托图像坐标系，也为相对坐标系。

坐标系变换

从世界坐标系到相机坐标系，为刚体变换（旋转+平移）
$$P_c=T_{cw}{P}_w$$

即
$$\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right]=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]$$

从相机坐标系到图像坐标系，会丢失一个自由度$z$，$f$为焦距。
$$\frac{z_c}{f}=\frac{x_c}{x}=\frac{y_c}{y}$$

即
$$\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=\frac{1}{z_c}\left[\begin{array}{ccc}f& 0& 0\\ 0& f& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]=\frac{1}{z_c}\left[\begin{array}{cccc}f& 0& 0& 0\\ 0& f& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right]$$

从图像坐标系到像素坐标系，其中像素坐标系原点位于左上角，图像坐标系原点不一定位于中心

$\alpha =\frac{1}{dx}$，$\beta =\frac{1}{dy}$，$dx$和$dy$分别表示sensor上每个点在图像坐标系横纵轴的物理尺寸
$$\begin{gathered} u=\alpha x+c_x \\ v=\beta y+c_y \end{gathered}$$

即
$$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}\alpha & 0& c_x\\ 0& \beta & c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]$$

因此，世界坐标系到像素坐标系
$$p=\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\frac{1}{z_c}\left[\begin{array}{ccc}\alpha & 0& c_x\\ 0& \beta & c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}f& 0& 0& 0\\ 0& f& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]=\frac{1}{z_c}KT{P}_w$$

或者写成
$$p=\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\frac{1}{z_c}\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& t_1\\ r_{21}& r_{22}& r_{23}& t_2\\ r_{31}& r_{32}& r_{33}& t_3\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]=\frac{1}{z_c}KT{P}_w$$

真正的镜头通常会有一些畸变，主要是径向畸变，还有轻微的切向畸变
$$\left[\begin{array}{c}u\\ v\end{array}\right]=\left[\begin{array}{c}{f}_x{x}^{\prime }+c_x\\ f_y{y}^{\prime }+c_y\end{array}\right]$$

而
$$\{\begin{array}{l}\begin{array}{rl}{x}^{\prime }& =x(1+k_1{r}^2+k_2{r}^4+k_3{r}^6)+2p_{1}xy+p_2(r^2+2x^2)\\ y^{\prime }& =y(1+k_1{r}^2+k_2{r}^4+k_3{r}^6)+2p_{2}xy+p_1(r^2+2y^2)\end{array}\end{array}$$

其中，$r^2=x^2+y^2$

这里的$(x,y)$不同于上面的，而是 $x=X_c/Z_c$，$y=Y_c/Z_c$

相机标定示例程序

#include "opencv.hpp"
#include <iostream>
#include <fstream> 
#include <io.h>  
#include <string>  
#include <vector>

using namespace cv;
using namespace std;

void getFilesName(string& fileDirectory, string& fileType, vector<string>& filesPath);
void m_calibration(vector<string>& FilesName, Size board_size, Size square_size, Mat& cameraMatrix, Mat& distCoeffs, vector<Mat>& rvecsMat, vector<Mat>& tvecsMat);

int main()
{
    string fileDirectory = "D:/VS2019Projects/cameraCalibration/cameraCalibration/chessImage";
    string fileType = ".jpg";    //查找指定的文件类型
    vector<string> filesPath;    //存放文件路径名的容器

    getFilesName(fileDirectory, fileType, filesPath);              //寻找文件的路径

    Size boardSize = Size(9, 6);                                   // 标定板上每行、列的角点数 
    Size squareSize = Size(30, 30);                                // 实际测量得到的标定板上每个棋盘格的物理尺寸，单位mm

    Mat cameraMatrix = 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> rVecsMat;                                          // 存放所有图像的旋转向量，每一副图像的旋转向量为一个mat
    vector<Mat> tVecsMat;                                          // 存放所有图像的平移向量，每一副图像的平移向量为一个mat

    m_calibration(filesPath, boardSize, squareSize, cameraMatrix, distCoeffs, rVecsMat, tVecsMat);

    return 0;
}

/*
@param fileDirectory 为文件夹目录
@param fileType 为需要查找的文件类型
@param filesPath 为存放文件名的容器
*/
void getFilesName(string& fileDirectory, string& fileType, vector<string>& filesPath)
{
    string buffer = fileDirectory + "\\*" + fileType;
    _finddata_t fileInfo;   //存放文件信息的结构体
    intptr_t hFile;
    hFile = _findfirst(buffer.c_str(), &fileInfo); //找第一个文件

    if (hFile == -1L) {
        //没找到指定类型的文件
        cout << "No " << fileType << " files in current directory!" << endl;
    }
    else {
        string fullFilePath;
        do {
            fullFilePath.clear();
            fullFilePath = fileDirectory + "\\" + fileInfo.name;
            filesPath.push_back(fullFilePath);

        } while (_findnext(hFile, &fileInfo) == 0);  //如果找到下个文件的名字成功的话就返回0,否则返回-1  
        _findclose(hFile);
    }
}

void m_calibration(vector<string>& filesPath, Size boardSize, Size squareSize, Mat& cameraMatrix, Mat& distCoeffs, vector<Mat>& rVecsMat, vector<Mat>& tVecsMat)
{
    ofstream fout("caliberation_left.txt");                       // 保存标定结果的文件 

    cout << "开始提取角点………………" << endl;
    int imageCount = 0;                                             // 图像数量 
    Size imageSize;                                                 // 图像的尺寸 

    vector<Point2f> imagePoints;                                    // 缓存每幅图像上检测到的角点
    vector<vector<Point2f>> imagePointsSeq;                         // 保存检测到的所有角点

    for (int i = 0; i < filesPath.size(); i++)
    {
        imageCount++;

        // 用于观察检验输出
        cout << "image_count = " << imageCount << endl;
        Mat imageInput = imread(filesPath[i]);
        if (imageCount == 1)  //读入第一张图片时获取图像宽高信息
        {
            imageSize.width = imageInput.cols;
            imageSize.height = imageInput.rows;
            cout << "image_size.width = " << imageSize.width << endl;
            cout << "image_size.height = " << imageSize.height << endl;
        }

        // 提取角点
        bool ok = findChessboardCorners(imageInput, boardSize, imagePoints, 
            CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE);
        //cout << "imagePoints.size = " << imagePoints.size() << endl;
        //for (int j = 0; j < imagePoints.size(); j++) {

        //    cout << "imagePoints: " << imagePoints[j] << endl;
        //}
        
        if (0 == ok)
        {
            cout << "第" << imageCount << "张照片提取角点失败，请删除后，重新标定！" << endl;
            imshow("失败照片", imageInput);
            waitKey(0);
        }
        else
        {
            Mat view_gray;
            cout << "imageInput.channels()=" << imageInput.channels() << endl;
            cvtColor(imageInput, view_gray, CV_RGB2GRAY);

            // 亚像素精确化  cv::Size(11, 11)不懂为什么取11*11
            cv::cornerSubPix(view_gray, imagePoints, cv::Size(11, 11), cv::Size(-1, -1), 
                cv::TermCriteria(CV_TERMCRIT_ITER + CV_TERMCRIT_EPS, 20, 0.01));

            imagePointsSeq.push_back(imagePoints);  //保存亚像素角点

            drawChessboardCorners(view_gray, boardSize, imagePoints, true);  //在图像上显示角点位置

            imshow("camera calibration", view_gray);
            waitKey(500);
        }
    }
    cout << "角点提取完成！！！" << endl;


    // 获取棋盘中角点的三维坐标信息
    Point3f realPoint;
    vector<Point3f> objectPoints;
    vector<vector<Point3f>> objectPointsSeq;
    for (int t = 0; t < imageCount; t++)
    {
        objectPoints.clear();
        for (int i = 0; i < boardSize.height; i++)
        {
            for (int j = 0; j < boardSize.width; j++)
            {              
                /* 假设标定板放在世界坐标系中z=0的平面上 */
                realPoint.x = i * squareSize.width;
                realPoint.y = j * squareSize.height;
                realPoint.z = 0;
                objectPoints.push_back(realPoint);
            }
        }
        objectPointsSeq.push_back(objectPoints);
    }


    // 运行标定函数
    double err_first = calibrateCamera(objectPointsSeq, imagePointsSeq, imageSize, 
        cameraMatrix, distCoeffs, rVecsMat, tVecsMat, CV_CALIB_FIX_K3);
    fout << "重投影误差1：" << err_first << "像素" << endl << endl;
    cout << "标定完成！！！" << endl;


    cout << "开始评价标定结果………………" << endl;
    double total_err = 0.0;            // 所有图像的平均误差的总和 
    double err = 0.0;                  // 每幅图像的平均误差
    double totalErr = 0.0;
    double totalPoints = 0.0;
    vector<Point2f> imagePointsPro;     // 保存重新计算得到的投影点

    for (int i = 0; i < imageCount; i++)
    {
        //通过得到的摄像机内外参数，对角点的空间三维坐标进行重新投影计算
        projectPoints(objectPointsSeq[i], rVecsMat[i], tVecsMat[i], cameraMatrix, distCoeffs, imagePointsPro);

        err = norm(Mat(imagePointsSeq[i]), Mat(imagePointsPro), NORM_L2);

        totalErr += err * err;
        totalPoints += objectPointsSeq[i].size();

        err /= objectPointsSeq[i].size();
        //fout << "第" << i + 1 << "幅图像的平均误差：" << err << "像素" << endl;
        total_err += err;
    }
    fout << "重投影误差2：" << sqrt(totalErr / totalPoints) << "像素" << endl << endl;
    fout << "重投影误差3：" << total_err / imageCount << "像素" << endl << endl;


    //保存定标结果    
    cout << "开始保存标定结果………………" << endl;
    Mat rotation_matrix = Mat(3, 3, CV_32FC1, Scalar::all(0)); /* 保存每幅图像的旋转矩阵 */
    fout << "相机内参数矩阵：" << endl;
    fout << cameraMatrix << endl << endl;
    fout << "畸变系数：" << endl;
    fout << distCoeffs << endl << endl << endl;
    for (int i = 0; i < imageCount; i++)
    {
        fout << "第" << i + 1 << "幅图像的旋转向量：" << endl;
        fout << rVecsMat[i] << endl;

        /* 将旋转向量转换为相对应的旋转矩阵 */
        Rodrigues(rVecsMat[i], rotation_matrix);
        fout << "第" << i + 1 << "幅图像的旋转矩阵：" << endl;
        fout << rotation_matrix << endl;
        fout << "第" << i + 1 << "幅图像的平移向量：" << endl;
        fout << tVecsMat[i] << endl << endl;
    }
    cout << "标定结果完成保存！！！" << endl;
    fout << endl;
}

棋盘格标定的相关函数

findChessboardCorners

findChessboardCorners函数原型

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

参数	含义
image	源棋盘视图。 它必须是8位灰度或彩色图像。
patternSize	每个棋盘行和列的内部角点数（patternsize = cvSize（points_per_row，points_per_colum）= cvSize（列，行））
corners	被检测到的角点的输出矩阵
flags	各种操作标志

该函数试图确定输入图像是否是棋盘图案的视图，并找到内部棋盘角。 如果找到所有角点，则该函数将返回非零值，并且将其放置在一定顺序（行逐行，每行从左到右）。 否则，如果该功能未能找到所有的角或重新排序，则它将返回0。例如，常规棋盘具有8 x 8正方形和7 x 7的内部拐角，即黑色正方形相互接触的点。 检测到的坐标是近似的，于是为了更准确地确定其位置，可使用函数CornerSubpix。

cornerSubPix

cornerSubPix函数原型

void cv::cornerSubPix(InputArray 	image,
    InputOutputArray 	corners,
    Size 	winSize,
    Size 	zeroZone,
    TermCriteria 	criteria
)	

参数	含义
image	输入单通道，8位或浮点图像。
corners	输入角的初始坐标和用于输出的修正坐标
winSize	搜索窗口的一半长度的一半。 例如，如果winsize = size（5,5），则使用a（5 ∗ 2+1）×（5 ∗ 2+1）= 11×11搜索窗口。
zeroZone	搜索区中间的死区域大小的一半， 它有时用于避免自相关矩阵的可能奇异性。 （-1，-1）的值表示没有这样的大小。
criteria	终止角点迭代过程的准则

该函数迭代以找到角点的亚像素精确位置

cv::TermCriteria类介绍

定义迭代算法终止标准的类。您可以默认情况下对其进行初始化，然后覆盖任何参数，或者使用构造函数的高级变体可以完全初始初始化结构。

构造函数

cv::TermCriteria::TermCriteria(int 	type,
    int 	maxCount,
    double 	epsilon
)	

参数	含义
type	终止标准的类型
maxCount	要计算的最大迭代或元素数量
epsilon	迭代算法停止的所需准确性或参数的变化

drawChessboardCorners

drawChessboardCorners函数原型

void cv::drawChessboardCorners(InputOutputArray 	image,
    Size 	patternSize,
    InputArray 	corners,
    bool 	patternWasFound
)

参数	含义
image	目标图像。 它必须是8位颜色图像。
patternSize	每个棋盘行和列的内部角点数（patternsize = cv :: size（points_per_row，points_per_column））
corners	检测到的角点的数组，即FindchessboardCorners的输出。
patternWasFound	参数指示是否找到完整的板

该函数在图像上渲染检测到的棋盘角点

calibrateCamera

calibrateCamera函数原型一

double cv::calibrateCamera(InputArrayOfArrays 	objectPoints,
    InputArrayOfArrays 	imagePoints,
    Size 	imageSize,
    InputOutputArray 	cameraMatrix,
    InputOutputArray 	distCoeffs,
    OutputArrayOfArrays 	rvecs,
    OutputArrayOfArrays 	tvecs,
    OutputArray 	stdDeviationsIntrinsics,
    OutputArray 	stdDeviationsExtrinsics,
    OutputArray 	perViewErrors,
    int 	flags = 0,
    TermCriteria 	criteria = TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON)
)	

calibrateCamera函数原型二

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

参数	含义
objectPoints	世界坐标系中的点，在新界面中，它是校准模式坐标空间中校准模式点向量的向量（例如std::vector<std::vector<cv::vec3f>>）。尽管这些点为3D，但它们都位于校准模式的XY坐标平面（因此在Z坐标中为0）
imagePoints	图像像素坐标点，在新界面中，它是校准模式点投影的向量的向量（例如，std::vector<std:vector<cv::vec2f>>）。ImagePoints.Size()和ObjectPoints.Size()必须相等，ImagePoints[i].size()和ObjectPoints[i].size()，对于每个i，必须分别相等
imageSize	图像的大小仅用于初始化固有的摄像机矩阵
cameraMatrix	输入/输出3x3相机内参数矩阵
distCoeffs	畸变系数的输入/输出向量（4, 5, 8, 12 or 14 个元素）
rvecs	估计每个模式视图的旋转矢量的输出向量（例如std::vector<cv:mat>>）
tvecs	对每个模式视图估计的平移向量的输出向量
stdDeviationsIntrinsics	估计内在参数的标准偏差的输出向量
stdDeviationsEntrinsics	估计外在参数的标准偏差的输出向量
perViewErrors	每个标定图片的重投影均方根误差的输出向量
flags	标定函数所采用的模型
criteria	迭代优化算法的终止标准

标定模型

flags	含义
CALIB_USE_INTRINSIC_GUESS	cameraMatrix 包含进一步优化的 fx、fy、cx、cy 的有效初始值。 否则，最初将 (cx, cy) 设置为图像中心（使用 imageSize），并以最小二乘方式计算焦距。 请注意，如果内部参数已知，则无需仅使用此函数来估计外部参数。 请改用solvePnP。
CALIB_FIX_PRINCIPAL_POINT	全局优化期间主点不变。 当 CALIB_USE_INTRINSIC_GUESS 也被设置时，它会停留在中心或指定的不同位置。
CALIB_FIX_ASPECT_RATIO	这些函数仅将 fy 视为自由参数。 fx/fy 的比率与输入 cameraMatrix 中的比率相同。 当 CALIB_USE_INTRINSIC_GUESS 未设置时，将忽略 fx 和 fy 的实际输入值，仅计算并进一步使用它们的比率。
CALIB_ZERO_TANGENT_DIST	切向畸变系数 (p1,p2) 设置为零并保持为零
CALIB_FIX_K1,…,CALIB_FIX_K6	优化过程中相应的径向畸变系数不变。 如果设置了 CALIB_USE_INTRINSIC_GUESS，则使用提供的 distCoeffs 矩阵中的系数。 否则，将其设置为 0。
CALIB_RATIONAL_MODEL	启用系数 k4、k5 和 k6。 为了提供向后兼容性，应明确指定此额外标志以使校准函数使用有理模型并返回 8 个系数。 如果未设置该标志，则该函数仅计算并返回 5 个失真系数。
CALIB_THIN_PRISM_MODEL	启用系数 s1、s2、s3 和 s4。 为了提供向后兼容性，应明确指定此额外标志以使校准函数使用薄棱镜模型并返回 12 个系数。 如果未设置该标志，则该函数仅计算并返回 5 个失真系数。
CALIB_FIX_S1_S2_S3_S4	薄棱镜畸变系数在优化过程中没有改变。 如果设置了 CALIB_USE_INTRINSIC_GUESS，则使用提供的 distCoeffs 矩阵中的系数。 否则，将其设置为0。
CALIB_TILTED_MODEL	启用系数 tauX 和 tauY。 为了提供向后兼容性，应明确指定此额外标志以使校准函数使用倾斜传感器模型并返回 14 个系数。 如果未设置该标志，则该函数仅计算并返回 5 个失真系数。
CALIB_FIX_TAUX_TAUY	倾斜传感器模型的系数在优化过程中没有改变。 如果设置了 CALIB_USE_INTRINSIC_GUESS，则使用提供的 distCoeffs 矩阵中的系数。 否则，将其设置为 0。

projectPoints

projectPoints函数原型

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

参数	含义
objectPoints	用 wrt 表示的对象点数组。 世界坐标系。 一个 3xN/Nx3 1 通道或 1xN/Nx1 3 通道（或 vector< Point3f >），其中 N 是视图中的点数。
rvec	旋转矢量
tvec	平移向量
cameraMatrix	3x3相机内参数矩阵
distCoeffs	畸变系数向量（4, 5, 8, 12 or 14 个元素）
imagePoints	图像点的输出数组，1xN/Nx1 2 通道，或 vector< Point2f >
jacobian	雅可比矩阵
aspectRatio	可选的“固定纵横比”参数。 如果参数不为0，则函数假定纵横比（fx/fy）是固定的，并相应地调整雅可比矩阵。

该函数在给定内部和外部相机参数的情况下计算 3D 点到图像平面的 2D 投影。 可选地，该函数计算图像点坐标的偏导数（作为所有输入参数的函数）关于特定参数、内在和/或外在的雅可比矩阵。 Jacobians 用于 calibrateCamera、solvePnP 和 stereoCalibrate 的全局优化。 给定当前的内在和外在参数，该函数本身也可用于计算重投影误差。

norm

norm函数原型

double cv::norm(InputArray 	src1,
    InputArray 	src2,
    int 	normType = NORM_L2,
    InputArray 	mask = noArray()
)

参数	含义
src1	第一个输入数组
src2	与 src1 大小和类型相同的第二个输入数组
normType	norm类型
mask	可选操作掩码； 它必须具有与 src1 和 CV_8UC1 类型相同的大小。

enum cv::NormTypes常用枚举值

• NORM_INF $norm=\Vert src1-src2{\Vert }_{L_{\infty }}=ma{x}_I|src1(I)-src2(I)|$
• NORM_L1 $norm=\Vert src1-src2{\Vert }_{L_1}={\Sigma }_I|src1(I)-src2(I)|$
• NORM_L2 $norm=\Vert src1-src2{\Vert }_{L_2}=\sqrt{{\Sigma }_I(src1(I)-src2(I){)}^2}$

此版本的 cv::norm 计算数组 src1 和 src2 的绝对差范数或相对差范数。 使用 NormTypes 指定要计算的范数类型。

Rodrigues

Rodrigues函数原型

void cv::Rodrigues(InputArray 	src,
    OutputArray 	dst,
    OutputArray 	jacobian = noArray()
)

参数	含义
src	输入旋转向量（3x1 和 1x3）或旋转矩阵（3x3）
dst	输出旋转矩阵 (3x3) 或旋转向量 (3x1 或 1x3)
jacobian	可选输出雅可比矩阵，3x9 或 9x3，它是输出数组分量相对于输入数组分量的偏导数矩阵

将旋转矩阵转换为旋转向量，反之亦然。

待标定图片百度链接：

链接：https://pan.baidu.com/s/1kdwKtE8CaHHLr4RtRKn9lA 
提取码：ms39