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

基于张正友定标法的Opencv3.1定标程序，先用findchessboard找到棋盘，用cornersubpix做亚像素定位，再用calibrateCamera进行定标，最后将定标结果储存在xml文件里

程序基于vs2013和opencv3.1，要注意的是3.1根之前2XX系列很多地方不同，很多函数不同，具体情况看代码，

新手，难免犯诸如switch语句没有default之类的错误，大神看见请轻拍...

/***********************************************************************************
*  @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 "string"
#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;




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




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




/***************************************************************************** 
    功    能	:全局函数声明
    描    述	:
*****************************************************************************/  
//向xml文件写入矩阵
void uprintMatrix(CvFileStorage *fs, Mat umatrix, string uname);
//向xml文件写入矩阵组
void uprintVectorMat(CvFileStorage *fs, vector<Mat> umatrix, string uname);


//显示矩阵
void PrintfMatrix(Mat umatrix);
//显示矩阵组
void PrintfVectorMat(vector<Mat> umatrix);




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


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


	VideoCapture cap(0);											//定义视频流
	namedWindow("棋盘角点检测", WINDOW_AUTOSIZE);					//创建源视频窗口()
	namedWindow("标定及矫正后", WINDOW_AUTOSIZE);					//创建矫正后视频窗口


	//TermCriteria criteria = TermCriteria(TermCriteria::EPS + TermCriteria::MAX_ITER, 1000, 0.00001);


	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 < 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 < 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<Point2f> imagePoints_temp;							//定义临时的角点二重二维坐标组		
						vector<Point3f> objectPoints_temp;							//定义临时的角点二重三维坐标组		
						for (int j = 0; j < 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<Mat> rvecs, tvecs;												//定义旋转向量组和平移向量组
			double time0 = 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<Point2f> 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 < imagePoints[i].size(); j++)
				{
					imagePoints2Mat.at<Vec2f>(0, j) = Vec2f(imagePoints2[j].x, imagePoints2[j].y);				//将角点图像坐标存入矩阵
					tempImagePointMat.at<Vec2f>(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，储存标定结果
			//定义文件储存指针
			CvFileStorage *fs = cvOpenFileStorage("123.xml",						//文件名称
													0,
													CV_STORAGE_WRITE,				//模式为写入模式
													"GB2312"						//文本编码
													);
			cvWriteComment(fs, "这个文件用于记录定标结果。", 0);					//向文件写入短语


			//储存标定结果
			cvWriteComment(fs, "定标参数如下：", 0);								//向文件写入短语
			uprintMatrix(fs, intrinsic_Matrix, "相机内参矩阵");						//写入相机内参
			uprintMatrix(fs, distortion_coeffs, "相机畸变矩阵");					//写入畸变矩阵
			uprintVectorMat(fs, rvecs, "旋转向量组");								//写入旋转向量组
			uprintVectorMat(fs, tvecs, "平移向量组");								//写入平移向量组
			cvWriteComment(fs, "定标参数写入结束", 0);								//向文件写入短语


			//储存定标误差
			cvWriteComment(fs, "定标误差如下：", 0);								//向文件写入短语
			cvWriteReal(fs, "total_average_error", total_error_value / chessBoard_Num);
																					//写入所有图片的平均误差
			cvWriteComment(fs, "每幅图片定标误差如下：", 0);						//向文件写入短语
			for (int i = 0; i < chessBoard_Num; i++)
			{
				cvWriteComment(fs, "Photo_Counter", 0);
				cvWriteReal(fs, "error_value", error_value[i]);						//写入每幅图片误差	
			}
			cvWriteComment(fs, "误差写入结束", 0);									//向文件写入短语


			//储存角点坐标
			cvWriteComment(fs, "角点图像坐标为：", 0);								//向文件写入短语
			for (int i = 0; i < chessBoard_Num; i++)
			{
				cvWriteInt(fs, "Photes_Counter", i);
				for (int j = 0; j < corners.size(); j++)
				{
					cvWriteInt(fs, "Corners_Counter", j);							//写入角点序号
					float temp_coordinate[] = { imagePoints[i][j].x, imagePoints[i][j].y};
																					//定义用于输出坐标的临时变量
					cvStartWriteStruct(fs, "coordinates", CV_NODE_SEQ);				//开始写入结构体
					cvWriteRawData(fs, temp_coordinate, 2, "f");					//写坐标
					cvEndWriteStruct(fs);											//结构体写入结束
				}
			}
			cvWriteComment(fs, "角点图像坐标写入结束", 0);							//向文件写入短语


			cvWriteComment(fs, "角点实际坐标为：", 0);								//向文件写入短语
			for (int i = 0; i < chessBoard_Num; i++)
			{
				cvWriteInt(fs, "Photos_Counter", i);
				for (int j = 0; j < corners.size(); j++)
				{
					cvWriteInt(fs, "Corners_Counter", j);							//写入角点序号
					float temp_coordinate[] = { objectPoints[i][j].x, objectPoints[i][j].y, objectPoints[i][j].z };
																					//定义用于输出坐标的临时变量
					cvStartWriteStruct(fs, "coordinates", CV_NODE_SEQ);				//开始写入结构体
					cvWriteRawData(fs, temp_coordinate, 3, "f");					//写坐标
					cvEndWriteStruct(fs);											//结构体写入结束
				}
			}
			cvWriteComment(fs, "角点实际坐标写入结束", 0);							//向文件写入短语
			cvReleaseFileStorage(&fs);												//文件写入结束，释放文件指针


			//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;
}




/***************************************************************************** 
    函数名称	:uprintVectorMat()函数
    函数功能	:
    作    者	:LiZichuan
    日    期	:2016/3/26 20:06 
*****************************************************************************/  
void uprintVectorMat(CvFileStorage *fs, vector<Mat> umatrix, string uname)
{


	cvWriteComment(fs, uname.c_str(), 0);
	//逐次按矩阵、按行、按列写入矩阵组值
	for (int i = 0; i < umatrix.size(); ++ i)
	{
		cvWriteInt(fs, "Mat_Number", i);
		for (int j = 0; j < umatrix[i].rows; ++ j)
		{
			for (int k = 0; k < umatrix[i].cols; ++k)
			{
				cvWriteReal(fs, "Value", umatrix[i].at<double>(j, k));
			}
		}
	}
	cvWriteComment(fs, "写入结束", 0);


}




/***************************************************************************** 
    函数名称	:uprintMatrix()函数
    函数功能	:
    作    者	:LiZichuan
    日    期	:2016/3/26 20:21 
*****************************************************************************/  
void uprintMatrix(CvFileStorage *fs, Mat umatrix, string uname)
{
	cvWriteComment(fs, uname.c_str(), 0);
	//逐次按行按列写入矩阵值
	for (int i = 0; i < umatrix.rows; ++ i)
	{
		for (int j = 0; j < umatrix.cols; ++ j)
		{
			cvWriteReal(fs, "Value", umatrix.at<double>(i, j));
		}
	}
	cvWriteComment(fs, "写入结束", 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<double>(i, j));
		}
		printf("\n");
	}
	printf("\n");
}




/***************************************************************************** 
    函数名称	:PrintfVectorMat()函数
    函数功能	:
    作    者	:LiZichuan
    日    期	:2016/3/26 20:51 
*****************************************************************************/  
void PrintfVectorMat(vector<Mat> umatrix)
{
	for (int i = 0; i < 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<double>(j, k));
			}
			printf("\n");
		}
		printf("\n");
	}
}