【OpenCv】相机标定介绍及python/c++实现

针孔相机内外参标定简单介绍

之前有一个项目需要公司标内参，之前对这方面没有接触过，网上找了很多资料，记录下相机标定的基础知识。文章是个人浅显理解。如有错误还请指正，非常感谢！

---

参考链接：

1. 坐标系转换：相机参数标定（camera calibration）及标定结果如何使用_Aoulun的博客-CSDN博客
2. 标定opencv代码：Opencv 相机内参标定及使用_Gene_2022的博客-CSDN博客_相机内参标定
3. opencv标定函数介绍：【OpenCV3学习笔记 】相机标定函数 calibrateCamera( ) 使用详解（附相机标定程序和数据）_ZealCV的博客-CSDN博客_calibratecamera参数
4. 标定的数学原理：相机标定之张正友标定法数学原理详解（含python源码） - 知乎 (zhihu.com)
5. 流程及代码：Opencv——相机标定 - 一抹烟霞 - 博客园 (cnblogs.com)
6. 相机模型介绍：相机模型-鱼眼模型/Omnidirectional Camera(1)_苏源流的博客-CSDN博客_davide scaramuzza
7. 图像去畸变，对极约束之undistort,initUndistortRectifyMap，undistort

---

相机标定的目的是找到三维空间与二维空间转换的关系，也称为几何模型，这个几何模型就是相机标定要求解的参数。

1. 坐标系转换

首先需要了解几个坐标系：像素坐标系、图像坐标系、相机坐标系、世界坐标系。

像素坐标系

通常从相机采集到的图像以像素为单位。一般处理时都是以图像的左上角为原点，水平是u方向，竖直是v方向，就是像素坐标系，如下u-v坐标系，某一点在像素坐标系中的位置为 $(u_i,v_j)$。

图像坐标系

图像坐标系以图像中心为原点，以尺寸为单位，通常一个像素的尺寸为$dx\ast dy$。那么像素坐标系与图像坐标系之间就有一个转换关系，图像坐标系以x为水平方向，y为竖直方向。

像素坐标系与图像坐标系的转换关系如下：
$$\{\begin{array}{c}x=udx-u_{0}dx\\ y=vdy-v_{0}dy\end{array}$$
转换为矩阵格式并转换为齐次坐标：
$$\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=\left[\begin{array}{ccc}dx& 0& 0\\ 0& dy& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}u\\ v\\ 0\end{array}\right]+\left[\begin{array}{c}-u_{0}dx\\ -v_{0}dy\\ 1\end{array}\right]$$

齐次坐标可以区分点和向量，便于计算机做图像处理时进行放射变换。
点和向量的区分方式是最后一个数值是否为1，如果为1是点，如果是0是向量。
普通坐标系变换到齐次坐标系：
- 点(x,y,z)变换为(x,y,z,1)；
- 向量(x,y,z)变换为(x,y,z,0)；
齐次坐标系变换到普通坐标系：
- 点(x,y,z,1)变换为(x,y,z)；
- 向量(x,y,z,0)变换为(x,y,z)；

相机坐标系

相机坐标系是一个三维的坐标系，以相机的光轴为$z$轴，光线在相机光学系统的中心位置是原点$O_c$，相机坐标系的$x_c$轴与图像坐标系的$x$轴平行，相机坐标系的$y_c$轴与图像坐标系的$y$轴平行，如下：

在相机坐标系与图像坐标系转换前要了解一下什么是虚平面。我们都知道小孔成像：用一个带小孔的板遮挡在墙体和物之间，墙上就会出现倒立的实像。

本来成像平面在小孔的后面，为了方便计算，我们在物体与小孔中间设置一个虚平面，虚平面到小孔的距离与成像平面到小孔的距离相等，在虚平面上得到的图像就是正的，且与成像平面的大小一致。

根据相似三角形可以得到：

$\frac{O_c{O}_i}{O_{c}B}=\frac{O_{c}C}{O_{c}A}=\frac{pC}{PA}=\frac{O_{i}p}{AB}$

=> $\frac{f}{Z}=\frac{x}{X_c}=\frac{y}{Y_c}$

=> $X_c=\frac{x{Z}_c}{f}$，$Y_c=\frac{y{Z}_c}{f}$

=>
$$\left[\begin{array}{c}{Y}_c\\ X_c\\ Z_c\\ 1\end{array}\right]=\left[\begin{array}{ccc}\frac{Z_c}{f}& 0& 0\\ 0& \frac{Z_c}{f}& 0\\ 0& 0& Z_c\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]$$

世界坐标系

世界坐标系是现实世界的坐标。世界坐标系反映了图像与真实物体之间的一个映射关系。对于单目，是物体真实尺寸与图像尺寸之间的关系，对于双目，需要知道多个相机之间的关系。

世界坐标系的原点是$O_w$，且世界坐标系的$Xw,Y_w,Z_w$轴与其它三个坐标系不是平行的，有一定的旋转和平移。

旋转后坐标系与原来坐标系关系如下：

旋转参数可以表示为：
$$\{\begin{array}{c}{X}_w=R_x{X}_c\\ Y_w=R_y{Y}_c\\ Z_w=R_z{Z}_c\end{array}$$
平移可以表示为：
$$\{\begin{array}{c}{X}_w=X_c+t_x\\ Y_w=Y_c+t_y\\ Z_w=Z_c+t_z\end{array}$$
旋转和平移表示为：
$$\{\begin{array}{c}{X}_w=R_x{X}_c+t_x\\ Y_w=R_y{Y}_c+t_y\\ Z_w=R_z{Z}_c+t_z\end{array}$$

2. 内参和外参

由上面的各个坐标系的变换我们最终可以得到世界坐标系和像素坐标系之间的转换关系：
$$\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]=\left[\begin{array}{cc}R& T\\ 0& 1\end{array}\right]\left[\begin{array}{ccc}\frac{Z_c}{f}& 0& 0\\ 0& \frac{Z_c}{f}& 0\\ 0& 0& Z_c\\ 0& 0& 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]$$
=>
$$\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]=\left[\begin{array}{cc}R& T\\ 0& 1\end{array}\right]\left[\begin{array}{ccc}\frac{Z_{c}dx}{f}& 0& -\frac{Z_c{u}_{0}dx}{f}\\ 0& \frac{Z_{c}dy}{f}& -\frac{Z_c{v}_{0}dy}{f}\\ 0& 0& Z_c\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]$$
其中$\left[\begin{array}{cc}R& T\\ 0& 1\end{array}\right]$表示相机外参，$\left[\begin{array}{ccc}\frac{Z_{c}dx}{f}& 0& -\frac{Z_c{u}_{0}dx}{f}\\ 0& \frac{Z_{c}dy}{f}& -\frac{Z_c{v}_{0}dy}{f}\\ 0& 0& Z_c\\ 0& 0& 1\end{array}\right]$表示相机内参。

3. 畸变

畸变总结起来可以分为两类：径向畸变和切向畸变。

径向畸变

校正公式：
$$\begin{gathered} x_{dr}=x(1+k_1{r}^2+k_2{r}^4+k_3{r}^6) \\ {y}_{dr}=y(1+k_1{r}^2+k_2{r}^4+k_3{r}^6) \\ {r}^2=x^2+y^2 \end{gathered}$$
切向畸变

校正公式：
$$\begin{gathered} x_{dt}=2p_{1}xy+p_2(r^2+2x^2)+1 \\ {y}_{dt}=2p_1(r^2+2y^2)+2p_{2}xy+1 \end{gathered}$$
畸变校正：
$$\begin{gathered} x_d=x_{dr}+xdt \\ {y}_d=y_{dr}+y_{dt} \end{gathered}$$

3. 标定

参考：Opencv——相机标定 - 一抹烟霞 - 博客园 (cnblogs.com)

目的：获取到相机内参和每一幅图像的外参(旋转和平移参数)，有了内参和外参就可以对之后拍摄的图像进行校正。

输入：图像上所有内角点的坐标，标定板上所有内角点的空间三维坐标。

输出：相机的内参和外参。

流程：

1. 准备标定板/chart图，拍摄图片，建议10~20张，不能少于3张；
2. 对每一张标定图片提取角点信息findChessboardCorners()；
3. 对每一张标定图片进一步提取亚像素角点信息cornerSubPix()/ find4QuadCornerSubpix()；
4. 找到每一张标定图片上的内角点(为了显示用) find4QuadCornerSubpix()；
5. 标定(调用函数)calibrateCamera()；
6. 对标定结果进行评价calibrateCamera()；得到内外参后，对空间的三维点进行重新投影计算，得到空间三维点在图像上新的投影点的坐标，计算投影坐标和亚像素角点坐标之间的偏差，偏差越小，标定结果越好；
7. 查看标定效果，利用标定结果对拍摄到的图像进行校正**initUndistortRectifyMap()+remap()/undistort()；

CPP代码：

#include 
#include 
#include 
#include 
#include 
#include 
#include 

using namespace std;
using namespace cv;

void GetAllFormatFiles(string path, vector<string>& files, string format)
{
	//文件句柄      
	//long   hFile = 0;//win7使用
	intptr_t hFile = 0;//win10使用
					   //文件信息      
	struct _finddata_t fileinfo;
	string p;
	if ((hFile = _findfirst(p.assign(path).append("\\*" + format).c_str(), &fileinfo)) != -1)
	{
		do
		{
			if ((fileinfo.attrib & _A_SUBDIR))
			{
				if (strcmp(fileinfo.name, ".") != 0 && strcmp(fileinfo.name, "..") != 0)
				{
					//files.push_back(p.assign(path).append("\\").append(fileinfo.name) );    
					GetAllFormatFiles(p.assign(path).append("\\").append(fileinfo.name), files, format);
				}
			}
			else
			{
				//files.push_back(p.assign(path).append("\\").append(fileinfo.name));//将文件路径保存
				files.push_back(p.assign(fileinfo.name));  //只保存文件名:  
			}
		} while (_findnext(hFile, &fileinfo) == 0);
		_findclose(hFile);
	}
}


void m_calibration(vector<string> &FilesName, Size board_size, Size square_size, Mat &cameraMatrix, Mat &distCoeffs, vector<Mat> &rvecsMat, vector<Mat> &tvecsMat)
{
	ofstream fout("caliberation_result.txt");                       // 保存标定结果的文件 

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

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

	for (int i = 0;i < FilesName.size();i++)
	{
		image_count++;

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

		/* 提取角点 */
		bool ok = findChessboardCorners(imageInput, board_size, 
			image_points,  cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_NORMALIZE_IMAGE);
		if (0 == ok)
		{
			cout <<"第"<< image_count <<"张照片提取角点失败，请删除后，重新标定！"<<endl; //找不到角点
			imshow("失败照片", imageInput);
			waitKey(0);
		}
		else
		{
			Mat view_gray;
			cout << "imageInput.channels()=" << imageInput.channels() << endl;
			cvtColor(imageInput, view_gray, cv::COLOR_RGB2GRAY);

			/* 亚像素精确化 */
			//find4QuadCornerSubpix(view_gray, image_points, Size(5, 5)); //对粗提取的角点进行精确化
			cv::cornerSubPix(view_gray, image_points, cv::Size(11, 11), cv::Size(-1, -1), cv::TermCriteria( TermCriteria::EPS + TermCriteria::COUNT, 20, 0.01));

			image_points_seq.push_back(image_points);  //保存亚像素角点

			/* 在图像上显示角点位置 */
			drawChessboardCorners(view_gray, board_size, image_points, true); 

			//imshow("Camera Calibration", view_gray);//显示图片
			//waitKey(100);//暂停0.1S		
		}
	}
	cout << "角点提取完成！！！" << endl;


	/*棋盘三维信息*/
	vector<vector<Point3f>> object_points_seq;                     // 保存标定板上角点的三维坐标

	for (int t = 0;t < image_count;t++)
	{
		vector<Point3f> object_points;
		for (int i = 0;i < board_size.height;i++)
		{
			for (int j = 0;j < board_size.width;j++)
			{
				Point3f realPoint;
				/* 假设标定板放在世界坐标系中z=0的平面上 */
				realPoint.x = i*square_size.width;
				realPoint.y = j*square_size.height;
				realPoint.z = 0;
				object_points.push_back(realPoint);
			}
		}
		object_points_seq.push_back(object_points);
	}

	/* 运行标定函数 */
	double err_first = calibrateCamera(object_points_seq, image_points_seq, image_size, 
		cameraMatrix, distCoeffs, rvecsMat, tvecsMat, cv::CALIB_FIX_K3);
	fout << "重投影误差1：" << err_first << "像素" << endl << endl;
	cout << "标定完成！！！" << endl;


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

	for (int i = 0;i < image_count;i++)
	{

		projectPoints(object_points_seq[i], rvecsMat[i], tvecsMat[i], cameraMatrix, distCoeffs, image_points_pro);   //通过得到的摄像机内外参数，对角点的空间三维坐标进行重新投影计算

		err = norm(Mat(image_points_seq[i]), Mat(image_points_pro), NORM_L2);

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

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


	//保存定标结果  	
	cout << "开始保存定标结果………………" << endl;
	Mat rotation_matrix = Mat(3, 3, CV_32FC1, Scalar::all(0)); /* 保存每幅图像的旋转矩阵 */
	fout << "相机内参数矩阵：" << endl;
	fout << cameraMatrix << endl << endl;
	fout << "畸变系数：\n";
	fout << distCoeffs << endl << endl << endl;
	for (int i = 0; i < image_count; 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;
}

void m_undistort(const string &path,vector<string> &FilesName, Size image_size, Mat &cameraMatrix, Mat &distCoeffs)
{

	Mat mapx = Mat(image_size, CV_32FC1);   //X 坐标重映射参数
	Mat mapy = Mat(image_size, CV_32FC1);   //Y 坐标重映射参数
	Mat R = Mat::eye(3, 3, CV_32F);
	cout << "保存矫正图像" << endl;
	string imageFileName;                  //校正后图像的保存路径
	stringstream StrStm;
	string temp;

	for (int i = 0; i < FilesName.size(); i++)
	{
		Mat imageSource = imread(FilesName[i]);

		Mat newimage = imageSource.clone();

		//方法一：使用initUndistortRectifyMap和remap两个函数配合实现
		//initUndistortRectifyMap(cameraMatrix,distCoeffs,R, Mat(),image_size,CV_32FC1,mapx,mapy);
		//	remap(imageSource,newimage,mapx, mapy, INTER_LINEAR);

		//方法二：不需要转换矩阵的方式，使用undistort函数实现
		undistort(imageSource, newimage, cameraMatrix, distCoeffs);

		StrStm << i + 1;
		StrStm >> temp;
		imageFileName = path + temp + "_d.jpg";
		imwrite(imageFileName, newimage);

		StrStm.clear();
		imageFileName.clear();
	}
	std::cout << "保存结束" << endl;
}

void main()
{
	vector<string> imageFilesName;
	vector<string> files;
	imageFilesName.clear(); files.clear();
	string filePath = "D:\\chess_image";

	string format = ".jpg";
	GetAllFormatFiles(filePath, imageFilesName, format);
	cout << "找到的文件有" << endl;
	for (int i = 0; i < imageFilesName.size(); i++)
	{
		files.push_back(filePath + "\\" + imageFilesName[i]);
		cout << files[i] << endl;
	}
	string calibrateDir = filePath + "\\calibrateImage\\";
	_mkdir(calibrateDir.c_str());


	Size board_size = Size(7, 6);                         // 标定板上每行、列的角点数 根据实际标定板上的角点个数填写
	Size square_size = 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(files, board_size, square_size, cameraMatrix, distCoeffs, rvecsMat, tvecsMat);
	cv::Size image_size(640, 480);
	m_undistort(calibrateDir,files, image_size, cameraMatrix, distCoeffs);

	return;
}

python代码：

import argparse
from argparse import RawTextHelpFormatter
import numpy as np
import cv2


def cam_calib_find_corners(img, rlt_dir, img_idx, col, row):
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    ret, corners = cv2.findChessboardCorners(gray, (col, row), None)
    # 为了得到稍微精确一点的角点坐标，进一步对角点进行亚像素寻找
    corners2 = cv2.cornerSubPix(gray, corners, (5, 5), (-1, -1),
                                (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_COUNT, 10, 0.001))

    if ret == True:
        # 保存角点图像
        sav_path = rlt_dir + "\\" + str(img_idx) + "_corner.jpg"
        cv2.drawChessboardCorners(img, (col, row), corners2, ret)
        cv2.imwrite(sav_path, img)

    return (ret, corners2)


def cam_calib_calibrate(img_dir, rlt_dir, col, row, img_num):
    w = 0
    h = 0
    all_corners = []
    patterns = []
    # 标定相机,先搞这么一个假想的板子，标定就是把图像中的板子往假想的板子上靠，靠的过程就是计算参数的过程
    x, y = np.meshgrid(range(col), range(row))
    prod = row * col
    pattern_points = np.hstack((x.reshape(prod, 1), y.reshape(prod, 1), np.zeros((prod, 1)))).astype(np.float32)

    for i in range(1, img_num + 1):
        img_path = img_dir + "\\" + str(i) + ".jpg"
        print(img_path)
        # 读取图像
        img = cv2.imread(img_path)
        (h, w) = img.shape[:2]
        # 提取角点
        ret, corners = cam_calib_find_corners(img, rlt_dir, i, col, row)

        # 合并所有角点
        all_corners.append(corners)
        patterns.append(pattern_points)

    rms, cameraMatrix, distCoeffs, rvecs, tvecs = cv2.calibrateCamera(patterns, all_corners, (w, h), None, None)

    return (cameraMatrix, distCoeffs)


def cam_calib_correct_img(crct_img_dir, cameraMatrix, distCoeffs):
    for i in range(1, 3):
        crct_img_path = crct_img_dir + "\\" + str(i) + ".jpg"
        img = cv2.imread(crct_img_path)
        (h1, w1) = img.shape[:2]
        # 对参数做处理，使得最后的输出的矫正图像去表不必要的边缘。
        newcameramtx, roi = cv2.getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, (w1, h1), 1, (w1, h1))

        # 矫正
        dst = cv2.undistort(img, cameraMatrix, distCoeffs, None, newcameramtx)

        # 保存矫正图像
        x, y, w, h = roi
        dst = dst[y:y + h, x:x + w]
        rlt_path = crct_img_dir + "\\rlt\\" + str(i) + "_crct.jpg"
        cv2.imwrite(rlt_path, dst)


if __name__ == "__main__":
    parser = argparse.ArgumentParser(description="读取标定的图片并保存结果", formatter_class=RawTextHelpFormatter)
    parser.add_argument("--img_dir", help="标定图片路径", type=str, metavar='', default="D:\\calib_img")
    parser.add_argument("--rlt_dir", help="保存路径", type=str, metavar='', default="D:\\calib_img\\rlt")
    parser.add_argument("--crct_img_dir", help="待矫正图像路径", type=str, metavar='', default="D:\\calib_img\\crct_img")
    parser.add_argument("--row_num", help="每一行有多少个角点，边缘处的不算", type=int, metavar='', default="7")  # 根据实际标定板填写
    parser.add_argument("--col_num", help="每一列有多少个角点，边缘处的不算", type=int, metavar='', default="6")  # 根据实际标定板填写
    parser.add_argument("--img_num", help="多少幅图像", type=int, metavar='', default="21")

    args = parser.parse_args()

    # 标定相机
    cameraMatrix, distCoeffs = cam_calib_calibrate(args.img_dir, args.rlt_dir, args.row_num, args.col_num, args.img_num)

    # 矫正图片
    cam_calib_correct_img(args.crct_img_dir, cameraMatrix, distCoeffs)