计算机视觉教程0-4：手推张正友标定法，详解图像去畸变(附代码)

---

1 相机为什么要标定？

直接给结论：相机标定的目的是为了获得相机矩阵和畸变系数。这部分内容可以参考计算机视觉教程0-3：为何拍照会有死亡视角？详解相机矩阵与畸变。获得了相机矩阵和畸变系数后就能进行更高级的应用，主要应用于立体视觉领域，例如视觉SLAM、三维导航、双目测距等，总之，相机标定相当重要。

相机标定的经典方法是张正友标定法，需要下面的张正友标定板。本节将详细地从底层数学原理推导张正友标定法。

2 反解相机矩阵

不考虑畸变，对图像中标定板角点进行检测，得到角点像素坐标值${\left[\begin{array}{ccc}u& v& 1\end{array}\right]}^T$，根据已知棋盘格大小，计算世界坐标系下标定板角点的世界坐标值${\left[\begin{array}{cccc}{}^{\mathbf{W}}X& {}^{\mathbf{W}}Y& {}^{\mathbf{W}}Z& 1\end{array}\right]}^T$，此时从世界坐标到像素坐标的映射为

$$\left[\begin{array}{c}\hat{u}\\ \hat{v}\\ 1\end{array}\right]=\mathbf{K}\left[\begin{array}{cc}{}_{\mathbf{W}}^{\mathbf{C}}\mathbf{R}& {}^{\mathbf{C}}{\mathbf{p}}_{w_0}\end{array}\right]\left[\begin{array}{c}{}^{\mathbf{W}}X\\ {}^{\mathbf{W}}Y\\ {}^{\mathbf{W}}Z\\ 1\end{array}\right]$$

通常将世界坐标系与标定板固连使${}^{\mathbf{W}}Z=0$，如图所示

此时映射关系简化为

$$\left[\begin{array}{c}\hat{u}\\ \hat{v}\\ 1\end{array}\right]=\mathbf{K}\left[\begin{array}{ccc}{\mathbf{r}}_1& {\mathbf{r}}_2& \mathbf{p}\end{array}\right]\left[\begin{array}{c}{}^{\mathbf{W}}X\\ {}^{\mathbf{W}}Y\\ 1\end{array}\right]$$

其中${\mathbf{r}}_1$、${\mathbf{r}}_2$是旋转矩阵${}_{\mathbf{W}}^{\mathbf{C}}\mathbf{R}$的前两列。

设从标定板到成像平面的单应性矩阵为

$$\mathbf{H}=\left[\begin{array}{ccc}{h}_{11}& h_{12}& h_{13}\\ h_{21}& h_{22}& h_{23}\\ h_{31}& h_{32}& h_{33}\end{array}\right]=\left[\begin{array}{ccc}{\mathbf{h}}_1& {\mathbf{h}}_2& {\mathbf{h}}_3\end{array}\right]$$

这部分内容可以参考计算机视觉教程1-2：单应性矩阵估计

下面基于已知的$\mathbf{H}$反解内参矩阵$\mathbf{K}$，代入${\mathbf{h}}_i={\mathbf{K}\mathbf{r}}_i\left(i=1,2\right)$可得

$$\{\begin{array}{l}{\mathbf{h}}_1^T{\mathbf{Q}\mathbf{h}}_2=0\\ {\mathbf{h}}_1^T{\mathbf{Q}\mathbf{h}}_1={\mathbf{h}}_2^T{\mathbf{Q}\mathbf{h}}_2=1\end{array}$$

其中对称矩阵$\mathbf{Q}={\mathbf{K}}^{-T}{\mathbf{K}}^{-1}$

考虑到

$$\begin{gathered} {\mathbf{h}}_i^T{\mathbf{Q}\mathbf{h}}_j=\left[\begin{array}{ccc}{h}_{i1}& h_{i2}& h_{i3}\end{array}\right]\left[\begin{array}{ccc}{q}_1& q_2& q_3\\ q_2& q_4& q_5\\ q_3& q_5& q_6\end{array}\right]\left[\begin{array}{c}{h}_{j1}\\ h_{j2}\\ h_{j3}\end{array}\right] \\ ={\mathbf{v}}_{ij}^T\mathbf{q} \end{gathered}$$

可以改写为

$$\left[\begin{array}{c}{\mathbf{v}}_{12}^T\\ {\mathbf{v}}_{11}^T-{\mathbf{v}}_{22}^T\end{array}\right]\mathbf{q}=0$$

其中矩阵$\mathbf{V}$完全由已知的$\mathbf{H}$导出，$\mathbf{q}$为$\mathbf{Q}$的向量形式。由于一个$\mathbf{H}$最多只能贡献两个线性方程，因此确定内参矩阵至少要3张标定板(工程上一般取15~20张为宜)，这些方程共同构成$\mathbf{V}\mathbf{q}=0$，可用计算机视觉教程1-2：单应性矩阵估计的基本DLT算法解算，解出来就是相机内参矩阵。

3 反解畸变系数

张氏标定法仅考虑畸变中影响较大的径向畸变而忽略切向畸变，用求得的内参矩阵反解畸变参数。

设无畸变像素坐标、成像平面坐标分别为${\left[\begin{array}{cc}\hat{u}& \hat{v}\end{array}\right]}^T$、${\left[\begin{array}{cc}\hat{x}& \hat{y}\end{array}\right]}^T$；有畸变像素坐标、成像平面坐标分别为${\left[\begin{array}{cc}u& v\end{array}\right]}^T$、${\left[\begin{array}{cc}x& y\end{array}\right]}^T$；设内参$s=0$，则

$$\{\begin{array}{l}u=f_{u}x+c_u\\ v=f_{v}y+c_v\end{array}\{\begin{array}{l}\hat{u}=f_u\hat{x}+c_u\\ \hat{v}=f_v\hat{y}+c_v\end{array}$$

两式相减，并代入$f_{u}x=u-c_u$、$f_{v}y=v-c_v$以及径向畸变公式

$$\{\begin{array}{l}\hat{x}=x\left(1+{\kappa }_1{r}^2+{\kappa }_2{r}^4\right)\\ \hat{y}=y\left(1+{\kappa }_1{r}^2+{\kappa }_2{r}^4\right)\end{array}$$

即得

$$\left[\begin{array}{cc}\left(u-c_u\right)r^2& \left(u-c_u\right)r^4\\ \left(v-c_v\right)r^2& \left(v-c_v\right)r^4\end{array}\right]\left[\begin{array}{c}{\kappa }_1\\ {\kappa }_2\end{array}\right]=\left[\begin{array}{c}\hat{u}-u\\ \hat{v}-v\end{array}\right]$$

考虑到

$$\left[\begin{array}{c}\hat{u}\\ \hat{v}\\ 1\end{array}\right]=\mathbf{K}\left[\begin{array}{cc}{}_{\mathbf{W}}^{\mathbf{C}}\mathbf{R}& {}^{\mathbf{C}}{\mathbf{p}}_{w_0}\end{array}\right]\left[\begin{array}{c}{}^{\mathbf{W}}X\\ {}^{\mathbf{W}}Y\\ {}^{\mathbf{W}}Z\\ 1\end{array}\right]$$

对于$M$幅标定板图片，每张图片取$N$个角点，则可构造非齐次线性方程

$${\mathbf{D}}_{2MN\times 2}\mathbf{\kappa }={\mathbf{d}}_{2MN\times 1}$$

其中$\mathbf{\kappa }={\left[\begin{array}{cc}{\kappa }_1& {\kappa }_2\end{array}\right]}^T$即为畸变系数。通过最小二乘法进行回归

$$\mathbf{\kappa }={\left({\mathbf{D}}^T\mathbf{D}\right)}^{-1}{\mathbf{D}}^T\mathbf{d}$$

即得畸变系数

4 去畸变原理与实战

综合上面两节，给出去畸变的原理

• 准备张氏标定法的标准棋盘格，用相机对其进行不同角度拍摄，得到一组图像
• 忽略畸变，每张标定板可通过世界坐标与像素坐标的关系，求解二者间的单应性矩阵，用于反解相机内参矩阵；忽略切向畸变与内参矩阵误差，反解径向畸变参数
• 利用L-M(Levenberg Marquardt)等非线性迭代优化算法对上述参数进行优化，得到最终的内参矩阵与畸变参数。

按照上面的原理，我们将相机标定与去畸变的代码写出来

/*
 * @breif:采用张氏标定法标定相机并生成标定文件.txt
 * @param[in]:folderPath->标定板图像集的文件夹路径;
 * @param[in]:boardSize->标定板上每行、列的内角点数;
 * @param[in]:realSizePerBox->实际测量的标定板棋盘格几何尺寸，与相机矩阵单位pix/mm对应，Unit:mm;
 * @param[in]:savePath->标定文件.txt的保存路径;
 * @retval:None
 */
void cameraCalibration(String folderPath, Size boardSize, Size realSizePerBox, String savePath)
{
	std::vector<cv::String> fileNames;						// 存放标定图片文件名的序列
	cv::glob(folderPath, fileNames);						// 将标定文件夹中的文件名读取到序列

	Mat cameraMatrix = Mat(3, 3, CV_32FC1, Scalar::all(0)); // 像机内参矩阵
	Mat distCoeffs = Mat(1, 5, CV_32FC1, Scalar::all(0));	// 像机畸变系数：k1,k2,p1,p2,k3
	vector<Mat> tvecsMat;									// 相机外参->旋转向量
	vector<Mat> rvecsMat;									// 相机外参->平移向量
	int imgCnt = 0;											// 图像数量
	vector<int> ptCnt;										// 图像中角点的数量
	Size imgSize;											// 图像尺寸
	vector<Point2f> imgPtBuffer;							// 缓存每幅图像上检测到的角点
	vector<vector<Point2f>> imgPtSequence;					// 检测到的所有角点序列，Unit:pix
	vector<vector<Point3f>> realImgPtSequence;				// 检测到的所有角点序列，Unit:mm
	double totalErr = 0.0;									// 所有图像的重投影误差和

	/*========================================= 像素平面角点坐标提取 =========================================================*/
	for (size_t i = 0; i < fileNames.size(); ++i)
	{
		imgCnt++;
		printf("read the number %d image\n", i+1);
		Mat imgInput = imread(fileNames[i]);
		ptCnt.push_back(boardSize.width * boardSize.height);	// 初始化每幅图像中的角点数量，假定每幅图像中都可看到完整标定板

		if (imgCnt == 1)								//读入第一张图片时获取图像宽高信息
		{
			imgSize.width = imgInput.cols;
			imgSize.height = imgInput.rows;
		}

		if (findChessboardCorners(imgInput, boardSize, imgPtBuffer) == 0)	// 提取ChessBoard类型角点
		{
			printf("can't find the corner point of image number %d, please check!\n", i);
			return;
		}
		else
		{
			Mat imgGray;
			cvtColor(imgInput, imgGray, CV_RGB2GRAY);	//图像灰度化
			find4QuadCornerSubpix(imgGray, imgPtBuffer, Size(11, 11));		//对粗提取的角点进行亚像素精确化
			imgPtSequence.push_back(imgPtBuffer);	
		}
	}

	/*========================================= 几何平面角点坐标提取 =========================================================*/
	for (int i = 0; i < imgCnt; i++)
	{
		vector<Point3f> tempPtSetPerImg;
		for (int j = 0; j < boardSize.height; j++)
		{
			for (int k = 0; k < boardSize.width; k++)
			{
				Point3f realPoint;
				realPoint.x = j * realSizePerBox.width;
				realPoint.y = k * realSizePerBox.height;
				realPoint.z = 0;						// 张氏标定法假设标定板放在世界坐标系中z=0的平面上
				tempPtSetPerImg.push_back(realPoint);
			}
		}
		realImgPtSequence.push_back(tempPtSetPerImg);
	}

	/*======================================== 相机标定与重投影误差计算 ======================================================*/
	calibrateCamera(realImgPtSequence, imgPtSequence, imgSize, cameraMatrix, distCoeffs, rvecsMat, tvecsMat);
	totalErr = calReprojectErr(realImgPtSequence, imgPtSequence, rvecsMat, tvecsMat, cameraMatrix, distCoeffs, ptCnt);
}

看看效果，只看标定格：

---

计算机视觉基础教程说明

章号                                    内容
  0                              色彩空间与数字成像
  1                              计算机几何基础
  2                              图像增强、滤波、金字塔
  3                              图像特征提取
  4                              图像特征描述
  5                              图像特征匹配
  6                              立体视觉
  7                              项目实战

更多精彩专栏：

• 《机器人原理与技术》
• 《计算机视觉教程》
• 《机器学习》
• 《嵌入式系统》
• 《数值优化方法》
• …

欢迎加入社区和更多志同道合的朋友交流：AI 技术社

配套代码 · 优质体验 · 系统知识 请关注