C/C++实现双目矫正(不使用OpenCV内部函数)及矫正源码解析

C/CPP实现双目矫正(不使用OpenCV)及矫正源码解析

这篇文章是之前【要matlab标定数据做双目相机矫正OpenCV C++】的补充,再加上了双目矫正的原理及代码注释。更新中……

本文所需数据为matlab标定的双目摄像头外参及内参,具体使用stereoCameraCalibrator 步骤可看之前那篇文章。后续将会实现CPP版本的双目标定,又挖了个坑。

补充了,在最下面。
再补充一个棋盘格标定板(可白嫖

找paper搭配 Sci-Hub 食用更佳 (๑•̀ㅂ•́)و✧
Sci-Hub 实时更新 : https://tool.yovisun.com/scihub/
公益科研通文献求助:https://www.ablesci.com/
立体匹配算法排行榜:KITTI 立体匹配算法排行榜,Middlebury 立体匹配算法排行榜
C/C++实现双目矫正(不使用OpenCV内部函数)及矫正源码解析_第1张图片
熟悉立体匹配的同学对这幅图一定不陌生,计算双目深度就是按照上图中的相似三角形进行求解。

视 差     d i s p a r i t y = X R − X T , 视差~~~disparity = X_R - X_T,    disparity=XRXT,

深 度     z / B a s e l i n e = ( z − f ) / ( B a s e l i n e − d i s p a r i t y ) , 深度~~~z / Baseline = (z - f) /( Baseline - disparity),    z/Baseline=(zf)/(Baselinedisparity)
得 到 得到
            z = f ∗ B a s e l i n e / d i s p a r i t y ~~~~~~~~~~~z = f*Baseline/disparity            z=fBaseline/disparity, 也即视差与深度成反比。
重投影矩阵Q
1 / − Q [ 14 ]     b a s e l i n e ( m m )         根 据 标 定 的 相 机 外 参 计 算 。 如 果 只 需 要 相 对 深 度 取 1 即 可 1 / -Q[14] ~~~baseline (mm) ~~~~~~~ 根据标定的相机外参计算。如果只需要相对深度取1即可 1/Q[14]   baseline(mm)       1
Q [ 11 ]             f o c a l   l e n g t h ( p x )     由 标 定 内 参 得 到 Q[11] ~~~~ ~~~~~~~ focal~length (px) ~~~ 由标定内参得到 Q[11]           focal length(px)   

以上的计算建立在双目相机的光轴完全平行,但这是不可能的,为此需要进行标定和矫正。
矫正主函数:

void StereoRectify(double *left_intrinsic, double *left_distort, double *right_intrinsic, double *right_distort,
	double *rotation_mat, double *translation_vector, double *left_rota, double *right_rota, double *left_proj, double *right_proj, double *Q)

旋转矩阵求逆:

void mat33Inverse(double *PR, double *PRI) {
	/* 3*3 matrix inverse */
	double temp;
	temp = PR[0] * PR[4] * PR[8] + PR[1] * PR[5] * PR[6] + PR[2] * PR[3] * PR[7] -
		PR[2] * PR[4] * PR[6] - PR[1] * PR[3] * PR[8] - PR[0] * PR[5] * PR[7];
	PRI[0] = (PR[4] * PR[8] - PR[5] * PR[7]) / temp;
	PRI[1] = -(PR[1] * PR[8] - PR[2] * PR[7]) / temp;
	PRI[2] = (PR[1] * PR[5] - PR[2] * PR[4]) / temp;
	PRI[3] = -(PR[3] * PR[8] - PR[5] * PR[6]) / temp;
	PRI[4] = (PR[0] * PR[8] - PR[2] * PR[6]) / temp;
	PRI[5] = -(PR[0] * PR[5] - PR[2] * PR[3]) / temp;
	PRI[6] = (PR[3] * PR[7] - PR[4] * PR[6]) / temp;
	PRI[7] = -(PR[0] * PR[7] - PR[1] * PR[6]) / temp;
	PRI[8] = (PR[0] * PR[4] - PR[1] * PR[3]) / temp;
}
// 3*3 -> 3*1 rodrigues Transform
rodriguesTransform(inv_rotation_mat, u, v, w);

罗格里斯变换使用到奇异值分解,使用的《Numerical Recipes in C》一书中的代码 svdcmp.c
C/C++实现双目矫正(不使用OpenCV内部函数)及矫正源码解析_第2张图片


void rodriguesTransform(double *inv_rotation_mat, double **u, double **v, double w[])
{	 /*
     * 使用 ** 来声明,然后用dmatrix来申请空间
     * 这样在函数引用矩阵时,就不用声明矩阵大小了
     */
	//double **a;
	int i, j, k;
	double t;
	double t1[MN+1], t2[MN+1];
	/* 矩阵均以M,N中的最大值来申请空间,避免越界 */
	//a = dmatrix(1, MN, 1, MN);  // inv_rotation_mat
	for (i = 1; i <= M; i++) {
		for (j = 1; j <= N; j++) {
			//u[i][j] = a[i][j];
			u[i][j] = inv_rotation_mat[i * 3 + j - 4];
		}
	}
	svdcmp(u, MN, MN, w, v);
	/* Sort the singular values in descending order */
	for (i = 1; i <= N; i++) {
		for (j = i + 1; j <= N; j++) {
			if (w[i] < w[j]) { /* 对特异值排序 */
				t = w[i];
				w[i] = w[j];
				w[j] = t;
				/* 同时也要把矩阵U,V的列位置交换 */
				/* 矩阵U */
				for (k = 1; k <= M; k++) {
					t1[k] = u[k][i];
				}
				for (k = 1; k <= M; k++) {
					u[k][i] = u[k][j];
				}
				for (k = 1; k <= M; k++) {
					u[k][j] = t1[k];
				}

				/* 矩阵V */
				for (k = 1; k <= N; k++) {
					t2[k] = v[k][i];
				}
				for (k = 1; k <= N; k++) {
					v[k][i] = v[k][j];
				}
				for (k = 1; k <= N; k++) {
					v[k][j] = t2[k];
				}
			}
		}
	}
}
	// u * v'
	R[0] = u[1][1] * v[1][1] + u[1][2] * v[1][2] + u[1][3] * v[1][3];
	R[1] = u[1][1] * v[2][1] + u[1][2] * v[2][2] + u[1][3] * v[2][3];
	R[2] = u[1][1] * v[3][1] + u[1][2] * v[3][2] + u[1][3] * v[3][3];
	R[3] = u[2][1] * v[1][1] + u[2][2] * v[1][2] + u[2][3] * v[1][3];
	R[4] = u[2][1] * v[2][1] + u[2][2] * v[2][2] + u[2][3] * v[2][3];
	R[5] = u[2][1] * v[3][1] + u[2][2] * v[3][2] + u[2][3] * v[3][3];
	R[6] = u[3][1] * v[1][1] + u[3][2] * v[1][2] + u[3][3] * v[1][3];
	R[7] = u[3][1] * v[2][1] + u[3][2] * v[2][2] + u[3][3] * v[2][3];
	R[8] = u[3][1] * v[3][1] + u[3][2] * v[3][2] + u[3][3] * v[3][3];

	double r[3];
	r[0] = R[7] - R[5];
	r[1] = R[2] - R[6];
	r[2] = R[3] - R[1];

	float s = sqrt((r[0] * r[0] + r[1] * r[1] + r[2] * r[2]) * 0.25);
	float c = (R[0] + R[4] + R[8] - 1) * 0.5;
	c = c > 1. ? 1. : c < -1. ? -1. : c;  // 1  -1
	float theta = acos(c);
	
	double rr[3]; // 1*3 一行三列
	if (s < 1.0e-5)
	{
		float t;
		if (c > 0)
		{
			double rr[3] = { 0, 0, 0 };
		}
		else
		{
			t = (R[0] + 1) * 0.5;
			rr[0] = sqrt(MAX(t, float(0.)));
			t = (R[4] + 1) * 0.5;
			rr[1] = sqrt(MAX(t, float(0.))) * (R[1] < 0 ? -1. : 1.);
			t = (R[8] + 1) * 0.5;
			rr[2] = sqrt(MAX(t, float(0.))) * (R[2] < 0 ? -1. : 1.);
			if (fabs(rr[0]) < fabs(rr[1]) && fabs(rr[0]) < fabs(rr[2]) && (R[5] > 0) != (rr[1] * rr[2] > 0))
				rr[2] = -rr[2];
			double rrsqrt = sqrt(rr[0] * rr[0] + rr[1] * rr[1] + rr[2] * rr[2]);
			theta /= rrsqrt;
			rr[0] = theta * rr[0];
			rr[1] = theta * rr[1];
			rr[2] = theta * rr[2];			
		}
	}
	else
	{
		float vth = 1 / (2 * s);
		vth *= theta;
		rr[0] = vth * r[0];
		rr[1] = vth * r[1];
		rr[2] = vth * r[2];		
	}
	double eul[3];
	eul[0] = rr[0] * (-0.5);
	eul[1] = rr[1] * (-0.5);
	eul[2] = rr[2] * (-0.5);
std::string GetExePath()
{
	char szFilePath[MAX_PATH + 1] = { 0 };
	GetModuleFileNameA(NULL, szFilePath, MAX_PATH);
	/*
	strrchr:函数功能:查找一个字符c在另一个字符串str中末次出现的位置(也就是从str的右侧开始查找字符c首次出现的位置),
	并返回这个位置的地址。如果未能找到指定字符,那么函数将返回NULL。
	使用这个地址返回从最后一个字符c到str末尾的字符串。
	*/
	(strrchr(szFilePath, '\\'))[0] = 0; // 删除文件名,只获得路径字串//
	std::string path = szFilePath;

	int pos = path.rfind("\\", path.length());//从后往前寻找第一个/出现的位置
	path = path.substr(0, pos);//剪切[0,pos]
	return path;
}
void getYamlParamters(std::string file_extrinsics, std::string file_intrisics)
{
	cv::FileStorage fs_read(file_intrisics, cv::FileStorage::READ);
	if (fs_read.isOpened()) {
		NDouble cameraMatrixL[3 * 3], cameraMatrixR[3 * 3], distCoeffL[5], distCoeffR[5];
		fs_read["cameraMatrixL"] >> cameraMatrixL;
		fs_read["cameraDistcoeffL"] >> distCoeffL;
		fs_read["cameraMatrixR"] >> cameraMatrixR;
		fs_read["cameraDistcoeffR"] >> distCoeffR;
		fs_read.release();
	}
	fs_read.open(file_extrinsics, cv::FileStorage::READ);
	if (fs_read.isOpened()) {
		NDouble Rotation_mat[3 * 3], Translation_vector[3];
		fs_read["R"] >> Rotation_mat;
		fs_read["T"] >> Translation_vector;
		fs_read.release();
	}
}
	FileStorage fs("intrisics.yml", FileStorage::WRITE);
	//time_t rawtime;
	//time(&rawtime);
	/*struct tm tm = *localtime(&(time_t) { time (NULL) });
	char str[26];
	asctime_s(str, sizeof str, &tm);
	fs << "Time" << str;*/

	if (fs.isOpened())
	{
		fs << "cameraMatrixL" << cameraMatrix << "cameraDistcoeffL" << distCoeffs << "cameraMatrixR" << cameraMatrixR << "cameraDistcoeffR" << distCoeffsR;
		fs.release();
		cout << "cameraMatrixL=:" << cameraMatrix << endl << "cameraDistcoeffL=:" << distCoeffs << endl << "cameraMatrixR=:" << cameraMatrixR << endl << "cameraDistcoeffR=:" << distCoeffsR << endl;
	}
	else
	{
		cout << "Error: can not save the intrinsics!!!!" << endl;
	}

	fs.open("extrinsics.yml", FileStorage::WRITE);
	if (fs.isOpened())
	{
		fs << "R" << R << "T" << T << "E" << E << "F" << F << "Rl" << Rl << "Rr" << Rr << "Pl" << Pl << "Pr" << Pr << "Q" << Q;
		cout << "R=" << R << endl << "T=" << T << endl << "E=" << E << endl << "F=" << F << endl << "Rl=" << Rl << endl << "Rr" << Rr << endl << "Pl" << Pl << endl << "Pr" << Pr << endl << "Q" << Q << endl;
		fs.release();
	}
	else
	{
		cout << "Error: can not save the extrinsic parameters\n";
	}
#include 
#include 
#include 
#include 
#include 

#include 
#include 
#include 
#include 

using namespace cv;
using namespace std;
//#define calibration

int getImagePathList(std::string folder, std::vector<cv::String> &imagePathList);
void calRealPoint(vector<vector<Point3f>>& obj, int boardWidth, int boardHeight, int imgNumber, int squareSize);
void outputCameraParam(Mat cameraMatrix, Mat distCoeffs, Mat cameraMatrixR, Mat distCoeffsR, Mat R, Mat T, Mat E, Mat F, Mat Rl, Mat Rr, Mat Pl, Mat Pr, Mat Q);

int getImagePathList(std::string folder, std::vector<cv::String> &imagePathList)
{
	//search all the image in a folder
	cv::glob(folder, imagePathList);
	return 0;
}

int main()
{
	//ifstream fin("left_img.txt");             /* 标定所用图像文件的路径 */
	//ofstream fout("caliberation_result_left640.txt");  /* 保存标定结果的文件 */

	// 读取每一幅图像,从中提取出角点,然后对角点进行亚像素精确化
	int image_count = 0;  /* 图像数量 */
	Size image_size;      /* 图像的尺寸 */
	Size board_size = Size(8, 11);             /* 标定板上每行、列的角点数 */
	vector<Point2f> image_points_buf;         /* 缓存每幅图像上检测到的角点 */
	vector<vector<Point2f>> image_points_seq; /* 保存检测到的所有角点 */
	string filenameL;      // 图片名
	vector<string> filenamesL;

	cout << "请输入左棋盘格目录路径: " << endl;
	string checkboard_path;
	cin >> checkboard_path;
	cout << "Checkboard left path: " << checkboard_path << endl << endl;

	cout << "接着输入右棋盘格目录路径: " << endl;
	string checkboard_pathR;
	cin >> checkboard_pathR;
	cout << "Checkboard right path: " << checkboard_pathR << endl << endl;

	String folder = "F:/code/imgs/left_biao1/"; // checkboard_path // "F:/kinect/multi_thread/chess/chess/left/";//"F:/kinect/multi_thread/chess/RGB/left640/";  "F:/kinect/multi_thread/chess/RGB/left1/";// "F:/code/imgs/left/0.png"
	std::vector<String> imagePathList;
	getImagePathList(folder, imagePathList);
	// right folder
	String folderight = "F:/code/imgs/right_biao1/"; // checkboard_pathR // "F:/kinect/multi_thread/chess/chess/right/";// "F:/kinect/multi_thread/chess/RGB/right640/";   "F:/kinect/multi_thread/chess/RGB/right1/";// "F:/code/imgs/right/0.png"
	std::vector<String> imagePathListright;
	getImagePathList(folderight, imagePathListright);

	//while (getline(fin, filename))
	for (int i = 0; i < imagePathList.size(); i++)
	{
		++image_count;
		filenameL = imagePathList[i];
		Mat imageInput = imread(filenameL);

		std::cout << filenameL << endl;
		

		// 读入第一张图片时获取图片大小
		if (image_count == 1)
		{
			image_size.width = imageInput.cols;
			image_size.height = imageInput.rows;
		}

		/* 提取角点 */
		if (0 == findChessboardCorners(imageInput, board_size, image_points_buf))
		{
			//cout << "can not find chessboard corners!\n";  // 找不到角点
			cout << "**" << filenameL << "** can not find chessboard corners!\n";
			//exit(1);
			image_count--;
		}
		else
		{
			filenamesL.push_back(filenameL);
			Mat view_gray;
			cvtColor(imageInput, view_gray, COLOR_RGB2GRAY);  // 转灰度图IMREAD_GRAYSCALE

			/* 亚像素精确化 */
			// image_points_buf 初始的角点坐标向量,同时作为亚像素坐标位置的输出
			// Size(5,5) 搜索窗口大小
			// (-1,-1)表示没有死区
			// TermCriteria 角点的迭代过程的终止条件, 可以为迭代次数和角点精度两者的组合
			cornerSubPix(view_gray, image_points_buf, Size(5, 5), Size(-1, -1), TermCriteria(TermCriteria::EPS + TermCriteria::COUNT, 30, 0.1)); // CV_TERMCRIT_ITER、CV_TERMCRIT_EPS

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

			/* 在图像上显示角点位置 */
			drawChessboardCorners(view_gray, board_size, image_points_buf, false); // 用于在图片中标记角点

			imshow("Camera Calibration", view_gray);       // 显示图片

			waitKey(10); //暂停0.5S      
		}
	}
	int CornerNum = board_size.width * board_size.height;  // 每张图片上总的角点数
	cout << board_size.width << board_size.height << image_size.width << endl;
	//-------------以下是摄像机标定------------------

	/*棋盘三维信息*/
	Size square_size = Size(25, 25); // Size(60, 60);         /* 实际测量得到的标定板上每个棋盘格的大小 */
	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);
	}
	cout << "\n begin calibration \n" << endl;
	/* 开始标定 */
	// object_points 世界坐标系中的角点的三维坐标
	// image_points_seq 每一个内角点对应的图像坐标点
	// image_size 图像的像素尺寸大小
	// cameraMatrix 输出,内参矩阵
	// distCoeffs 输出,畸变系数
	// rvecsMat 输出,旋转向量
	// tvecsMat 输出,位移向量
	// 0 标定时所采用的算法
	calibrateCamera(object_points, image_points_seq, image_size, cameraMatrix, distCoeffs, rvecsMat, tvecsMat, 0);

	//------------------------标定完成------------------------------------

	// -------------------对标定结果进行评价------------------------------

	double total_err = 0.0;         /* 所有图像的平均误差的总和 */
	double err = 0.0;               /* 每幅图像的平均误差 */
	vector<Point2f> image_points2;  /* 保存重新计算得到的投影点 */
	cout << "每幅图像的标定误差:\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;
	}
	cout << "总体平均误差:" << total_err / image_count << "像素" << endl << endl;

	//-------------------------评价完成---------------------------------------------

	//-----------------------保存定标结果------------------------------------------- 
	Mat rotation_matrix = Mat(3, 3, CV_32FC1, Scalar::all(0));  /* 保存每幅图像的旋转矩阵 */
	cout << "相机内参数矩阵:" << endl;
	cout << cameraMatrix << endl << endl;
	cout << "畸变系数:\n";
	cout << distCoeffs << endl << endl << endl;
	//for (int i = 0; i < image_count; i++)
	//{
	//	cout << "第" << i + 1 << "幅图像的旋转向量:" << endl;
	//	cout << tvecsMat[i] << endl;

	//	/* 将旋转向量转换为相对应的旋转矩阵 */
	//	Rodrigues(tvecsMat[i], rotation_matrix);
	//	cout << "第" << i + 1 << "幅图像的旋转矩阵:" << endl;
	//	cout << rotation_matrix << endl;
	//	cout << "第" << i + 1 << "幅图像的平移向量:" << endl;
	//	cout << rvecsMat[i] << endl << endl;
	//}
	cout << "\n 左图标定完毕!\n" << endl;

	// 右图
	image_count = 0;
	string filenameR;      // 图片名
	vector<string> filenamesR;
	vector<vector<Point2f>> image_points_seqR; /* 保存检测到的所有角点 */
	for (int i = 0; i < imagePathListright.size(); i++)
	{
		++image_count;
		filenameR = imagePathListright[i];
		Mat imageInputR = imread(filenameR);

		std::cout << filenameR << endl;
		//

		// 读入第一张图片时获取图片大小
		if (image_count == 1)
		{
			image_size.width = imageInputR.cols;
			image_size.height = imageInputR.rows;
		}

		/* 提取角点 */
		if (0 == findChessboardCorners(imageInputR, board_size, image_points_buf))
		{
			//cout << "can not find chessboard corners!\n";  // 找不到角点
			cout << "**" << filenameR << "** can not find chessboard corners!\n";
			//exit(1);
			image_count--;
		}
		else
		{
			filenamesR.push_back(filenameR);
			Mat view_gray;
			cvtColor(imageInputR, view_gray, COLOR_RGB2GRAY);  // 转灰度图IMREAD_GRAYSCALE

			/* 亚像素精确化 */
			// image_points_buf 初始的角点坐标向量,同时作为亚像素坐标位置的输出
			// Size(5,5) 搜索窗口大小
			// (-1,-1)表示没有死区
			// TermCriteria 角点的迭代过程的终止条件, 可以为迭代次数和角点精度两者的组合
			cornerSubPix(view_gray, image_points_buf, Size(5, 5), Size(-1, -1), TermCriteria(TermCriteria::EPS + TermCriteria::COUNT, 30, 0.1)); // CV_TERMCRIT_ITER、CV_TERMCRIT_EPS

			image_points_seqR.push_back(image_points_buf);  // 保存亚像素角点

			/* 在图像上显示角点位置 */
			drawChessboardCorners(view_gray, board_size, image_points_buf, false); // 用于在图片中标记角点

			imshow("Camera Calibration", view_gray);       // 显示图片

			waitKey(10); //暂停0.5S      
		}
	}
	CornerNum = board_size.width * board_size.height;  // 每张图片上总的角点数
	cout << board_size.width << board_size.height << image_size.width << endl;
	//-------------以下是摄像机标定------------------

	/*棋盘三维信息*/
	//Size square_size = Size(25, 25); // Size(60, 60);         /* 实际测量得到的标定板上每个棋盘格的大小 */
	vector<vector<Point3f>> object_pointsR;   /* 保存标定板上角点的三维坐标 */

	/*内外参数*/
	Mat cameraMatrixR = Mat(3, 3, CV_32FC1, Scalar::all(0));  /* 摄像机内参数矩阵 */
	vector<int> point_countsR;   // 每幅图像中角点的数量
	Mat distCoeffsR = Mat(1, 5, CV_32FC1, Scalar::all(0));       /* 摄像机的5个畸变系数:k1,k2,p1,p2,k3 */
	vector<Mat> tvecsMatR;      /* 每幅图像的旋转向量 */
	vector<Mat> rvecsMatR;      /* 每幅图像的平移向量 */

	/* 初始化标定板上角点的三维坐标 */
	//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_pointsR.push_back(tempPointSet);
	}

	/* 初始化每幅图像中的角点数量,假定每幅图像中都可以看到完整的标定板 */
	for (i = 0; i < image_count; i++)
	{
		point_countsR.push_back(board_size.width * board_size.height);
	}
	cout << "\n begin calibration \n" << endl;
	/* 开始标定 */
	// object_pointsR 世界坐标系中的角点的三维坐标
	// image_points_seqR 每一个内角点对应的图像坐标点
	// image_size 图像的像素尺寸大小
	// cameraMatrix 输出,内参矩阵
	// distCoeffs 输出,畸变系数
	// rvecsMatR 输出,旋转向量
	// tvecsMatR 输出,位移向量
	// 0 标定时所采用的算法
	calibrateCamera(object_pointsR, image_points_seqR, image_size, cameraMatrixR, distCoeffsR, rvecsMatR, tvecsMatR, 0);

	//------------------------标定完成------------------------------------

	// -------------------对标定结果进行评价------------------------------

	total_err = 0.0;         /* 所有图像的平均误差的总和 */
	err = 0.0;               /* 每幅图像的平均误差 */
	vector<Point2f> image_points2R;  /* 保存重新计算得到的投影点 */
	cout << "每幅图像的标定误差:\n";

	for (i = 0; i < image_count; i++)
	{
		vector<Point3f> tempPointSet = object_pointsR[i];

		/* 通过得到的摄像机内外参数,对空间的三维点进行重新投影计算,得到新的投影点 */
		projectPoints(tempPointSet, rvecsMatR[i], tvecsMatR[i], cameraMatrixR, distCoeffsR, image_points2R);

		/* 计算新的投影点和旧的投影点之间的误差*/
		vector<Point2f> tempImagePoint = image_points_seqR[i];
		Mat tempImagePointMat = Mat(1, tempImagePoint.size(), CV_32FC2);
		Mat image_points2Mat = Mat(1, image_points2R.size(), CV_32FC2);

		for (int j = 0; j < tempImagePoint.size(); j++)
		{
			image_points2Mat.at<Vec2f>(0, j) = Vec2f(image_points2R[j].x, image_points2R[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_countsR[i];
		cout << "第" << i + 1 << "幅图像的平均误差:" << err << "像素" << endl;
	}
	cout << "总体平均误差:" << total_err / image_count << "像素" << endl << endl;

	//-------------------------评价完成---------------------------------------------

	//-----------------------保存定标结果------------------------------------------- 
	Mat rotation_matrixR = Mat(3, 3, CV_32FC1, Scalar::all(0));  /* 保存每幅图像的旋转矩阵 */
	cout << "相机内参数矩阵:" << endl;
	cout << cameraMatrixR << endl << endl;
	cout << "畸变系数:\n";
	cout << distCoeffsR << endl << endl << endl;
	//for (int i = 0; i < image_count; i++)
	//{
	//	cout << "第" << i + 1 << "幅图像的旋转向量:" << endl;
	//	cout << tvecsMatR[i] << endl;

	//	/* 将旋转向量转换为相对应的旋转矩阵 */
	//	Rodrigues(tvecsMatR[i], rotation_matrixR);
	//	cout << "第" << i + 1 << "幅图像的旋转矩阵:" << endl;
	//	cout << rotation_matrixR << endl;
	//	cout << "第" << i + 1 << "幅图像的平移向量:" << endl;
	//	cout << rvecsMatR[i] << endl << endl;
	//}
	cout << "\n 右图标定完毕!\n" << endl;

	//横向的角点数目
	const int boardWidth = 8;
	//纵向的角点数目
	const int boardHeight = 11;
	//总的角点数目
	const int boardCorner = boardWidth * boardHeight;
	//相机标定时需要采用的图像帧数
	const int frameNumber = image_count;
	//标定板黑白格子的大小 单位是mm
	const int squareSize = 25;
	//标定板的总内角点
	const Size boardSize = Size(boardWidth, boardHeight);
	Mat R, T, E, F;
	//R旋转矢量 T平移矢量 E本征矩阵 F基础矩阵
	vector<Mat> rvecs; //R
	vector<Mat> tvecs; //T
	//左边摄像机所有照片角点的坐标集合
	vector<vector<Point2f>> imagePointL;
	//右边摄像机所有照片角点的坐标集合
	vector<vector<Point2f>> imagePointR;
	//各图像的角点的实际的物理坐标集合
	vector<vector<Point3f>> objRealPoint;
	//左边摄像机某一照片角点坐标集合
	vector<Point2f> cornerL;
	//右边摄像机某一照片角点坐标集合
	vector<Point2f> cornerR;

	Mat rgbImageL, grayImageL;
	Mat rgbImageR, grayImageR;

	Mat intrinsic;
	Mat distortion_coeff;
	//校正旋转矩阵R,投影矩阵P,重投影矩阵Q
	Mat Rl, Rr, Pl, Pr, Q;
	//映射表
	Mat mapLx, mapLy, mapRx, mapRy;
	Rect validROIL, validROIR;

	int goodFrameCount = 0;

	for (i = 0; i < frameNumber; i++) {
		rgbImageL = imread(filenamesL[i], 1);
		cvtColor(rgbImageL, grayImageL, COLOR_RGB2GRAY);

		rgbImageR = imread(filenamesR[i], 1);
		cvtColor(rgbImageR, grayImageR, COLOR_RGB2GRAY);

		bool isFindL, isFindR;
		isFindL = findChessboardCorners(rgbImageL, boardSize, cornerL);
		isFindR = findChessboardCorners(rgbImageR, boardSize, cornerR);
		if (isFindL == true && isFindR == true)
		{
			cornerSubPix(grayImageL, cornerL, Size(5, 5), Size(-1, 1), TermCriteria(TermCriteria::EPS | TermCriteria::COUNT, 20, 0.1));
			drawChessboardCorners(rgbImageL, boardSize, cornerL, isFindL);
			imshow("chessboardL", rgbImageL);
			imagePointL.push_back(cornerL);

			cornerSubPix(grayImageR, cornerR, Size(5, 5), Size(-1, -1), TermCriteria(TermCriteria::EPS | TermCriteria::COUNT, 20, 0.1));
			drawChessboardCorners(rgbImageR, boardSize, cornerR, isFindR);
			imshow("chessboardR", rgbImageR);
			imagePointR.push_back(cornerR);

			goodFrameCount++;
			cout << "the image" << goodFrameCount << " is good" << endl;
		}
		else
		{
			cout << "the image is bad please try again" << endl;
		}
		if (waitKey(10) == 'q')
		{
			break;
		}
	}
	//计算实际的校正点的三维坐标,根据实际标定格子的大小来设置
	calRealPoint(objRealPoint, boardWidth, boardHeight, frameNumber, squareSize);
	cout << "cal real successful" << endl;

	//标定摄像头
	double rms = stereoCalibrate(objRealPoint, imagePointL, imagePointR,
		cameraMatrix, distCoeffs,
		cameraMatrixR, distCoeffsR,
		image_size, R, T, E, F, CALIB_USE_INTRINSIC_GUESS,
		TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 100, 1e-5));

	cout << "Stereo Calibration done with RMS error = " << rms << endl;
	stereoRectify(cameraMatrix, distCoeffs, cameraMatrixR, distCoeffsR, image_size, R, T, Rl,
		Rr, Pl, Pr, Q, CALIB_ZERO_DISPARITY, -1, image_size, &validROIL, &validROIR);

	//摄像机校正映射
	initUndistortRectifyMap(cameraMatrix, distCoeffs, Rl, Pl, image_size, CV_32FC1, mapLx, mapLy);
	initUndistortRectifyMap(cameraMatrixR, distCoeffsR, Rr, Pr, image_size, CV_32FC1, mapRx, mapRy);

	Mat rectifyImageL, rectifyImageR;
	cvtColor(grayImageL, rectifyImageL, COLOR_GRAY2BGR); //CV_GRAY2BGR
	cvtColor(grayImageR, rectifyImageR, COLOR_GRAY2BGR);

	imshow("Recitify Before", rectifyImageL);
	cout << "按Q1退出..." << endl;
	//经过remap之后,左右相机的图像已经共面并且行对准了
	Mat rectifyImageL2, rectifyImageR2;
	remap(rectifyImageL, rectifyImageL2, mapLx, mapLy, INTER_LINEAR);
	remap(rectifyImageR, rectifyImageR2, mapRx, mapRy, INTER_LINEAR);
	cout << "按Q2退出..." << endl;

	imshow("rectifyImageL", rectifyImageL2);
	imshow("rectifyImageR", rectifyImageR2);

	outputCameraParam(cameraMatrix, distCoeffs, cameraMatrixR, distCoeffsR, R, T, E, F, Rl, Rr, Pl, Pr, Q);

	//显示校正结果
	Mat canvas;
	double sf;
	int w, h;
	sf = 600. / MAX(image_size.width, image_size.height);
	w = cvRound(image_size.width * sf);
	h = cvRound(image_size.height * sf);
	canvas.create(h, w * 2, CV_8UC3);

	//左图像画到画布上
	Mat canvasPart = canvas(Rect(0, 0, w, h));
	resize(rectifyImageL2, canvasPart, canvasPart.size(), 0, 0, INTER_AREA);
	Rect vroiL(cvRound(validROIL.x*sf), cvRound(validROIL.y*sf),
		cvRound(validROIL.width*sf), cvRound(validROIL.height*sf));
	rectangle(canvasPart, vroiL, Scalar(0, 0, 255), 3, 8);

	cout << "Painted ImageL" << endl;

	//右图像画到画布上
	canvasPart = canvas(Rect(w, 0, w, h));
	resize(rectifyImageR2, canvasPart, canvasPart.size(), 0, 0, INTER_LINEAR);
	Rect vroiR(cvRound(validROIR.x*sf), cvRound(validROIR.y*sf),
		cvRound(validROIR.width*sf), cvRound(validROIR.height*sf));
	rectangle(canvasPart, vroiR, Scalar(0, 255, 0), 3, 8);

	cout << "Painted ImageR" << endl;

	//画上对应的线条
	for (int i = 0; i < canvas.rows; i += 16)
		line(canvas, Point(0, i), Point(canvas.cols, i), Scalar(0, 255, 0), 1, 8);

	imshow("rectified", canvas);

	cout << "wait key" << endl;
	waitKey(0);
	return 0;
}

/*计算标定板上模块的实际物理坐标*/
void calRealPoint(vector<vector<Point3f>>& obj, int boardWidth, int boardHeight, int imgNumber, int squareSize)
{
	vector<Point3f> imgpoint;
	for (int rowIndex = 0; rowIndex < boardHeight; rowIndex++)
	{
		for (int colIndex = 0; colIndex < boardWidth; colIndex++)
		{
			imgpoint.push_back(Point3f(rowIndex * squareSize, colIndex * squareSize, 0));
		}
	}
	for (int imgIndex = 0; imgIndex < imgNumber; imgIndex++)
	{
		obj.push_back(imgpoint);
	}
}

你可能感兴趣的:(双目相机,计算机视觉,相机标定,双目深度估计,视觉检测,visual,studio)