OpenCV系列【2】，一个简单定标并储存结果的程序

基于张正友定标法的Opencv3.1定标程序，先用findchessboard找到棋盘，用cornersubpix做亚像素定位，再用calibrateCamera进行定标，最后将定标结果储存在xml文件里
程序基于vs2013和opencv3.1，要注意的是3.1根之前2XX系列很多地方不同，很多函数不同，具体情况看代码，
新手，难免犯诸如switch语句没有default之类的错误，大神看见请轻拍...

上一个版本看的人不少，回帖确是寥寥无几，只能高呼一声，看帖要回啊！

更新一个版本，修改了xml文件编辑，可以直接从生成的xml文件提取标定结果，用于双目联合定标或者其他的什么工作。

两段程序，一个是修改之后的定标程序，一个是提取定标参数并矫正摄像头的程序，均是使用opencv3.1+vs2013编写的。

/***********************************************************************************
*  @COPYRIGHT NOTICE
*  @Copyright (c) 2016, LiZichuan
*  @All rights reserved

文件名  : 0.0，摄像头的打开.cpp
版本	: ver 1.0

作者   	: LiZichuan
日期    : 2016/3/22 13:37
简介    : 0.0，棋盘格读取与标定
***********************************************************************************/

/*****************************************************************************
功    能	:头文件、命名空间包含部分
描    述	:
*****************************************************************************/
#include "stdafx.h"
#include "windows.h"
//#include "stdio.h"
#include "time.h"
#include "iostream"
#include "opencv2/core.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/opencv.hpp"
#include "opencv2/calib3d.hpp"
#include "opencv2/xfeatures2d.hpp"

using namespace std;
using namespace cv;
using namespace cv::xfeatures2d;


/***************************************************************************** 
    功    能	:宏定义部分
    描    述	:
*****************************************************************************/  


/***************************************************************************** 
    功    能	:全局变量声明
    描    述	:
*****************************************************************************/  


/***************************************************************************** 
    功    能	:全局函数声明
    描    述	:
*****************************************************************************/  
//显示矩阵
void PrintfMatrix(Mat umatrix);
//显示矩阵组
void PrintfVectorMat(vector umatrix);


/*****************************************************************************
函数名称	:main()函数
函数功能	:
作    者	:LiZichuan
日    期	:2016/3/22 13:40
*****************************************************************************/
int _tmain(int argc, _TCHAR* argv[])
{
	//1，定标准备
	//参数准备	
	const double RealLengthOfChess = 20.2;							//棋盘格单格实际宽度
	const int chessBoard_Num = 20;									//棋盘数量——定标图像读取次数
	const int board_wide_Num = 9;									//棋盘格每行格子数量
	const int board_heigh_Num = 9;									//棋盘格每列格子数量

	Mat tempImage, tempImage_gray;									//定义原始图像和灰度图
	vector corners;										//每次检测得到的角点坐标
	vector> imagePoints;							//定义检测到角点在图像中的二重二维坐标组
	vector> objectPoints;							//定义检测到角点在实际中的二重三维坐标组
	bool flag_chess_find;											//定义棋盘检测成功的标志位

	VideoCapture cap(1);											//定义视频流
	namedWindow("棋盘角点检测", WINDOW_AUTOSIZE);					//创建源视频窗口()
	namedWindow("标定及矫正后", WINDOW_AUTOSIZE);					//创建矫正后视频窗口
	cap >> tempImage;												//采集视频
	Size imageSize = Size(tempImage.cols, tempImage.rows);			//定义采集视频窗口尺寸

	//2，定标并显示结果，进行矫正并显示图像
	while (true)
	{
		//2.1，开始定标
		cout << "程序准备完成，按下‘c’开始采集图像" << endl;
		if (waitKey(0) == 'c')
		{
			//2.1.1，开始第n次图像读取并检测棋盘图，进行定标
			for (int detectTime = 0; detectTime < chessBoard_Num; ++detectTime)
			{
				//2.1.1.1，从摄像头采集图像
				cout << "按下'v',开始检测棋盘格" << endl;
				bool continue_flag = 0;												//定义一个布尔变量作为棋盘检测成功的标志位			
				//不断循环采集图像，直到按下V按键进行棋盘格检测
				while (1)							
				{
					cap >> tempImage;												//采集视频
					cvtColor(tempImage, tempImage_gray, CV_BGR2GRAY);				//将采集到视频转化为灰度图
					imshow("棋盘角点检测", tempImage);								//显示源视频

					//按下V，开始采集图像
					if (waitKey(1) == 'v')							
					{
						//检测棋盘格，并生成检测成功标志位
						flag_chess_find = findChessboardCorners(tempImage, Size(9, 9), corners, CV_CALIB_CB_ADAPTIVE_THRESH + CV_CALIB_CB_FAST_CHECK);
						//如果检测成功则按照不同颜色依次序画出角点，如果未成功则用红色画出检测到的角点
						drawChessboardCorners(tempImage, Size(9, 9), corners, flag_chess_find);	

						//显示角点
						imshow("棋盘角点检测", tempImage);
						cout << "第" << detectTime << "次棋盘检测结果如下，v按‘c’继续标定，按‘r’重新检测" << endl;

						//等待按键
						switch (waitKey(0))
						{
						case 'c':continue_flag = 1;
						default:break;
						}
					}
					//根据上一个函数结果，如果按下C则跳出循环执行下一步，否则继续循环采集视频
					if (continue_flag)					
						break;				
				}

				//2.1.1.2，如果角点检测成功，进行亚像素焦点定位并储存角点坐标于坐标组中
				if (corners.size() > 2)
				{
					//亚像素角点检测
					cornerSubPix(tempImage_gray, corners, Size(5, 5), Size(-1, -1), TermCriteria(TermCriteria::EPS + TermCriteria::MAX_ITER, 1000, 0.00001));
					//绘制图像并显示
					for (int i = 0; i < (int)corners.size(); i++)
					{
						circle(tempImage, corners[i], 8, Scalar(40, 25, 255), 2, 8);
					}
					imshow("棋盘角点检测", tempImage);

					//显示文字提示并循环显示检测到的棋盘图角点坐标
					cout << "棋盘格亚像素精确定位结果如下" << endl;
					cout << "	棋盘检测角点数目：" << corners.size() << endl;
					for (int i = 0; i < (int)corners.size(); i++)
					{
						if (i%2 == 0)	//如果是偶数则下一次继续写（角点标号从0开始）
						{
							cout << "角点" << i << "精确坐标为：" << corners[i].x << "," << corners[i].y;
						}
						else			//如果是奇数则下一次回车
						{
							cout << "   角点" << i << "精确坐标为：" << corners[i].x << "," << corners[i].y << endl;
						}
					}
					cout << endl;
					cout << "第" << detectTime + 1 << "次棋盘检测结束" << endl;
					cout << "按‘r’重新进行本次检测，按‘c’进行下次检测" << endl;

					//等待按键，如果按下C则储存角点坐标，按下R则重新检测棋盘格获取角点坐标
					switch (waitKey(0))
					{
					case 'r': detectTime = detectTime - 1;							//本次检测结果作废，重新检测
					case 'c':														//将角点坐标存入二重坐标组
					{
						vector imagePoints_temp;							//定义临时的角点二重二维坐标组		
						vector objectPoints_temp;							//定义临时的角点二重三维坐标组		
						for (int j = 0; j < (int)corners.size(); ++j)				//利用循环依次将检测到角点坐标存入临时角点坐标组
						{
							Point2f imgPoint_temp2;									
							Point3f objPoint_temp2;
							imgPoint_temp2.x = corners[j].x;
							imgPoint_temp2.y = corners[j].y;

							objPoint_temp2.x = j % board_wide_Num * ((float)RealLengthOfChess);
							objPoint_temp2.y = j / board_wide_Num * ((float)RealLengthOfChess);
							objPoint_temp2.z = 0;

							imagePoints_temp.push_back(imgPoint_temp2);
							objectPoints_temp.push_back(objPoint_temp2);
						}
						imagePoints.push_back(imagePoints_temp);					//将临时角点坐标组存入图像角点的二重二维坐标组
						objectPoints.push_back(objectPoints_temp);					//将临时角点坐标组存入实际角点的二重三维坐标组
					}
					}

				}

				//2.1.1.3，如果检测失败，等待进行下一次棋盘检测或者退出
				else
				{
					cout << "棋盘格检测失败，按‘r’重新检测，按‘q’退出" << "检测数量为" << corners.size() << endl;
					switch (waitKey(0))
					{
					case 'r':detectTime = detectTime - 1;
					default: return false;
					}
				}

			}

			//2.1.2，根据之前角点坐标亚像素检测结果，进行相机标定
			Mat intrinsic_Matrix(3, 3, CV_64F), distortion_coeffs(8, 1, CV_64F);	
			vector rvecs, tvecs;												//定义旋转向量组和平移向量组
			double time0 = (double)getTickCount();
			
			//2.1.3，进行定标
			calibrateCamera(objectPoints,											//角点的实际坐标				
							imagePoints,											//角点的图像坐标
							imageSize,												//定标图像尺寸
							intrinsic_Matrix,										//相机内参数矩阵
							distortion_coeffs,										//畸变参数矩阵
							rvecs,													//旋转向量组
							tvecs													//平移向量组
							);

			//2.1.4，标定
			cout << "相机定标结束，用时：" << (getTickCount()-time0)/1000 << "毫秒，定标结果如下：" << endl;

			//2.1.5，计算定标误差：
			double total_error_value = 0.0, error_value[chessBoard_Num];
			for (int i = 0; i < chessBoard_Num; i++)
			{
				vector imagePoints2;										//定义一个临时图像角点二重二维坐标组
				//根据旋转向量、平移向量、内参矩阵和相机畸变矩阵计算实际角点在图像坐标席上投影
				projectPoints(objectPoints[i],										//角点实际坐标
					rvecs[i],														//旋转向量
					tvecs[i],														//平移向量
					intrinsic_Matrix,												//内参矩阵
					distortion_coeffs,												//畸变矩阵
					imagePoints2													//储存计算得到的图像角点坐标
					);
				Mat tempImagePointMat = Mat(1, imagePoints[i].size(), CV_32FC2);	//创建用于储存角点的图像坐标的矩阵
				Mat	imagePoints2Mat = Mat(1, imagePoints2.size(), CV_32FC2);		//创建用于储存角点的图像坐标理论值的矩阵
				for (int j = 0; j < (int)imagePoints[i].size(); j++)
				{
					imagePoints2Mat.at(0, j) = Vec2f(imagePoints2[j].x, imagePoints2[j].y);				//将角点图像坐标存入矩阵
					tempImagePointMat.at(0, j) = Vec2f(imagePoints[i][j].x, imagePoints[i][j].y);		//将角点图像坐标理论值存入矩阵
				}
				error_value[i] = norm(imagePoints2Mat, tempImagePointMat, NORM_L2);								//对图像坐标和图像坐标理论值进行归一化
				total_error_value += error_value[i] /= imagePoints[i].size();									//结果累加然后除以角点数

				cout << "第" << i + 1 << "幅图像的平均误差：" << error_value[i] << "像素" << endl;
			}
			cout << "总体平均误差：" << total_error_value / chessBoard_Num << "像素" << endl;

			//2.1.6，显示定标结果
			//在命令台显示结果
			printf("相机内参矩阵：\n");												
			PrintfMatrix(intrinsic_Matrix);											//命令台显示相机内参矩阵

			printf("相机畸变矩阵：\n");
			PrintfMatrix(distortion_coeffs);										//命令台显示畸变矩阵

			printf("旋转向量组：\n");
			PrintfVectorMat(rvecs);													//命令台显示旋转向量组

			printf("平移向量组：\n");
			PrintfVectorMat(tvecs);													//命令台显示平移向量组

			//2.1.7，储存标定结果
			//定义文件储存指针，创建123.xml文件
			FileStorage fs("123.xml", FileStorage::WRITE);
			time_t currentTime;	
			time(¤tTime);
			fs << "Calibration_Date" << asctime(localtime(¤tTime));			//写入储存标定时间
			fs << "Intrinsic_Matrix" << intrinsic_Matrix;							//写入内参矩阵
			fs << "Distortion_Coeffs" << distortion_coeffs;							//写入畸变矩阵
			fs << "Total_average_error" << total_error_value;						//写入平均误差
			//向123.xml文件写入旋转矢量组
			fs << "Rotation_Vectors" << "[";
			for (int i = 0; i < chessBoard_Num; i ++)
			{
				stringstream cvter;
				string name_str = "The_ErrorValue_of_", name_num;					//组成字符串：“The_(i)th_RotateVec”
				cvter << i;
				cvter >> name_num;
				name_str.append(name_num);
				name_str.append("th_Photo");
				fs << "{:" << name_str << rvecs[i] << "}";							//依次写入旋转向量
			}
			fs << "]";
			//向123.xml文件写入平移矢量组
			fs << "Translation_Vectors" << "[";
			for (int i = 0; i < chessBoard_Num; i++)
			{
				stringstream cvter;
				string name_str = "The_ErrorValue_of_", name_num;					//组成字符串：“The_(i)th_TranslateVec”
				cvter << i;
				cvter >> name_num;
				name_str.append(name_num);
				name_str.append("th_Photo");
				fs << "{:" << name_str << rvecs[i] << "}";							//依次写入平移向量
			}
			fs << "]";
			//向123.xml写入每幅图像误差
			fs << "Errors_of_Per_Image" << "[";
			for (int i = 0; i < chessBoard_Num; i++)
			{
				stringstream cvter;
				string name_str = "The_ErrorValue_of_",name_num;					//组成字符串：“The_ErrorValue_of_(i)th_Photo”
				cvter << i;
				cvter >> name_num;
				name_str.append(name_num);
				name_str.append("th_Photo");
				fs << "{:" << name_str << error_value[i] << "}";					//依次写入图像误差
			}
			fs << "]";
			fs.release();															//123.xml文件写入结束，释放内存
			//定义文件储存指针，创建124.xml文件
			FileStorage fs2("124.xml", FileStorage::WRITE);
			fs2 << "Calibration_Date" << asctime(localtime(¤tTime));			//写入储存标定时间
			//向124.xml文件写入角点图像坐标
			fs2 << "image_corners_coordinates" << "[";
			for (int i = 0; i < chessBoard_Num; i++)
			{
				fs2 << "{:" << "Photos_num" << i << "coordinates" << "[:";			
				for (int j = 0; j < (int)corners.size(); j ++)
				{
					fs2 << imagePoints[i][j] ;										//依次写入角点图像坐标
				}
				fs2 << "]" << "}";
			}
			fs2 << "]";
			fs2 << "object_corners_coordinates" << "[";
			for (int i = 0; i < chessBoard_Num; i++)
			{
				fs2 << "{:" << "Photos_num" << i << "coordinates" << "[:";
				for (int j = 0; j < (int)corners.size(); j++)
				{
					fs2 << objectPoints[i][j] ;										//依次写入角点实际坐标
				}
				fs2 << "]" << "}";
			}
			fs2 << "]";
			fs2.release();															//124.xml文件写入结束，释放内存

			//2.1.8，使用定标结果矫正采集到图像，并显示出来
			cout << "显示矫正结果，按'w'键继续" << endl;
			//不断循环显示矫正后图像
			while (true)
			{
				Mat srcImage, srcImage1;											//定义临时矩阵变量
				cap >> srcImage;													//从相机采集图像

				//进行矫正
				undistort(srcImage,													//输入源图像	
							srcImage1,												//存放矫正后图像
							intrinsic_Matrix,										//内参矩阵
							distortion_coeffs										//畸变矩阵
							);

				imshow("棋盘角点检测", srcImage);									//显示结果
				imshow("标定及矫正后", srcImage1);
				if (waitKey(5) == 'q')
					break;
			}
			cout << "本轮定标结束，按‘q’键退出，‘r’键进行下一轮定标" << endl;
		}

		//2.2，如果按下q，则退出循环，按下r，重新定标
		if (waitKey(0) == 'q')
		{
			break;
		}
		else
		{
			while (waitKey(0) != 'r');
		}
 	}
	return 0;
}


/***************************************************************************** 
    函数名称	:PrintfMatrix()函数
    函数功能	:
    作    者	:LiZichuan
    日    期	:2016/3/26 20:51 
*****************************************************************************/  
void PrintfMatrix(Mat umatrix)
{
	for (int i = 0; i < umatrix.rows; ++ i)
	{
		for (int j = 0; j < umatrix.cols; ++ j)
		{
			printf("  %lf", umatrix.at(i, j));
		}
		printf("\n");
	}
	printf("\n");
}


/***************************************************************************** 
    函数名称	:PrintfVectorMat()函数
    函数功能	:
    作    者	:LiZichuan
    日    期	:2016/3/26 20:51 
*****************************************************************************/  
void PrintfVectorMat(vector umatrix)
{
	for (int i = 0; i < (int)umatrix.size(); ++ i)
	{
		for (int j = 0; j < umatrix[i].rows; ++ j)
		{
			for (int k = 0; k < umatrix[i].cols; ++ k)
			{
				printf("  %lf", umatrix[i].at(j, k));
			}
			printf("\n");
		}
		printf("\n");
	}
}

以下是矫正程序：

/***************************************************************************** 
    函数名称	:main()函数
    函数功能	:
    作    者	:LiZichuan
    日    期	:2016/3/31 9:55 
*****************************************************************************/  
int _tmain(int argc, _TCHAR* argv[])
{
	VideoCapture cap_L(0), cap_R(1);

	namedWindow("左相机图像", WINDOW_AUTOSIZE);
	namedWindow("右相机图像", WINDOW_AUTOSIZE);

	Mat IntrinsicMatrix_R, DistortionCoeffs_R, IntrinsicMatrix_L, DistortionCoeffs_L;
	FileStorage fs("calibrate_result_right.xml", FileStorage::READ);
	fs["Intrinsic_Matrix"] >> IntrinsicMatrix_R;
	fs["Distortion_Coeffs"] >> DistortionCoeffs_R;
	fs.release();

	FileStorage fs2("calibrate_result_left.xml", FileStorage::READ);
	fs2["Intrinsic_Matrix"] >> IntrinsicMatrix_L;
	fs2["Distortion_Coeffs"] >> DistortionCoeffs_L;
	fs2.release();

	cout << "Intrinsic Matrix" << endl << IntrinsicMatrix_L << endl << "Distortion Coeffs" << endl << DistortionCoeffs_L << endl;
	cout << "Intrinsic Matrix" << endl << IntrinsicMatrix_R << endl << "Distortion Coeffs" << endl << DistortionCoeffs_R << endl;

	while (true)
	{
		Mat srcImage_L, srcImage_R, srcImage_temp_L, srcImage_temp_R;
		cap_L >> srcImage_L;
		cap_R >> srcImage_R;

		undistort(srcImage_L,
			srcImage_temp_L,
			IntrinsicMatrix_L,
			DistortionCoeffs_L
			);
		undistort(srcImage_R,
			srcImage_temp_R,
			IntrinsicMatrix_R,
			DistortionCoeffs_R
			);

 		imshow("左相机图像", srcImage_temp_L);
 		imshow("右相机图像", srcImage_temp_R);






		if (waitKey(20) == 'q')
		{
			break;
		}
	}

	return 0;
}