opencv3.4.6 实现双目校正+立体匹配+测距

S1. 采用matlab标定工具箱进行左、右相机单目和双目标定；

S2.  根据标定结果计算校正旋转矩阵、投影矩阵、重投影矩阵Q；

S3. 根据校正旋转矩阵、投影矩阵，计算映射表，校正图像使其共面、共行；

S4. 采用SGBM计算视差disp；

S5. 根据重投影矩阵Q和视差disp，计算三维坐标点；

#include 
#include 

using namespace std;
using namespace cv;

const int imageWidth = 320;  //摄像头的分辨率  
const int imageHeight = 240;
Size imageSize = Size(imageWidth, imageHeight);

Mat grayImageL, grayImageR;
Mat rectifyImageL, rectifyImageR;

Rect validROIL;//图像校正之后，会对图像进行裁剪，这里的validROI就是指裁剪之后的区域, 其内部的所有像素都有效
Rect validROIR;

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

Mat matlab_cameraMatrixL;
Mat matlab_distCoeffL;
Mat matlab_cameraMatrixR;
Mat matlab_distCoeffR;
Mat matlab_Rl, matlab_Rr, matlab_Pl, matlab_Pr, matlab_R, matlab_T;
Mat R;

/*****立体匹配*****/
void stereo_match(cv::Mat rectifyImageL, cv::Mat rectifyImageR, cv::Mat &XYZ)
{
	int numberOfDisparities = ((imageSize.width / 8) + 15) & -16;
	int numDisparities = 6;

	cv::Ptr sgbm = StereoSGBM::create(0, 16, 3);
	sgbm->setPreFilterCap(32);
	int SADWindowSize = 9;
	int sgbmWinSize = SADWindowSize > 0 ? SADWindowSize : 3;
	sgbm->setBlockSize(sgbmWinSize);
	int cn = rectifyImageL.channels();
	sgbm->setP1(8 * cn*sgbmWinSize*sgbmWinSize);
	sgbm->setP2(32 * cn*sgbmWinSize*sgbmWinSize);
	sgbm->setMinDisparity(0);
	sgbm->setNumDisparities(numberOfDisparities);
	sgbm->setUniquenessRatio(10);
	sgbm->setSpeckleWindowSize(100);
	sgbm->setSpeckleRange(32);
	sgbm->setDisp12MaxDiff(1);
	sgbm->setMode(cv::StereoSGBM::MODE_SGBM);

	Mat disp, disp8;
	sgbm->compute(rectifyImageL, rectifyImageR, disp);//输入图像必须为灰度图

	disp.convertTo(disp8, CV_8U, 255 / (numberOfDisparities*16.));//计算出的视差是CV_16S格式
	
	Mat xyz;              //三维坐标
	reprojectImageTo3D(disp, xyz, Q, true); //在实际求距离时，ReprojectTo3D出来的X / W, Y / W, Z / W都要乘以16(也就是W除以16)，才能得到正确的三维坐标信息。
	XYZ = xyz * 16;
}

/*****主函数*****/
int main()
{
	matlab_cameraMatrixL = (Mat_(3, 3) << 265.53128, 0, 156.44112,
		0, 265.49126, 118.19951,
		0, 0, 1);
	matlab_distCoeffL = (Mat_(5, 1) << 0.06731, -0.12510, 0.00167, 0.00104, 0.00000);
	matlab_cameraMatrixR = (Mat_(3, 3) << 265.15642, 0, 156.58105,
		0, 265.09738, 119.33607,
		0, 0, 1);
	matlab_distCoeffR = (Mat_(5, 1) << 0.06789, -0.14533, 0.00158, -0.00365, 0.00000);

	matlab_R = (Mat_(3, 1) << -0.02944, -0.02725, -0.03523);
	matlab_T = (Mat_(3, 1) << -28.86386, 0.03070, 0.04467);

	R = (Mat_(3, 3) <<
		0, 0, 0,
		0, 0, 0,
		0, 0, 0);
	Rodrigues(matlab_R, R); //Rodrigues变换
	
	//经过双目标定得到摄像头的各项参数后，采用OpenCV中的stereoRectify(立体校正)得到校正旋转矩阵R、投影矩阵P、重投影矩阵Q
	//flags-可选的标志有两种零或者 CV_CALIB_ZERO_DISPARITY ,如果设置 CV_CALIB_ZERO_DISPARITY 的话，该函数会让两幅校正后的图像的主点有相同的像素坐标。否则该函数会水平或垂直的移动图像，以使得其有用的范围最大
	//alpha-拉伸参数。如果设置为负或忽略，将不进行拉伸。如果设置为0，那么校正后图像只有有效的部分会被显示（没有黑色的部分），如果设置为1，那么就会显示整个图像。设置为0~1之间的某个值，其效果也居于两者之间。
	stereoRectify(matlab_cameraMatrixL, matlab_distCoeffL, matlab_cameraMatrixR, matlab_distCoeffR, imageSize, R, matlab_T, Rl, Rr, Pl, Pr, Q, CALIB_ZERO_DISPARITY,
		0, imageSize, &validROIL, &validROIR);
	//再采用映射变换计算函数initUndistortRectifyMap得出校准映射参数,该函数功能是计算畸变矫正和立体校正的映射变换
	initUndistortRectifyMap(matlab_cameraMatrixL, matlab_distCoeffL, Rl, Pr, imageSize, CV_32FC1, mapLx, mapLy);
	initUndistortRectifyMap(matlab_cameraMatrixR, matlab_distCoeffR, Rr, Pr, imageSize, CV_32FC1, mapRx, mapRy);


	grayImageL = imread("..\\image\\left01.jpg", 0);
	grayImageR = imread("..\\image\\right01.jpg", 0);

	//然后用remap来校准输入的左右图像
	//interpolation-插值方法，但是不支持最近邻插值
	remap(grayImageL, rectifyImageL, mapLx, mapLy, INTER_LINEAR);
	remap(grayImageR, rectifyImageR, mapRx, mapRy, INTER_LINEAR);

	cv::Mat XYZMat;
	stereo_match(rectifyImageL, rectifyImageR, XYZMat);

	return 0;
}