相机标定(一)——内参标定与程序实现

1
2
3
4
bool findChessboardCorners(InputArray image, 
                           Size patternSize, 
                           OutputArray corners, 
                           int flags=CALIB_CB_ADAPTIVE_THRESH+CALIB_CB_NORMALIZE_IMAGE )

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

1
bool find4QuadCornerSubpix(InputArray img, InputOutputArray corners, Size region_size);  

1
2
3
4
5
6
7
8
9
double 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) )

1
2
3
4
5
6
7
8
void projectPoints( InputArray objectPoints,  
                               InputArray rvec, 
                               InputArray tvec,  
                               InputArray cameraMatrix, 
                               InputArray distCoeffs,  
                               OutputArray imagePoints,  
                               OutputArray jacobian=noArray(),  
                               double aspectRatio=0 ); 

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
	//保存定标结果      
	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;
	}
	fout << endl;

1
2
3
4
5
6
7
	//保存相机内参数矩阵和畸变系数和相机距离到xml
	cout << "开始保存相机内参数矩阵和畸变系数………………" << endl;
	string xmlResult = filePath + "\calibrateImage\caliberation_camera.xml";
	FileStorage fs(xmlResult, FileStorage::WRITE); //创建XML文件  
	fs << "zConst" << 100.0;
	fs << "cameraMatrix" << cameraMatrix << "distCoeffs" << distCoeffs;
	fs.release();

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
#include "stdafx.h"
#include <iostream>  
#include <time.h>
#include <fstream>  
#include <io.h>     
#include <string>
#include <direct.h>    
#include <vector>    
using namespace std;

#include <opencv2/opencv.hpp>
using namespace cv;

//标定板方格边长，行角点，列角点   
#define BOARD_SCALE 30
#define BOARD_HEIGHT 11
#define BOARD_WIDTH 8

//获取特定格式的文件名    
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 main()
{
	vector<string> imageFilesName;
	vector<string> files;
	imageFilesName.clear(); files.clear();
	string filePath;
	cout << "请输入标定照片文件绝对目录路径" << endl;
	cin >> filePath;
	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());

	//读取每一幅图像，从中提取出角点，然后对角点进行亚像素精确化   
	cout << "开始提取角点………………" << endl;
	int image_count = 0;  /* 图像数量 */
	Size image_size;  /* 图像的尺寸 */
	Size board_size = Size(BOARD_HEIGHT, BOARD_WIDTH);    /* 标定板上每行、列的角点数 */
	vector<Point2f> image_points_buf;  /* 缓存每幅图像上检测到的角点 */
	vector<vector<Point2f>> image_points_seq; /* 保存检测到的所有角点 */

	for (int i = 0; i<files.size(); i++)
	{
		cout << files[i] << endl;

		Mat imageInput = imread(files[i]);
		/* 提取角点 */
		if (0 == findChessboardCorners(imageInput, board_size, image_points_buf))
		{
			cout << "can not find chessboard corners!n"; //找不到角点  
			continue;
		}
		else
		{
			//找到一幅有效的图片
			image_count++;
			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;
			}

			Mat view_gray;
			cvtColor(imageInput, view_gray, CV_RGB2GRAY);

			/* 亚像素精确化 */
			find4QuadCornerSubpix(view_gray, image_points_buf, Size(5, 5)); //对粗提取的角点进行精确化，更适合用于棋盘标定  
			/*cornerSubPix(view_gray, image_points_buf,//另一种对粗提取的角点进行精确化
							Size(5, 5),
							Size(-1, -1),
							TermCriteria(TermCriteria::MAX_ITER + TermCriteria::EPS,
							30,        // max number of iterations
							0.1));     // min accuracy*/
			image_points_seq.push_back(image_points_buf);  //保存亚像素角点

														   /* 在图像上显示角点位置 */
			drawChessboardCorners(view_gray, board_size, image_points_buf, true); //用于在图片中标记角点 
			//string filePath = files[i];//写入文件
			string filePath = calibrateDir + "\"+ imageFilesName[i] + ".jpg";
			imwrite(filePath, view_gray);
		}
	}

	int total = image_points_seq.size();
	cout << "共使用了" << total << "幅图片" << endl;
	cout << "角点提取完成！n";
	cout << "开始标定………………n";
	/*棋盘三维信息*/
	Size square_size = Size(BOARD_SCALE, BOARD_SCALE);  /* 实际测量得到的标定板上每个棋盘格的大小 */
	vector<vector<Point3f>> object_points; /* 保存标定板上角点的三维坐标 */

	/*内外参数*/
	Mat cameraMatrix = Mat(3, 3, CV_32FC1, Scalar::all(0)); /* 摄像机内参数矩阵 */
	vector<int> point_counts;  // 每幅图像中角点的数量  
	Mat distCoeffs = Mat(1, 5, CV_32FC1, Scalar::all(0)); /* 摄像机的5个畸变系数：k1,k2,p1,p2,k3 */
	vector<Mat> tvecsMat;  /* 每幅图像的旋转向量 */
	vector<Mat> rvecsMat; /* 每幅图像的平移向量 */
						  /* 初始化标定板上角点的三维坐标 */
	int i, j, t;
	for (t = 0; t<image_count; t++)
	{
		vector<Point3f> tempPointSet;
		for (i = 0; i<board_size.height; i++)
		{
			for (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;
				tempPointSet.push_back(realPoint);
			}
		}
		object_points.push_back(tempPointSet);
	}

	/* 初始化每幅图像中的角点数量，假定每幅图像中都可以看到完整的标定板 */
	for (i = 0; i<image_count; i++)
	{
		point_counts.push_back(board_size.width*board_size.height);
	}

	/* 开始标定 */
	calibrateCamera(object_points, image_points_seq, image_size, cameraMatrix, distCoeffs, rvecsMat, tvecsMat, 0);
	cout << "标定完成！n";

	//对标定结果进行评价
	string txtResult = filePath + "\calibrateImage\caliberation_result.txt";
	ofstream fout(txtResult);  /* 保存标定结果的文件 */

	double total_err = 0.0; /* 所有图像的平均误差的总和 */
	double err = 0.0; /* 每幅图像的平均误差 */
	vector<Point2f> image_points2; /* 保存重新计算得到的投影点 */
	cout << "t每幅图像的标定误差：n";
	fout << "每幅图像的标定误差：n";
	for (i = 0; i<image_count; i++)
	{
		vector<Point3f> tempPointSet = object_points[i];
		/* 通过得到的摄像机内外参数，对空间的三维点进行重新投影计算，得到新的投影点 */
		projectPoints(tempPointSet, rvecsMat[i], tvecsMat[i], cameraMatrix, distCoeffs, image_points2);
		/* 计算新的投影点和旧的投影点之间的误差*/
		vector<Point2f> tempImagePoint = image_points_seq[i];
		Mat tempImagePointMat = Mat(1, tempImagePoint.size(), CV_32FC2);
		Mat image_points2Mat = Mat(1, image_points2.size(), CV_32FC2);
		for (int j = 0; j < tempImagePoint.size(); j++)
		{
			image_points2Mat.at<Vec2f>(0, j) = Vec2f(image_points2[j].x, image_points2[j].y);
			tempImagePointMat.at<Vec2f>(0, j) = Vec2f(tempImagePoint[j].x, tempImagePoint[j].y);
		}
		err = norm(image_points2Mat, tempImagePointMat, NORM_L2);
		total_err += err /= point_counts[i];
		cout << "第" << i + 1 << "幅图像的平均误差：" << err << "像素" << endl;
		fout << "第" << i + 1 << "幅图像的平均误差：" << err << "像素" << endl;
	}

	cout << "总体平均误差：" << total_err / image_count << "像素" << endl;
	fout << "总体平均误差：" << 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;
	}
	fout << endl;

	//保存相机内参数矩阵和畸变系数和相机距离到xml
	cout << "开始保存相机内参数矩阵和畸变系数………………" << endl;
	string xmlResult = filePath + "\calibrateImage\caliberation_camera.xml";
	FileStorage fs(xmlResult, FileStorage::WRITE); //创建XML文件  
	fs << "zConst" << 100.0;
	fs << "cameraMatrix" << cameraMatrix << "distCoeffs" << distCoeffs;
	fs.release();
	//保存平移矩阵和旋转矩阵和s到xml
	string xml2Result = filePath + "\calibrateImage\solvePnP_camera.xml";
	FileStorage fs2(xml2Result, FileStorage::WRITE); //创建XML文件 
	double s = 100.0;
	fs2 << "s" << s;
	fs2 << "rotation_matrix" << rotation_matrix << "tvecsMat" << tvecsMat[0];
	fs2.release();
	cout << "保存完成" << endl;

	//保存矫正图像
	Mat mapx = Mat(image_size, CV_32FC1);
	Mat mapy = Mat(image_size, CV_32FC1);
	Mat R = Mat::eye(3, 3, CV_32F);
	cout << "保存矫正图像" << endl;
	initUndistortRectifyMap(cameraMatrix, distCoeffs, R, cameraMatrix, image_size, CV_32FC1, mapx, mapy);
	for (int i = 0; i != image_count; i++)
	{
		cout << "Frame #" << i + 1 << "..." << endl;
		Mat imageSource = imread(files[i]);
		Mat newimage = imageSource.clone();
		//另一种不需要转换矩阵的方式  
		//undistort(imageSource,newimage,cameraMatrix,distCoeffs);  
		remap(imageSource, newimage, mapx, mapy, INTER_LINEAR);//效率更高
		string imageFilePath = calibrateDir + "\" + imageFilesName[i] +"_d"+ ".jpg";
		imwrite(imageFilePath, newimage);
	}
	cout << "保存结束" << endl;

	cout << "全部工作结束" << endl;
	int key = cvWaitKey(0);
	return;
}