棋盘格生成 + 相机标定 + 立体匹配

1. 棋盘格生成

#include 
#include 
#include 
using namespace std;
using namespace cv;

const int perBoardPixel = 1000;  //定义每个格子的大小
const Size boardSize(9, 7);  //定义有几个格子

void createBoard() {

	//计算棋盘格的大小
	int height = perBoardPixel * boardSize.height;
	int width = perBoardPixel * boardSize.width;
	
	//定义一个Mat来存储棋盘格
	Mat board(height, width, CV_8UC1);

	//初始化棋盘格
	for (int i = 0; i < height; i++) {
		for (int j = 0; j < width; j++) {
			board.at<uchar>(i, j) = 0;
		}
	}

	//奇数格子的为白色，偶数格子的为黑色
	for (int i = 0; i < boardSize.height; i++) {
		for (int j = 0; j < boardSize.width; j++) {
			if ((i + j) % 2 == 1) {
				for (int r = i * perBoardPixel; r < (i + 1) * perBoardPixel; r++) {
					for (int c = j * perBoardPixel; c < (j + 1) * perBoardPixel; c++) {
						board.at<uchar>(r, c) = 255;
					}
				}
			}
		}
	}

	imshow("chessboard", board);
	waitKey(0);


	//存储棋盘格图片
	imwrite("D:\\学习资料\\C++\\C++ code\\相机标定棋盘格图片生成\\相机标定棋盘格图片生成chessboard.png", board);
	
}

int main() {
	//生成棋盘格
	createBoard();

	system("pause");

	return 0;
}

2. 捕获图片

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include     
#include  

using namespace std;
using namespace cv;

const int cap_width = 2560;
const int cap_height = 720;
const int img_width = 1280;
const int img_height = 720;

int camera_cap();

int main()            //程序主函数
{

	camera_cap();
	return 0;

}
int camera_cap()
{
	VideoCapture cap;
	cap.open(0);
	if (!cap.isOpened())                         //判断是否成功打开相机
	{
		cout << "请连接相机" << endl;
	}

	cap.set(CV_CAP_PROP_FRAME_WIDTH, cap_width);  //设置捕获视频的宽度
	cap.set(CV_CAP_PROP_FRAME_HEIGHT, cap_height);  //设置捕获视频的高度

	//设置图片的保存路径
	string pathNameLeft = "D:\\学习资料\\C++\\C++ code\\深度获取算法\\深度获取算法\\左原图";
	string pathNameRight = "D:\\学习资料\\C++\\C++ code\\深度获取算法\\深度获取算法\\右原图";
	string formatName = ".jpg";
	string num;
	string fileNameL;  //左相机图像的保存路径
	string fileNameR;  //右相机图像的保存路径

	long long i = 0;

	while (1)
	{
		Mat frame, frame_L, frame_R;
		cap >> frame;                            //从相机捕获一帧图像
		//cout << frame.rows << endl;
		//cout << frame.cols << endl;
		cvtColor(frame, frame, COLOR_BGR2GRAY);
		frame_L = frame(Rect(0, 0, img_width, img_height));  //获取裁剪后左Camera的图像
		frame_R = frame(Rect(img_width, 0, img_width, img_height)); //获取裁剪后右Camera的图像
		/*cout << frame_L.rows << endl;
		cout << frame_L.cols << endl;
		cout << "------" << endl;
		cout << frame_R.rows << endl;
		cout << frame_R.cols << endl;*/

		namedWindow("Video_L");
		namedWindow("Video_R");
		imshow("Video_L", frame_L); 
		imshow("Video_R", frame_R);
		char c = waitKey(1);
		if (c == 32)  //按空格采集图像
		{
			num = to_string(i);
			fileNameL = pathNameLeft + num + formatName;
			fileNameR = pathNameRight + num + formatName;
			imwrite(fileNameL, frame_L);
			imwrite(fileNameR, frame_L);
			i++;
			cout << "成功捕获图片！" << endl;
		}
		else if (c == 27)  //按ESC退出程序
			break;
		frame_L.release();
		frame_R.release();
		frame.release();
	}
	cap.release();                               //释放对相机的控制
	return 0;
}

3. 单目相机标定

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

using namespace std;
using namespace cv;

//相机标定函数
void cameraCalibrator()
{
	//指定输入图片路径和输出结果路径
	string basedir = "D:\\学习资料\\C++\\C++ code\\相机标定棋盘格图片生成\\left_Image\\";
	string infileName = "D:\\学习资料\\C++\\C++ code\\相机标定棋盘格图片生成\\left_Image\\filename.txt";  //保存所有输入图片的文件名
	string outfileName = "D:\\学习资料\\C++\\C++ code\\相机标定棋盘格图片生成\\left_Image\\caliberation_result.txt";

	//保存输入文件路径的vector
	vector<string>filenames;

	//创建输入文件流，读入图片
	ifstream ifs(infileName);
	//创建输出文件流，保存标定结果
	ofstream ofs(outfileName);

	/*
	1、读入图像，并提取每幅图像的角点
	*/
	cout << "开始提取角点" << endl;
	//记录图像数量
	int imgCount = 0;
	//图像尺寸
	Size imgSize;
	//标定板每行每列的角点数
	Size boardSize = Size(9, 6);
	//保存每幅图像上检测到的角点
	vector<Point2f>imgPointsBuf;
	//保存所有图像的角点
	vector<vector<Point2f>>imgPointsSeq;

	char filename[100];
	if (ifs.is_open()) {
		
		//读取每一张图片
		while (!ifs.eof()) {

			//从inputfileName中读取文件路径
			ifs.getline(filename, sizeof(filename) / sizeof(char));


			//保存文件名
			filenames.push_back(basedir + filename);


			//读取图片
			Mat imgInput = imread(filenames[imgCount]);
			//获取图片的宽和高
			if (imgCount == 0) {
				
				imgSize.height = imgInput.rows;
				imgSize.width = imgInput.cols;
				cout << "图片的宽是： " << imgSize.width << endl;
				cout << "图片的高是： " << imgSize.height << endl;
			}

			cout << "图像数：" << imgCount << endl;
			imgCount++;

			//对图像进行角点提取
			if (findChessboardCorners(imgInput, boardSize, imgPointsBuf) == false) {
				cout << "找不到角点" << endl;
				exit(1);  //异常退出
			}
			else {  //找到角点
				//将图片转化为单通道灰度图
				Mat viewGray;
				cvtColor(imgInput, viewGray, COLOR_BGR2GRAY);
				//进行亚像素提取
				find4QuadCornerSubpix(viewGray, imgPointsBuf, Size(5, 5));
				//保存亚像素角点
				imgPointsSeq.push_back(imgPointsBuf);
				//显示角点位置
				drawChessboardCorners(viewGray, boardSize, imgPointsBuf, true);
				//显示图片
				imshow("角点检测图像", viewGray);
				//等待0.5s
				/*waitKey(500);*/
			}
		}

		//计算每张图片上的角点数 54
		int cornerNum = boardSize.width * boardSize.height;

		for (int i = 0; i < imgPointsSeq.size(); i++) {
			cout << "\n输出第 " << i + 1 << " 张图片的角点数据：" << endl;
			for (int j = 0; j < cornerNum; j++) {
				cout << j + 1 << ": (" << imgPointsSeq[i][j].x << ", " << imgPointsSeq[i][j].y << ")\t";
				//每三个角点为一行
				if ((j + 1) % 3 == 0) {
					cout << endl;
				}
			}
			
		}

		cout << "角点提取完成!" << std::endl;

		/*
		2、相机标定，世界坐标系原点位于标定板左上角（第一个格子的左上角）
		*/
		cout << "开始标定" << endl;
		//设置棋盘格角点的世界坐标
		
		//设置每个格子的物理尺寸,mm
		Size squareSize = Size(26, 26);
		//每幅图像的角点数量
		vector<int>pointCounts;
		//标定板上三维点的坐标
		vector<vector<Point3f>>objectPoints;
		//相机内参矩阵 M=[fx γ u0,0 fy v0,0 0 1]
		Mat cameraMatrix = Mat(3, 3, CV_64F, Scalar::all(0));
		//相机畸变参数
		Mat distCoeffs = Mat(1, 5, CV_64F, Scalar::all(0));
		//每幅图片的旋转矩阵
		vector<Mat>rvecs;
		//每幅图片的平移向量
		vector<Mat>tvecs;

		//初始化标定板角点的三维坐标
		for (int n = 0; n < imgCount; n++) {
			//保存一张图片的角点
			vector<Point3f>tempPointSet;
			for (int i = 0; i < boardSize.height; i++) {
				for (int j = 0; j < boardSize.width; j++) {
					//存储三维点的坐标
					Point3f realPoint;
					realPoint.x = j * squareSize.width;
					realPoint.y = i * squareSize.height;
					realPoint.z = 0;
					tempPointSet.push_back(realPoint);
				}
			}
			objectPoints.push_back(tempPointSet);
		}

		//初始化每幅图像中的角点数量，假定每幅图像中都可以看到完整的标定板
		for (int i = 0; i < imgCount; i++)
		{
			pointCounts.push_back(boardSize.width * boardSize.height);
		}
		//开始标定
		calibrateCamera(objectPoints, imgPointsSeq, imgSize, cameraMatrix, distCoeffs, rvecs, tvecs);
		cout << "标定完成" << endl;
		//对标定结果进行评价
		cout << "开始评价标定结果" << endl;
		//所有图像的平均误差
		double totalErr = 0.0;
		//每幅图像的平均误差
		double err = 0.0;
		//保存重新计算得到的投影点
		vector<Point2f>imgPoints2;
		cout << "每幅图像的标定误差：" << endl;
		ofs << "每幅图像的标定误差：" << endl;
		//对每幅图像的标定误差进行计算
		for (int i = 0; i < imgCount; i++) {
			vector<Point3f>tempPointSet = objectPoints[i];
			//计算重投影的二维点
			projectPoints(tempPointSet, rvecs[i], tvecs[i], cameraMatrix, distCoeffs, imgPoints2);
			//计算重投影点和旧投影点之间的误差
			vector<Point2f> tempImagePoint = imgPointsSeq[i];
			//将角点转换成Mat格式，方便统计平均误差
			Mat tempImagePointMat = Mat(1, tempImagePoint.size(), CV_32FC2);
			Mat imgPoints2Mat = Mat(1, imgPoints2.size(), CV_32FC2);
			for (int j = 0; j < tempImagePoint.size(); j++) {
				imgPoints2Mat.at<Vec2f>(0, j) = Vec2f(imgPoints2[j].x, imgPoints2[j].y);
				tempImagePointMat.at<Vec2f>(0, j) = Vec2f(tempImagePoint[j].x, tempImagePoint[j].y);
			}
			//统计平均误差
			err = norm(imgPoints2Mat, tempImagePointMat, NORM_L2);
			err /= pointCounts[i];
			totalErr += err;
			cout << "第" << i + 1 << "幅图像的平均误差：" << err << "像素" << endl;
			ofs << "第" << i + 1 << "幅图像的平均误差：" << err << "像素" << endl;
		}

		//所有图像的平均误差
		totalErr /= imgCount;
		cout << "总体平均误差：" << totalErr << "像素" << endl;
		ofs << "总体平均误差：" << totalErr << "像素" << endl;
		cout << "评价完成!" << endl;
		//保存标定结果
		cout << "开始保存标定结果...." << endl;

		Mat rotationMatrix = Mat(3, 3, CV_32FC1, Scalar::all(0));
		ofs << "相机内参矩阵：" << endl;
		ofs << cameraMatrix << endl << endl;
		ofs << "畸变系数：" << endl;
		ofs << distCoeffs << endl << endl;

		for (int i = 0; i < imgCount; i++) {
			ofs << "第" << i + 1 << "幅图像的旋转向量" << endl;
			ofs << rvecs[i] << endl;
			//将旋转向量转换成旋转矩阵
			Rodrigues(rvecs[i], rotationMatrix);
			ofs << "第" << i + 1 << "幅图像的旋转矩阵" << endl;
			ofs << rotationMatrix << endl;
			ofs << "第" << i + 1 << "幅图像的平移向量" << endl;
			ofs << tvecs[i] << endl;
		}
		cout << "保存完成!" << endl;

		/*
		显示结果并对原图进行校正
		*/
		Mat mapx = cv::Mat(imgSize, CV_32FC1);
		Mat mapy = cv::Mat(imgSize, CV_32FC1);
		Mat R = cv::Mat::eye(3, 3, CV_32F);
		cout << "显示矫正图像" << endl;
		for (int i = 0; i != imgCount; i++)
		{
			std::cout << "Frame #" << i + 1 << "..." << endl;
			//计算图片畸变矫正的映射矩阵mapx、mapy(不进行立体校正、立体校正需要使用双摄)
			initUndistortRectifyMap(cameraMatrix, distCoeffs, R, cameraMatrix, imgSize, CV_32FC1, mapx, mapy);
			//读取一张图片
			Mat imageSource = imread(filenames[i]);
			Mat newimage = imageSource.clone();
			//另一种不需要转换矩阵的方式
			//undistort(imageSource,newimage,cameraMatrix,distCoeffs);
			//进行校正
			remap(imageSource, newimage, mapx, mapy, INTER_LINEAR);
			imshow("原始图像", imageSource);
			imshow("矫正后图像", newimage);
			waitKey();
		}

		//释放资源
		ifs.close();
		ofs.close();
	}

}

int main()
{
	cameraCalibrator();

	system("pause");
	return 0;
}

4. 双目相机标定

待更新

5. 极线校正 + BM立体匹配

/****************极线矫正程序1.2*******************************************/
/****************已完成功能：双目极限矫正拍照，BM算法出视差****************/

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

using namespace std;
using namespace cv;

/**********【拼接分辨率】*******************************/
const int cap_width = 2560;
const int cap_height = 720;

/**********【这里是镜头分辨率，不要弄混】***************/
const int nomoimageWidth = 1280;
const int nomoimageHeight = 720;

/**********【立体匹配算法程序相关参数】**************/
int blockSize = 6, uniquenessRatio = 30, numDisparities = 255;
Ptr<StereoBM> bm = StereoBM::create(255, 3);

/**********【声明函数】*********************************/
int Polar_correction(Mat& input_L, Mat& input_R, string fileNameL_rec, string fileNameR_rec);
int camera_cap();
void stereo_match(int, void*, Mat input_rectifyImageL, Mat input_rectifyImageR, Rect validROIL, Rect validROIR);

/***********【主函数】**********************************/
int main()
{
	camera_cap();
	return 0;
}

int camera_cap()
{
	VideoCapture cap;
	cap.open(0);
	if (!cap.isOpened())                         //判断是否成功打开相机
	{
		cout << "请连接相机" << endl;
	}

	cap.set(CAP_PROP_FRAME_WIDTH, cap_width);  //设置捕获视频的宽度
	cap.set(CAP_PROP_FRAME_HEIGHT, cap_height);  //设置捕获视频的高度

	//设置图片的保存路径
	string pathNameLeft = "D:\\学习资料\\C++\\C++ code\\深度获取算法\\深度获取算法\\左原图";
	string pathNameRight = "D:\\学习资料\\C++\\C++ code\\深度获取算法\\深度获取算法\\右原图";
	string pathNameLeft_rec = "D:\\学习资料\\C++\\C++ code\\深度获取算法\\深度获取算法\\左校正图";
	string pathNameRight_rec = "D:\\学习资料\\C++\\C++ code\\深度获取算法\\深度获取算法\\右校正图";

	string formatName = ".jpg";
	string num;
	string fileNameL;  //左相机图像的保存路径
	string fileNameR;  //右相机图像的保存路径
	string fileNameL_rec;  //左相机校准图像的保存路径
	string fileNameR_rec;  //右相机校准图像的保存路径

	int i = 0;

	while (1)
	{
		Mat frame, frame_L, frame_R;
		cap >> frame;
		cvtColor(frame, frame, CV_BGR2GRAY);//从相机捕获一帧图像
		frame_L = frame(Rect(0, 0, nomoimageWidth, nomoimageHeight));  //获取缩放后左Camera的图像
		frame_R = frame(Rect(nomoimageWidth, 0, nomoimageWidth, nomoimageHeight)); //获取缩放后右Camera的图像
		namedWindow("Video_L", 1);  namedWindow("Video_R", 2);
		imshow("Video_L", frame_L); imshow("Video_R", frame_R);
		char c = waitKey(1);
		if (c == 32)
		{
			num = to_string(i);
			fileNameL = pathNameLeft + num + formatName;
			fileNameR = pathNameRight + num + formatName;
			fileNameL_rec = pathNameLeft_rec + num + formatName;
			fileNameR_rec = pathNameRight_rec + num + formatName;
			//写入图片
			imwrite(fileNameL, frame_L);
			imwrite(fileNameR, frame_R);
			//极线校正 + 立体匹配计算视差图，并将校正后图像和视差图进行保存
			Polar_correction(frame_L, frame_R, fileNameL_rec, fileNameR_rec);
			i++;
		}
		else if (c == 27)
			break;
		frame_L.release();
		frame_R.release();
		frame.release();
	}
	cap.release();
	//释放对相机的控制
	return 0;
}

int Polar_correction(Mat& input_L, Mat& input_R, string fileNameL_rec, string fileNameR_rec)
{
	Size imageSize = Size(nomoimageWidth, nomoimageHeight);
	Mat grayImageL;
	Mat grayImageR;
	Mat rectifyImageL, rectifyImageR;

	//图像校正之后，会对图像进行裁剪，这里的validROI就是指裁剪之后的区域
	Rect validROIL;                                     
	Rect validROIR;
	Mat mapLx, mapLy, mapRx, mapRy;                   //映射表  
	Mat Rl, Rr, Pl, Pr, Q;                            //校正旋转矩阵R，投影矩阵P, 重投影矩阵Q
	Mat cameraMatrixL = (Mat_<double>(3, 3) << 1409.69465063705, -5.99010436417051, 793.100580616560, 0, 1390.65607363315, 403.090968727600, 0, 0, 1);
	Mat distCoeffL = (Mat_<double>(5, 1) << 0.027710572959967, 0.339574135874314, 0.011325178299636, 0.001675346032984, 0.705489274621470);
	Mat cameraMatrixR = (Mat_<double>(3, 3) << 1420.47912458566, -5.00144604375742, 663.890484487222, 0, 1401.16619687819, 381.587467659162, 0, 0, 1);
	Mat distCoeffR = (Mat_<double>(5, 1) << 0.085702725158753, -0.420790043549425, 0.012033196790578, 0.00001904883934207934, 2.501420052613930);//[kc_right_01,  kc_right_02,  kc_right_03,  kc_right_04,   kc_right_05]
	Mat T = (Mat_<double>(3, 1) << -48.0155, -0.9816, -0.1169);     //T平移向量[    T_01,    T_02,       T_03]
	//Mat rec = (Mat_(3, 1) << 0.0040, 0.0045, 0.0205);  //rec旋转向量[rec_01,  rec_02, rec_03]
	Mat R = (Mat_<double>(3, 3) << 0.999730015150978, -0.0229285407511850, 0.00376547809011760, 0.0229188502608065, 0.999733956049216, -0.00259681052574187, -0.00382401738341249, 0.00250980899773640, 0.999989538820205);  //R矩阵，用于中间计算
	//Rodrigues(rec, R);  //Rodrigues变换
	//CALIB_ZERO_DISPARITY参数会让校正后的左右图像的主点在同一像素坐标
	//设置为0的时候，校正后的左右图像只有有效的部分会被显示
	//这里应用的是bouguet极线校正
	stereoRectify(cameraMatrixL, distCoeffL, cameraMatrixR, distCoeffR, imageSize, R, T, Rl, Rr, Pl, Pr, Q, CALIB_ZERO_DISPARITY, 0, imageSize, &validROIL, &validROIR);

	//验证校正前后的图像的大小有没有发生变化
	cout << "原图像的大小为[ " << imageSize.width << ", " << imageSize.height << " ]" << endl;
	cout << "校正后的左图像的大小为[ " << validROIL.width << ", " << validROIL.height << " ]" << endl;
	cout << "校正后的又图像的大小为[ " << validROIR.width << ", " << validROIR.height << " ]" << endl;

	//畸变校正
	initUndistortRectifyMap(cameraMatrixL, distCoeffL, Rl, Pl, imageSize, CV_32FC1, mapLx, mapLy);
	initUndistortRectifyMap(cameraMatrixR, distCoeffR, Rr, Pr, imageSize, CV_32FC1, mapRx, mapRy);

	remap(input_L, rectifyImageL, mapLx, mapLy, INTER_LINEAR);
	remap(input_R, rectifyImageR, mapRx, mapRy, INTER_LINEAR);

	//将极线校正和畸变校正后的图像进行保存
	imwrite(fileNameL_rec, rectifyImageL);
	imwrite(fileNameR_rec, rectifyImageR);

	//立体匹配
	stereo_match(0, 0, rectifyImageL, rectifyImageR, validROIL, validROIR);
	//waitKey(0);
	return 0;
}

/*------------------------------立体匹配----------------------------------------*/
void stereo_match(int, void*, Mat input_rectifyImageL, Mat input_rectifyImageR, Rect validROIL, Rect validROIR)
{
	Ptr<cv::StereoBM> bm = cv::StereoBM::create(16, 9);
	Mat disp;

	bm->setPreFilterType(CV_STEREO_BM_NORMALIZED_RESPONSE);
	bm->setPreFilterSize(9);
	bm->setPreFilterCap(31);
	bm->setBlockSize(21);
	bm->setMinDisparity(0);
	bm->setNumDisparities(256);
	bm->setTextureThreshold(10);
	bm->setUniquenessRatio(15);
	bm->setSpeckleWindowSize(10);
	bm->setSpeckleRange(32);
	bm->setROI1(validROIL);
	bm->setROI2(validROIR);

	// Compute disparity
	bm->compute(input_rectifyImageL, input_rectifyImageR, disp);
	disp = disp * 5;
	imshow("视差图", disp);
	imwrite("Disparity.png", disp);

}

6. 极线校正 + SGBM立体匹配

/********************************************************************************/
/***************************    2020.06.17      *********************************/
/********************************************************************************/
#include 
#include 
#include 
#include 
#include 

using namespace std;
using namespace cv;

char Disp_SGBMName[100];//SGbm

const int cap_width = 2560;
const int cap_height = 720;

const int nomoimageWidth = 1280;
const int nomoimageHeight = 720;


const int mindisparity = 0;  
const int ndisparities = 64;  //必须是16的倍数
const int SADWindowSize = 15;  //一般是3 - 11的范围内，且必须是奇数

int camera_cap();
int Polar_correction(Mat& input_L, Mat& input_R);
void Stereo_Match_SGBM(const Mat& input_rectifyImageL, const Mat& input_rectifyImageR, int mindisparity, int ndisparities, int SADWindowSize);
void disp2Depth(Mat& dispMap, Mat& depthMap);
void showdistance(Mat& show_disp, Mat& disp);

//Here is the parameters, if you want to use the camera, you should calibarate it firstly.
Mat cameraMatrixL = (Mat_<double>(3, 3) << 2218.99510682661, 2.50199223879579, 664.626528887485, 0, 2198.04424606297, 258.133727158027, 0, 0, 1);
Mat distCoeffL = (Mat_<double>(5, 1) << 0.103882144131403, 1.061480801573703, 0.005184583303669, -0.027133600662323, -1.151901975983256e+02);
Mat cameraMatrixR = (Mat_<double>(3, 3) << 2223.79446455688, 1.61915842579726, 643.164824995920, 0, 2211.52692513356, 291.745225638993, 0, 0, 1);
Mat distCoeffR = (Mat_<double>(5, 1) << -0.090387911835514, 4.274397648672221, 0.003376316646605, -0.015886677361415, -45.957977870123685);//[kc_right_01,  kc_right_02,  kc_right_03,  kc_right_04,   kc_right_05]
Mat T = (Mat_<double>(3, 1) << -36.9567, -0.6789, 3.8307);
Mat rec = (Mat_<double>(3, 1) << 0.0089, -0.0416, 0.0330);
Mat element = getStructuringElement(MORPH_RECT, Size(17, 17));  //用于形态学运算，腐蚀、膨胀等


int main()
{
    camera_cap();
    return 0;
}

int camera_cap()
{
    VideoCapture cap;
    cap.open(0);

    cap.set(CAP_PROP_FRAME_WIDTH, cap_width);
    cap.set(CAP_PROP_FRAME_HEIGHT, cap_height);

    while (true)
    {
        Mat frame, frame_L, frame_R;
        cap >> frame;
        cvtColor(frame, frame, COLOR_BGR2GRAY);
        frame_L = frame(Rect(0, 0, nomoimageWidth, nomoimageHeight));  //左相机图像
        frame_R = frame(Rect(nomoimageWidth, 0, nomoimageWidth, nomoimageHeight));  //右相机图像
        //进行极线校正和畸变校正，同时将校正后的结果保存在frame_L,frame_R
        Polar_correction(frame_L, frame_R);  
        Mat frame_L_resize, frame_R_resize;
        //将校正的图像进行缩小，缩小为(640,480)
        resize(frame_L, frame_L_resize, Size(640, 480));
        resize(frame_R, frame_R_resize, Size(640, 480));
        namedWindow("Video_L", 1);  
        namedWindow("Video_R", 2);
        imshow("Video_L", frame_L);
        imshow("Video_R", frame_R); 
        Stereo_Match_SGBM(frame_L_resize, frame_R_resize, mindisparity, ndisparities, SADWindowSize);
        char c = waitKey(1);
        if (c == 'q')
        {
            break;
        }
        frame_L.release();
        frame_R.release();
        frame.release();
    }
    cap.release();
    return 0;
}

int Polar_correction(Mat& input_L, Mat& input_R)
{
    Size imageSize = Size(nomoimageWidth, nomoimageHeight);
    Mat resize_rectifyL, resize_rectifyR;
    Mat grayImageL;
    Mat grayImageR;
    Mat rectifyImageL, rectifyImageR;
    Rect validROIL;
    Rect validROIR;
    Mat mapLx, mapLy, mapRx, mapRy;
    Mat Rl, Rr, Pl, Pr, Q;
    Mat R;
    Rodrigues(rec, R);
    stereoRectify(cameraMatrixL, distCoeffL, cameraMatrixR, distCoeffR, imageSize, R, T, Rl, Rr, Pl, Pr, Q, CALIB_ZERO_DISPARITY, 0, imageSize, &validROIL, &validROIR);
    initUndistortRectifyMap(cameraMatrixL, distCoeffL, Rl, Pl, imageSize, CV_32FC1, mapLx, mapLy);
    initUndistortRectifyMap(cameraMatrixR, distCoeffR, Rr, Pr, imageSize, CV_32FC1, mapRx, mapRy);
    remap(input_L, input_L, mapLx, mapLy, INTER_LINEAR);
    remap(input_R, input_R, mapRx, mapRy, INTER_LINEAR);
    return 0;
}

void Stereo_Match_SGBM(const Mat& input_rectifyImageL, const Mat& input_rectifyImageR, int mindisparity, int ndisparities, int SADWindowSize)
{
    Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(mindisparity, ndisparities, SADWindowSize);
    Mat SGBM_Disp;
    //P1和P2是视差平滑度，下面的是经验得到的比较好的值
    int P1 = 8 * input_rectifyImageL.channels() * SADWindowSize * SADWindowSize;
    int P2 = 32 * input_rectifyImageR.channels() * SADWindowSize * SADWindowSize;
    sgbm->setP1(P1);
    sgbm->setP2(P2);
    sgbm->setPreFilterCap(15);
    sgbm->setUniquenessRatio(5);
    sgbm->setSpeckleRange(1);
    sgbm->setSpeckleWindowSize(100);
    sgbm->setDisp12MaxDiff(2);
    sgbm->setMode(8);
    //计算视差图
    sgbm->compute(input_rectifyImageL, input_rectifyImageR, SGBM_Disp);
    //除以16获得真实视差图
    Mat SGBM_Disp8U;
    SGBM_Disp.convertTo(SGBM_Disp, CV_8U, 1 / 16.0);
    
    //用形态学进行视差图的孔洞填充
    morphologyEx(SGBM_Disp8U, SGBM_Disp8U, MORPH_CLOSE, element);

    //利用视差图计算深度图
    Mat depth(SGBM_Disp8U.rows, SGBM_Disp8U.cols, CV_8U);
    disp2Depth(SGBM_Disp8U, depth);

    namedWindow("Depth", 3);
    imshow("Depth", depth);
    waitKey(1);


    clock_t time_name = clock();
    string name = R"(F:\cpppractice\sgbmstereo\cap\)" + to_string(time_name) + ".png";
    char capt = waitKey(1);
    if (capt == 'p')
    {

        imwrite(name, SGBM_Disp8U);
        imwrite("2222.png",depth);//保存深度图
    }   
    SGBM_Disp.release();
}

void disp2Depth(Mat& dispMap, Mat& depthMap)
{
    double fx = 1943.21;
    float baseline = 3.65;//双目标定中的T向量的第一个元素Tx就是基线距离
    float bf;
    bf = fx * baseline;
    int height = dispMap.rows;
    int width = dispMap.cols;
    for (int i = 0; i < height; i++)
    {
        for (int j = 0; j < width; j++)
        {
            if (!dispMap.at<uchar>(i, j)) continue;
            depthMap.at<uchar>(i, j) = bf / dispMap.at<uchar>(i, j);
        }
    }
}


void showdistance(Mat& show_disp, Mat& disp)
{
    Mat dist = disp(Rect(int(disp.cols / 2) - 10, int(disp.rows / 2) - 10, 20, 20));
    Scalar mean;
    Scalar stddev;
    meanStdDev(dist, mean, stddev);
    uchar  mean_pxl = mean.val[0];
    double fx = 1943.21;
    float baseline = 0.0365;//cm
    float bf = 0.5 * fx * baseline;
    float distance = bf / float(mean_pxl);
    Point orgin;
    orgin.x = 50;
    orgin.y = 50;
    string showtxt = "Distance : " + to_string(distance) + "m";
    cv::putText(show_disp, showtxt, orgin, cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255, 255, 255), 1, 8, 0);
}