可用的双目标定代码（先单目标定再双目标定）

最近做双目项目需要进行标定，网上查看一些资料。目前所用是先进行单目标定，然后在进行双目标定。代码如下，配以后使用

//biaoding.cpp
#include<opencv2/calib3d/calib3d.hpp>
#include<opencv2/core/core.hpp>
#include<opencv2/highgui/highgui.hpp>
#include<opencv2/imgproc/imgproc.hpp>
#include<opencv2/opencv.hpp>
#include <opencv2/imgproc/types_c.h>
#include <opencv2/calib3d/calib3d_c.h>
#include<iostream>
#include <dirent.h>

using namespace std;
using namespace cv;

int n = 1;
int m = 1;

int i = 1;
int j = 1;
//需要按实际情况修改=======
const int boardwidth = 8;  //棋盘格内角个数
const int boardhight = 6; //棋盘格内角个数
const int squaresize = 27.5; //棋盘格变长，mm
cv::Size imagesize ;
const cv::Size boardsize = cv::Size(boardwidth,boardhight);
//======================
vector<cv::Point3f> objectpoint;
vector<vector<cv::Point3f>> objpoint;

vector<cv::Point2f> cornerL;
vector<cv::Point2f> cornerR;

vector<vector<cv::Point2f>> imagepointL;
vector<vector<cv::Point2f>> imagepointR;


Mat cameraMatrixL = Mat(3, 3, CV_32FC1, Scalar::all(0)); 
Mat cameraMatrixR = Mat(3, 3, CV_32FC1, Scalar::all(0)); 
vector<Mat> rvecsMat_left;                                          // 存放所有图像的旋转向量，每一副图像的旋转向量为一个mat
vector<Mat> tvecsMat_left;                                          // 存放所有图像的平移向量，每一副图像的平移向量为一个mat
Mat distcoeffL = Mat(1, 5, CV_32FC1, Scalar::all(0)); 
Mat distcoeffR = Mat(1, 5, CV_32FC1, Scalar::all(0)); 
vector<Mat> rvecsMat_right;                                          // 存放所有图像的旋转向量，每一副图像的旋转向量为一个mat
	vector<Mat> tvecsMat_right;                                          // 存放所有图像的平移向量，每一副图像的平移向量为一个mat
cv::Mat R,T,E,F;
cv::Mat R1, R2, P1, P2 , Q;

cv::Mat maplx, maply, maprx, mapry;

cv::Mat imageL, grayimageL;
cv::Mat imageR, grayimageR;

cv::Rect validROIL, validROIR;

//单目标定
void m_calibration(vector<vector<Point2f>>& image_points_seq, Size board_size, Size square_size, Mat& cameraMatrix, Mat& distCoeffs, vector<Mat>& rvecsMat, vector<Mat>& tvecsMat,int image_count)
{
	/*棋盘三维信息*/
	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, imagesize, cameraMatrix, distCoeffs, rvecsMat, tvecsMat, CV_CALIB_FIX_K3);
	cout << "重投影误差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;
	}
	cout << "重投影误差2：" << sqrt(totalErr / totalPoints) << "像素" << endl << endl;
	cout << "重投影误差3：" << total_err / image_count << "像素" << endl << endl;


	//保存定标结果  	
	cout << "开始保存定标结果………………" << 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++)
	{
		Rodrigues(rvecsMat[i], rotation_matrix);
	}
	cout << "定标结果完成保存！！！" << endl;
}
void worldpoint(int framenumber)
{
	
	for (int i = 0; i < boardhight; i++)
	{
		for (int j = 0; j < boardwidth; j++)
		{
			objectpoint.push_back(cv::Point3f(i*squaresize,j*squaresize,0.0f));
		}
	}
	for (int w = 1; w <= framenumber;w++)
	{
		objpoint.push_back(objectpoint);
	}
}

void outputparam()
{
	cv::FileStorage fs("intrinsics.yml", cv::FileStorage::WRITE);
	fs << "cameraMatrixL" << cameraMatrixL << "cameraDistcoeffL" << distcoeffL << "cameraMatrixR" << cameraMatrixR << "cameraDistcoeffR" << distcoeffR;
	fs.release();
	std::cout << "cameraMatrixL=:" << cameraMatrixL << endl << "cameraDistcoeffL=:" << distcoeffL << endl << "cameraMatrixR=:" << cameraMatrixR << endl << "cameraDistcoeffR=:" << distcoeffR << endl;

	fs.open("extrinsics.yml", cv::FileStorage::WRITE);
	fs << "R" << R << "T" << T << "Rl" << R1 << "Rr" << R2 << "Pl" << P1 << "Pr" << P2 << "Q" << Q;
	std::cout << "R=" << R << endl << "T=" << T << endl << "Rl=" << R1 << endl << "Rr=" << R2 << endl << "Pl=" << P1 << endl << "Pr=" << P2 << endl << "Q=" << Q << endl;
	fs.release();

}

int getFilesName(string path)
{
    DIR *pDir;
	std::vector<std::string> filenames;
    struct dirent* ptr;
    if(!(pDir = opendir(path.c_str())))
        return -1;
    while((ptr = readdir(pDir))!=0) {
        if (strcmp(ptr->d_name, ".") != 0 && strcmp(ptr->d_name, "..") != 0)
            filenames.push_back(path + "/" + ptr->d_name);
    }
    closedir(pDir);
	return filenames.size();
}
int main()
{
	//读取图片
	char* leftStr="/home/×××/×××/biaoding/left/";
	char* rightStr="/home/×××/×××/biaoding/right/";

	int framenumberleft = getFilesName(leftStr);
	int framenumberright = getFilesName(leftStr);
	assert(framenumberleft==framenumberright);
	int framenumber = framenumberleft;
	int current = 1;
	std::cout<<framenumber<<endl;
	while (current <= framenumber)
	{
		char frameL[50];
		
		snprintf(frameL, sizeof(frameL),"%s%d.jpg", leftStr,n++);
		imageL = cv::imread(frameL);
			
		cv::cvtColor(imageL,grayimageL,cv::ColorConversionCodes::COLOR_BGR2GRAY);

		char frameR[50];
		
		snprintf(frameR, sizeof(frameR), "%s%d.jpg",rightStr, m++);
		imageR = cv::imread(frameR);
		if (current == 1)  //读入第一张图片时获取图像宽高信息
		{
			imagesize = cv::Size(imageL.cols,imageL.rows);
			cout << "image_size.width = " << imagesize<< endl;
		}

		cv::cvtColor(imageR, grayimageR, cv::ColorConversionCodes::COLOR_BGR2GRAY);

		bool foundL, foundR;


		foundL = cv::findChessboardCorners(imageL,boardsize,cornerL);
		foundR = cv::findChessboardCorners(imageR, boardsize, cornerR);

		if (foundL == true && foundR == true)
		{
		cv::cornerSubPix(grayimageL,cornerL,cv::Size(11,11),cv::Size(-1,-1),cv::TermCriteria(cv::TermCriteria::MAX_ITER|cv::TermCriteria::EPS, 30, 1e-6));
		cv::cornerSubPix(grayimageR, cornerR, cv::Size(11, 11), cv::Size(-1, -1), cv::TermCriteria(cv::TermCriteria::MAX_ITER | cv::TermCriteria::EPS, 30, 1e-6));

		cv::drawChessboardCorners(imageL,boardsize,cornerL,foundL);
		cv::imshow("L", imageL);
		cv::waitKey(10);
		cv::drawChessboardCorners(imageR, boardsize, cornerR, foundR);
		cv::imshow("R", imageR);
		cv::waitKey(10);

		imagepointL.push_back(cornerL);
		imagepointR.push_back(cornerR);

		cout << "The image  " << current << "  is good" << endl;
		}
		else
		{
			std::cout << "The image is bad please try again" << endl;
		}
		current++;
	}
	cout << "左右单目开始标定" << endl;
	//进行left标定
	std::cout<<"current:"<<current<<endl;
	//左图
	std::cout<<"============================ left start==========================="<<std::endl;
	m_calibration(imagepointL, boardsize, Size(squaresize,squaresize), cameraMatrixL, distcoeffL, rvecsMat_left, tvecsMat_left,current-1);
	//右图
	std::cout<<"============================ right start==========================="<<std::endl;
	m_calibration(imagepointR, boardsize, Size(squaresize,squaresize), cameraMatrixR, distcoeffR, rvecsMat_right, tvecsMat_right,current-1);

	std::cout<<"============================ left re==========================="<<std::endl;
	cout << "相机内参数矩阵：" << endl;
	cout << cameraMatrixL << endl << endl;
	cout << "畸变系数：\n";
	cout << distcoeffL << endl << endl << endl;

	std::cout<<"============================ right re==========================="<<std::endl;
	cout << "相机内参数矩阵：" << endl;
	cout << cameraMatrixR << endl << endl;
	cout << "畸变系数：\n";
	cout << distcoeffR << endl << endl << endl;

	worldpoint(framenumber);

	cout << "双目开始标定" << endl;

	double err = cv::stereoCalibrate(objpoint, imagepointL, imagepointR, cameraMatrixL, distcoeffL, cameraMatrixR, distcoeffR, imagesize, R, T, E, F, 	
		CALIB_USE_INTRINSIC_GUESS,
 cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 30, 1e-6));

	
	cout << "The err = " << err << endl;

	cv::stereoRectify(cameraMatrixL,distcoeffL, cameraMatrixR,distcoeffR,imagesize,R,T,R1,R2,P1,P2,Q, cv::CALIB_ZERO_DISPARITY, -1, imagesize, &validROIL, &validROIR);

	cv::initUndistortRectifyMap(cameraMatrixL,distcoeffL,R1,P1,imagesize, CV_32FC1,maplx,maply);
	cv::initUndistortRectifyMap(cameraMatrixR,distcoeffR,R2,P2,imagesize,CV_32FC1,maprx,mapry);

	outputparam();

	cv::Mat canvas;
	double sf;
	int w, h;
	sf = 600. / MAX(imagesize.width, imagesize.height);
	w = cvRound(imagesize.width * sf);
	h = cvRound(imagesize.height * sf);
	canvas.create(h, w * 2, CV_8UC3);


	int currents = 1;
	while (currents <= framenumber)
	{

		char frameL[50];

		snprintf(frameL, sizeof(frameL), "%s%d.jpg",leftStr, i++);
		imageL = cv::imread(frameL);
		

		char frameR[50];

		snprintf(frameR, sizeof(frameR), "%s%d.jpg",rightStr, j++);
		imageR = cv::imread(frameR);
		
		cv::Mat rectifyImageL2, rectifyImageR2;
		cv::remap(imageL, rectifyImageL2, maplx, maply, cv::InterpolationFlags::INTER_LINEAR);
		cv::remap(imageR, rectifyImageR2, maprx, mapry, cv::InterpolationFlags::INTER_LINEAR);

		cv::Mat canvasPart = canvas(cv::Rect(w * 0, 0, w, h));
		resize(rectifyImageL2, canvasPart, canvasPart.size(), 0, 0, cv::INTER_AREA);
		cv::Rect vroiL(cvRound(validROIL.x*sf), cvRound(validROIL.y*sf),
			cvRound(validROIL.width*sf), cvRound(validROIL.height*sf));
		cv::rectangle(canvasPart, vroiL, cv::Scalar(0, 0, 255), 3, 8);

		canvasPart = canvas(cv::Rect(w, 0, w, h));
		resize(rectifyImageR2, canvasPart, canvasPart.size(), 0, 0, cv::INTER_LINEAR);
		cv::Rect vroiR(cvRound(validROIR.x * sf), cvRound(validROIR.y*sf),
			cvRound(validROIR.width * sf), cvRound(validROIR.height * sf));
		cv::rectangle(canvasPart, vroiR, cv::Scalar(0, 255, 0), 3, 8);

		for (int i = 0; i < canvas.rows; i += 16)
			line(canvas, cv::Point(0, i), cv::Point(canvas.cols, i), cv::Scalar(0, 255, 0), 1, 8);

		cv::imshow("rectified", canvas);

		if (cv::waitKey() > 0)
		{
			currents++;
		}

	}
	return 0;
}

//编译
g++ biaoding.cpp -o biao  `pkg-config opencv4 --cflags --libs`

参考