（一）相机标定——四大坐标系的介绍、对应转换、畸变原理以及OpenCV完整代码实战（C++版）

一、四大坐标系介绍

1，世界坐标系

从这个世界（world）的视角来看物体
世界坐标系是3D空间坐标，每个点的位置用$(X_w,Y_w,Z_w)$表示

2，相机坐标系

相机本身具有一个坐标系，其也是3D空间坐标
从相机（camera）的视角来看物体，每个点的位置用$(X_c,Y_c,Z_c)$表示

3，图像坐标系

相机坐标系是3D空间的，而我们通过相机拍照得到的照片却是2D平面，这之间涉及到了透视投影(perspective projection)，大白话就是相似三角形，将3D空间上的点映射到2D平面上
图像坐标系是实际的物理坐标系$(x,y)$，其原点位置一般在相机光轴成像与成像平面的交点位置，通常为成像平面的中心点，物理单位为mm

4，像素坐标系

我们在处理图像数据的时候，使用的是像素坐标系$(u,v)$，比如这个图像的大小为1080*720，即长和宽为1080和780个像素；分辨率等相关概念也是这个大概意思，单位是像素pixel
每个像素都有对应的实际物理尺寸，比如1像素=0.5mm
像素有些情况是矩形，分为x和y方向的长度

5，总结

我们需要通过一些手段，将原本在世界坐标系下的点转换到像素坐标系下
世界坐标系是因为物体本身真实存在的位置，而转到像素坐标系下是因为我们在进行图像处理的时候针对的是像素
故，相机标定的最终目的是实现世界坐标系和像素坐标系之间的转换

二、四大坐标系转换

1，世界坐标系——相机坐标系

相机坐标系和世界坐标系都是3D空间坐标系，任何一个空间中的点，都可以通过旋转和平移进行相互转换

假设世界坐标系下有个点$P_w(X_w,Y_w,Z_w)$，通过乘以一个变换矩阵（旋转R+平移T）就可以得到相机坐标系下的对应位置$P_c(X_c,Y_c,Z_c)$

$$\left[\begin{array}{c}{X}_c\\ Y_c\\ Z_c\end{array}\right]=\left[\begin{array}{ccc}{R}_{11}& R_{12}& R_{13}\\ R_{21}& R_{22}& R_{23}\\ R_{31}& R_{32}& R_{33}\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\end{array}\right]+\left[\begin{array}{c}{T}_1\\ T_2\\ T_3\end{array}\right]$$

为了后续的计算方便，转换为齐次坐标系进行表示
$$\left[\begin{array}{c}{X}_c\\ Y_c\\ Z_c\\ 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\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]$$

其中这个变换矩阵（旋转R和平移T）称为相机外参：
$$\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\\ 0& 0& 0& 1\end{array}\right]$$

相机外参实现了某点所在的世界坐标系到相机坐标系之间的转换

2，相机坐标系——图像坐标系

就此我们实现了世界坐标系下点$P_w(X_w,Y_w,Z_w)$到其所对应的相机的坐标系下位置$P_c(X_c,Y_c,Z_c)$之间的转换

相机坐标系是3D空间坐标系，而相机拍出来的图片是2D平面，这之间涉及透视投影(perspective projection)，大白话为相似三角形

我们可以看到$X_c,Y_c,Z_c,O_c$这个坐标系为相机坐标系，淡蓝色的平面$x,y,o$为图像坐标系（成像平面）
相机成像的原理是小孔成像，故相机坐标系和图像坐标系的y轴是相反的
相机坐标系的原点$O_c$与图像坐标系的原点$o$之间的距离为焦距$f$（这里假设相机坐标系和图像坐标系的轴是相互平行且放置位置为正中心，但也有不平行情况，就需要考虑角度了）

相机坐标系下的点$P_c(X_c,Y_c,Z_c)$与相机坐标系原点$O_c$的连线过图像坐标系平面上的点$p(x,y)$
也就是相机坐标系下的点$P_c(X_c,Y_c,Z_c)$对应的图像坐标系下的位置为$p(x,y)$
根据相似三角形可知：
$$\{\begin{array}{l}\frac{x}{f}=\frac{X_c}{Z_c}\\ \frac{y}{f}=\frac{Y_c}{Z_c}\end{array}\Rightarrow \{\begin{array}{l}{Z}_c\cdot x=f\cdot X_c\\ Z_c\cdot y=f\cdot Y_c\end{array}$$

转化为矩阵形式
$$Z_c\left[\begin{array}{c}x\\ y\\ 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}{c}{X}_c\\ Y_c\\ Z_c\\ 1\end{array}\right]$$
其中$Z_c$为点所在相机坐标系下的Z轴方向位置，是个常量，又称为比例因子
$f$为相机的焦距

通过该矩阵就可以实现相机坐标系到图像坐标系之间的转换

3，图像坐标系——像素坐标系

图像坐标系是物理坐标系，因为它涉及到具体的尺寸大小，每个像素都有其对应的物理尺寸
一般情况下像素是矩形，通常情况下假设$1pixel=dxmm，1pixel=dymm$，其中$dx$和$dy$表示一个像素的长宽分别为多少mm

已知有个小蓝点在图像坐标系下的位置为$(x,y)$，图像坐标系原点所在像素坐标系下的位置为$(u_0,v_0)$
求解：小蓝点所对应的像素坐标系为多少？

假设：1个像素的长和宽分别为$dx$和$dy$ mm，图像坐标系下1mm对应像素坐标系下为$\frac{1}{dx}$个像素
小蓝点在图像坐标系下$(x,y)$应像素坐标为$(x\ast \frac{1}{dx},y\ast \frac{1}{dy})$，即$(\frac{x}{dx},\frac{y}{dy})$

$$\begin{gathered} u=\frac{x}{dx}+u_0 \\ v=\frac{y}{dy}+v_0 \end{gathered}$$

整理成矩阵形式：
$$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}\frac{1}{dx}& 0& u_0\\ 0& \frac{1}{dy}& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]$$

也可以写成另一种形式

$$\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=\left[\begin{array}{ccc}dx& 0& -u_{0}dx\\ 0& dy& -v_{0}dy\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]$$

由此可以得到一个矩阵，实现该点在图像坐标系$(x,y)$和像素坐标系$(u,v)$下的直接转换

4，各个坐标系转换相互推导结合

我们的最终目的是：世界坐标系转换到像素坐标系

若已知世界坐标系下点的坐标为$P_w(X_w,Y_w,Z_w)$

将世界坐标系$P_w(X_w,Y_w,Z_w)$转换为相机坐标系$P_c(X_c,Y_c,Z_c)$
$$\left[\begin{array}{c}{X}_c\\ Y_c\\ Z_c\\ 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\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]$$

将相机坐标系$P_c(X_c,Y_c,Z_c)$转化为图像坐标系$p(x,y)$

$$Z_c\left[\begin{array}{c}x\\ y\\ 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}{c}{X}_c\\ Y_c\\ Z_c\\ 1\end{array}\right]$$

将图像坐标系$(x,y)$转化为像素坐标系$(u,v)$，这里的$(u_0,v_0)$是图像坐标系的原点所对应的像素坐标系下的位置
$$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}\frac{1}{dx}& 0& u_0\\ 0& \frac{1}{dy}& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]$$

最终进行前后整理可得：
$$Z_c\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}\frac{1}{dx}& 0& u_0\\ 0& \frac{1}{dy}& v_0\\ 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}{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\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]$$

其中$Z_c$表示该点在相机坐标系下的Z轴方向的位置，又称为比例因子，本质是常量

相机内参为：
$$\left[\begin{array}{ccc}\frac{1}{dx}& 0& u_0\\ 0& \frac{1}{dy}& v_0\\ 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}{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\\ 0& 0& 0& 1\end{array}\right]$$

就此我们实现了世界坐标系和像素坐标系的转换，这就是相机标定的意义所在

5，总结

理想状态下的相机标定，其本质是求解相机的内外参数矩阵

相机外参矩阵需要求解旋转和平移共6个参数
相机内参菊展需要求解焦距$f$、图像坐标系原点所在的像素坐标系的坐标$(u_0,v_0)$也称为像主点坐标、单个像素点的长和宽$dx$和$dy$ mm，共5个参数

相机内外参数矩阵需要求解11个参数，这是不考虑畸变的理想状况，但实际相机都不可避免存在畸变，故还需要求解畸变系数

三、畸变

相机标定的最终目的是拿到相机的内外参数矩阵，这些内外参数针对同一个相机是固定的，只需要标定一次即可
相机硬件本身多多少少不可避免存在一定的误差，故需要求解畸变系数用于相机的校准，然后再进行求解内外参矩阵

畸变主要包括（影响最大）：切向畸变和径向畸变，是相机本身无法避免的误差

1，切向畸变

切向畸变产生于相机组装过程中，透镜本身与相机传感器成像平面不平行

2，径向畸变

径向畸变产生于透镜本身的形状，光线在远离透镜中心的地方比靠近中心的地方更加弯曲

径向畸变主要包括：桶形畸变和枕形畸变

切向畸变和径向畸变有对应的模型公式，网上一大堆教程，需要的小伙伴自行学习推导哈

畸变涉及到五个参数：径向畸变参数$k_1、k_2、k_3$；切向畸变参数$p_1、p_2$

四、OpenCV代码实战

求解方法很多，这里以张正友标定法（也称为棋盘格标定法）为例进行演示

1，准备棋盘格数据

①OpenCV自带几张棋盘格图片，大致路径为：opencv\sources\samples\data，当然也可以自己去拍几张棋盘格
把这几张图片放到VS项目中


②新建一个项目，需要用到OpenCV
③跑一下代码即可

2，完整代码

修改地方：
①棋盘格角点行列数：int CHECKERBOARD[2]{ 6,9 };

②图片所在文件夹路径：std::string path = "./image/*.jpg";

#include <opencv2/opencv.hpp>
#include <stdio.h>
#include <iostream>using namespace std;
using namespace cv;// Defining the dimensions of checkerboard
// 定义棋盘格的尺寸
int CHECKERBOARD[2]{ 6,9 }; // 一行有6个，一共有9行     数点的行列---6行9列int main()
{// Creating vector to store vectors of 3D points for each checkerboard image// 创建矢量以存储每个棋盘图像的三维点矢量std::vector<std::vector<cv::Point3f> > objpoints;// Creating vector to store vectors of 2D points for each checkerboard image// 创建矢量以存储每个棋盘图像的二维点矢量std::vector<std::vector<cv::Point2f> > imgpoints;// Defining the world coordinates for 3D points// 为三维点定义世界坐标系std::vector<cv::Point3f> objp;for (int i{ 0 }; i < CHECKERBOARD[1]; i++){for (int j{ 0 }; j < CHECKERBOARD[0]; j++){objp.push_back(cv::Point3f(j, i, 0));}}// Extracting path of individual image stored in a given directory// 提取存储在给定目录中的单个图像的路径std::vector<cv::String> images;// Path of the folder containing checkerboard images// 包含棋盘图像的文件夹的路径std::string path = "./image/*.jpg";// 使用glob函数读取所有图像的路径cv::glob(path, images);cv::Mat frame, gray;// vector to store the pixel coordinates of detected checker board corners// 存储检测到的棋盘转角像素坐标的矢量std::vector<cv::Point2f> corner_pts;bool success;// Looping over all the images in the directory// 循环读取图像for (int i{ 0 }; i < images.size(); i++){frame = cv::imread(images[i]);if (frame.empty()){continue;}if (i == 40){int b = 1;}cout << "the current image is " << i << "th" << endl;cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY);// Finding checker board corners// 寻找角点// If desired number of corners are found in the image then success = true// 如果在图像中找到所需数量的角，则success = true// opencv4以下版本，flag参数为CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGEsuccess = cv::findChessboardCorners(gray, cv::Size(CHECKERBOARD[0], CHECKERBOARD[1]), corner_pts, CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_FAST_CHECK | CALIB_CB_NORMALIZE_IMAGE);/** If desired number of corner are detected,* we refine the pixel coordinates and display* them on the images of checker board*/// 如果检测到所需数量的角点，我们将细化像素坐标并将其显示在棋盘图像上if (success){// 如果是OpenCV4以下版本，第一个参数为CV_TERMCRIT_EPS | CV_TERMCRIT_ITERcv::TermCriteria criteria(TermCriteria::EPS | TermCriteria::Type::MAX_ITER, 30, 0.001);// refining pixel coordinates for given 2d points.// 为给定的二维点细化像素坐标cv::cornerSubPix(gray, corner_pts, cv::Size(11, 11), cv::Size(-1, -1), criteria);// Displaying the detected corner points on the checker board// 在棋盘上显示检测到的角点cv::drawChessboardCorners(frame, cv::Size(CHECKERBOARD[0], CHECKERBOARD[1]), corner_pts, success);objpoints.push_back(objp);imgpoints.push_back(corner_pts);}cv::imshow("Image", frame);cv::waitKey(0);}cv::destroyAllWindows();cv::Mat cameraMatrix, distCoeffs, R, T;/** Performing camera calibration by* passing the value of known 3D points (objpoints)* and corresponding pixel coordinates of the* detected corners (imgpoints)*/// 通过传递已知3D点（objpoints）的值和检测到的角点（imgpoints）的相应像素坐标来执行相机校准cv::calibrateCamera(objpoints, imgpoints, cv::Size(gray.rows, gray.cols), cameraMatrix, distCoeffs, R, T);// 内参矩阵std::cout << "cameraMatrix : " << std::endl;std::cout << cameraMatrix << std::endl;// 透镜畸变系数std::cout << "distCoeffs : " << std::endl;std::cout << distCoeffs << std::endl;// rvecsstd::cout << "Rotation vector : " << std::endl;std::cout << R << std::endl;// tvecsstd::cout << "Translation vector : " << std::endl;std::cout << T << std::endl;return 0;
}

3，运行效果

求解得到内参矩阵、透镜畸变系数、旋转和平移向量