双目矫正+测距+深度图+点云

![灰度图](https://img-blog.csdnimg.cn/20200601204821393.jpg?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0FzYWJjMTIzNDU=,size_16,color_FFFFFF,t_70

因为只有一个视角的深度图，所以生成的点云图很粗糙只有个轮廓，顶多算是稀疏原始点云。还需要后期点云滤波、多点云拼接。至于为什么会有对称的两个轮廓，我觉得可能生成了左右两个视角的点云图，还有我的相机参数不完全匹配，导致两个分离了没有融合，有时间再研究研究。

//2020.07.14更新
之前视差图生成点云图显示有问题的原因找到了，大多数情况下大家都会遇到的问题是点云分层，呈放射状杂乱无章。不要慌，一般就两个原因：点云质量差和数据格式有问题。点云质量差意味着噪声多，生成的噪点点云杂乱分布，找不到规律，看不到立体效果，或者生成的视差图不够平滑空洞多，解决的办法就是使劲调参找到足够平滑的视差图；视差图数据格式是另外一个重要的因素，这里我就犯了严重的错误，ushort d = disp8.ptr<ushort>(row)[col];这里我用的是ushort类型，输入的却是disp8，对应的数据类型应该是CV_8U，uchar类型。所以应该把disp8改为disp，CV_16U，ushort类型才对。所以这一步改为ushort d = disp.ptr<ushort>(row)[col]就对了。需要说明的是为了让视差图的分布更平滑大部分常见的视差图格式都是16位png格式的图片，所以大家在格式转化上要格外注意。
分享一下更改后的效果：


视差图平滑到我这个程度生成的点云不会差的。

/******************************/
/*        立体匹配和测距        */
/******************************/

#include <pcl/visualization/cloud_viewer.h>
#include <iostream>  
#include <pcl/io/io.h>  
#include <pcl/io/pcd_io.h>  
#include <opencv2/opencv.hpp>  

using namespace cv;
using namespace std;
using namespace pcl;

using namespace std;
using namespace cv;
using namespace Eigen;

const int imageWidth = 640*2;                             //摄像头的分辨率  
const int imageHeight = 240*2;


Vec3f  point3;
float d;
Size imageSize = Size(imageWidth>>1, imageHeight);

Mat rgbImageL, grayImageL;
Mat rgbImageR, grayImageR;
Mat rgbRectifyImageL, rgbRectifyImageR;

Mat rectifyImageL, rectifyImageR;

Rect validROIL;//图像校正之后，会对图像进行裁剪，这里的validROI就是指裁剪之后的区域  
Rect validROIR;

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

Point origin;         //鼠标按下的起始点
Rect selection;      //定义矩形选框
bool selectObject = false;    //是否选择对象




int blockSize = 0, uniquenessRatio = 0, numDisparities = 0;
Ptr<StereoBM> bm = StereoBM::create(16, 9);

/*事先标定好的左相机的内参矩阵
fx 0 cx
0 fy cy
0  0  1
*/
Mat cameraMatrixL = (Mat_<double>(3, 3) << 5.780149331601065e+02, 0, 3.555745068743920e+02,
	0, 5.792595377241396e+02, 2.534936001929042e+02,
	0, 0, 1);
//获得的畸变参数



/*418.523322187048	0	0
-1.26842201390676	421.222568242056	0
344.758267538961	243.318992284899	1 */ //2

Mat distCoeffL = (Mat_<double>(5, 1) << 0.060326909619728, -0.006338890383364, -4.984238272469574e-05, -0.001636185247379, -0.247991841327280);
//[0.006636837611004,0.050240447649195] [0.006681263320267,0.003130367429418]


/*事先标定好的右相机的内参矩阵
fx 0 cx
0 fy cy
0  0  1
*/
Mat cameraMatrixR = (Mat_<double>(3, 3) << 5.778378235199527e+02, 0, 3.484120454356872e+02,
	0, 5.793064373178438e+02, 2.522698803266952e+02,
	0, 0, 1);


/*
417.417985082506	0	0
0.498638151824367	419.795432389420	0
309.903372309072	236.256106972796	1
*/ //2


Mat distCoeffR = (Mat_<double>(5, 1) << 0.069036951737383, -0.074429302261481, 5.694882132841171e-04, -0.002668327489554, -0.144742509783022);
//[-0.038407383078874,0.236392800301615]  [0.004121779274885,0.002296129959664]



Mat T = (Mat_<double>(3, 1) << -59.684781615760150, -0.319025755946363, 0.275826997339757);//T平移向量
//[-1.210187345641146e+02,0.519235426836325,-0.425535566316217]
															 //对应Matlab所得T参数
//Mat rec = (Mat_<double>(3, 1) << -0.00306, -0.03207, 0.00206);//rec旋转向量，对应matlab om参数  我 
Mat rec = (Mat_<double>(3, 3) << 0.999996782379644, 5.706428486117677e-04, -0.002471759919034,
	-5.720218896103840e-04, 0.999999681132956, -5.572476509248812e-04,
	0.002471441141484, 5.586597586930527e-04, 0.999996789933827);                //rec旋转向量，对应matlab om参数  我 

/* 0.999341122700880	0.000660748031451783	-0.0362888948713456
-0.00206388651740061	0.999250989651683	-0.0386419468010579
0.0362361815232777	0.0386913826603732	0.998593969567432 */

//Mat T = (Mat_<double>(3, 1) << -48.4, 0.241, -0.0344);//T平移向量
																							  //[-1.210187345641146e+02,0.519235426836325,-0.425535566316217]
																							  //对应Matlab所得T参数
//Mat rec = (Mat_<double>(3, 1) << -0.039, -0.04658, 0.00106);//rec旋转向量，对应matlab om参数   倬华

Mat R;//R 旋转矩阵
Mat frame, frame_L, frame_R;

static void saveXYZ(const char* filename, const Mat& mat)
{
	const double max_z = 1.0e4;
	FILE* fp = fopen(filename, "wt");
	for (int y = 0; y < mat.rows; y++)
	{
		for (int x = 0; x < mat.cols; x++)
		{
			Vec3f point = mat.at<Vec3f>(y, x);
			if (fabs(point[2] - max_z) < FLT_EPSILON || fabs(point[2]) > max_z) continue;
			fprintf(fp, "%f %f %f\n", point[0], point[1], point[2]);
		}
	}
	fclose(fp);
}

void viewerOneOff(visualization::PCLVisualizer& viewer)
{
	viewer.setBackgroundColor(0.0, 0.0, 0.0);
}


	  /*****立体匹配*****/
void stereo_match(int, void*)
{
	bm->setBlockSize(2 * blockSize + 5);     //SAD窗口大小，5~21之间为宜
	bm->setROI1(validROIL);
	bm->setROI2(validROIR);
	bm->setPreFilterCap(31);
	bm->setMinDisparity(0);  //最小视差，默认值为0, 可以是负值，int型
	bm->setNumDisparities(numDisparities * 16 + 16);//视差窗口，即最大视差值与最小视差值之差,窗口大小必须是16的整数倍，int型
	bm->setTextureThreshold(10);
	bm->setUniquenessRatio(uniquenessRatio);//uniquenessRatio主要可以防止误匹配
	bm->setSpeckleWindowSize(100);
	bm->setSpeckleRange(32);
	bm->setDisp12MaxDiff(-1);
	Mat disp;
	bm->compute(grayImageL, grayImageR, disp);//输入图像必须为灰度图
	disp.convertTo(disp8, CV_8U, 255 / ((numDisparities * 16 + 16) * 16.));//计算出的视差是CV_16S格式
	reprojectImageTo3D(disp, xyz, Q, true); //在实际求距离时，ReprojectTo3D出来的X / W, Y / W, Z / W都要乘以16(也就是W除以16)，才能得到正确的三维坐标信息。
	xyz = xyz * 16;
	//saveXYZ("point_cloud.txt", xyz);
	imshow("disparity", disp8);

}


/*****描述：鼠标操作回调*****/
static void onMouse(int event, int x, int y, int, void*)
{
	if (selectObject)
	{
		selection.x = MIN(x, origin.x);
		selection.y = MIN(y, origin.y);
		selection.width = std::abs(x - origin.x);
		selection.height = std::abs(y - origin.y);
	}

	switch (event)
	{
	case EVENT_LBUTTONDOWN:   //鼠标左按钮按下的事件
		origin = Point(x, y);
		selection = Rect(x, y, 0, 0);
		selectObject = true;
		//cout << origin << "in world coordinate is: " << xyz.at<Vec3f>(origin) << endl;
		point3 = xyz.at<Vec3f>(origin);
		point3[0];
		//cout << "point3[0]:" << point3[0] << "point3[1]:" << point3[1] << "point3[2]:" << point3[2]<<endl;
		cout << "世界坐标：" << endl;
		cout << "x: " << point3[0] << "  y: " << point3[1] << "  z: " << point3[2] << endl;
		d = point3[0] * point3[0] + point3[1] * point3[1] + point3[2] * point3[2];
		d = sqrt(d);   //mm
	   // cout << "距离是:" << d << "mm" << endl;

		d = d / 10.0;   //cm
		cout << "距离是:" << d << "cm" << endl;

		// d = d/1000.0;   //m
		// cout << "距离是:" << d << "m" << endl;

		break;
	case EVENT_LBUTTONUP:    //鼠标左按钮释放的事件
		selectObject = false;
		if (selection.width > 0 && selection.height > 0)
			break;
	}
}


/*****主函数*****/
int main()
{
	/*
	立体校正
	*/
	Rodrigues(rec, R); //Rodrigues变换
	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);

	/*
	打开摄像头
	*/
	VideoCapture cap;

	cap.open(1);                             //打开相机，电脑自带摄像头一般编号为0，外接摄像头编号为1，主要是在设备管理器中查看自己摄像头的编号。

	cap.set(CAP_PROP_FRAME_WIDTH, 640*2);  //设置捕获视频的宽度
	cap.set(CAP_PROP_FRAME_HEIGHT, 240*2);  //设置捕获视频的高度
	cap.set(CAP_PROP_FPS,15);
	if (!cap.isOpened())                         //判断是否成功打开相机

	{

		cout << "摄像头打开失败!" << endl;

		return -1;

	}


	

	cout << "Painted ImageL" << endl;
	cout << "Painted ImageR" << endl;

	while (1) {
		cap >> frame;                                //从相机捕获一帧图像
		double fScale = 1;                         //定义缩放系数，对2560*720图像进行缩放显示（2560*720图像过大，液晶屏分辨率较小时，需要缩放才可完整显示在屏幕）  

		Size dsize = Size(frame.cols * fScale, frame.rows * fScale);
		Mat imagedst = Mat(dsize, CV_32S);

		resize(frame, imagedst, dsize);
		char image_left[200];
		char image_right[200];
		frame_L = imagedst(Rect(0,0,320*2,240*2));  //获取缩放后左Camera的图像
	//	namedWindow("Video_L", 1);
		imshow("Video_L", frame_L);

		frame_R = imagedst(Rect(320*2, 0, 320*2, 240*2)); //获取缩放后右Camera的图像
//		namedWindow("Video_R", 2);
			imshow("Video_R", frame_R);
		//cap >> frame;
		/*
		读取图片
		*/
		//rgbImageL = imread("image_left_1.jpg", CV_LOAD_IMAGE_COLOR);
		
		//rgbImageR = imread("image_right_1.jpg", CV_LOAD_IMAGE_COLOR);


			/*
			经过remap之后，左右相机的图像已经共面并且行对准了
			*/
		remap(frame_L, rectifyImageL, mapLx, mapLy, INTER_LINEAR);
		remap(frame_R, rectifyImageR, mapRx, mapRy, INTER_LINEAR);
		cvtColor(rectifyImageL, grayImageL, COLOR_BGR2GRAY);
		cvtColor(rectifyImageR, grayImageR, COLOR_BGR2GRAY);
		imshow("Rectify grayImageL", grayImageL);
		imshow("Rectify grayImageR", grayImageR);
		//imshow("rectifyImageL", rectifyImageL);
		//imshow("rectifyImageR", rectifyImageR);

		/*
		把校正结果显示出来
		*/
		//Mat rgbRectifyImageL, rgbRectifyImageR;
		//cvtColor(rectifyImageL, rgbRectifyImageL, COLOR_GRAY2BGR);  //伪彩色图
		//cvtColor(rectifyImageR, rgbRectifyImageR, COLOR_GRAY2BGR);



		//单独显示
		rectangle(rectifyImageL, validROIL, Scalar(0, 0, 255), 3, 8);
		rectangle(rectifyImageR, validROIR, Scalar(0, 0, 255), 3, 8);
		imshow("ImageL After Rectify", rectifyImageL);
		imshow("ImageR After Rectify", rectifyImageR);

		//显示在同一张图上
		Mat canvas;
		double sf;
		int w, h;
		sf = 300. / MAX(imageSize.width, imageSize.height);
		w = cvRound(imageSize.width * sf);
		h = cvRound(imageSize.height * sf);
		canvas.create(h, w * 2, CV_8UC3);   //注意通道

											//左图像画到画布上
		Mat canvasPart = canvas(Rect(w * 0, 0, w, h));                                //得到画布的一部分  
		resize(rectifyImageL, canvasPart, canvasPart.size(), 0, 0, INTER_AREA);     //把图像缩放到跟canvasPart一样大小  
		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(rectifyImageR, 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, 0, 255), 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);

		/*
		立体匹配

		*/
		namedWindow("disparity", WINDOW_AUTOSIZE);
		// 创建SAD窗口 Trackbar
		createTrackbar("BlockSize:\n", "disparity", &blockSize, 18, stereo_match);
		// 创建视差唯一性百分比窗口 Trackbar
		createTrackbar("UniquenessRatio:\n", "disparity", &uniquenessRatio, 30, stereo_match);
		// 创建视差窗口 Trackbar
		createTrackbar("NumDisparities:\n", "disparity", &numDisparities, 16, stereo_match);
		//鼠标响应函数setMouseCallback(窗口名称, 鼠标回调函数, 传给回调函数的参数，一般取0)
		setMouseCallback("disparity", onMouse, 0);
		stereo_match(0, 0);
		//saveXYZ("point_cloud.txt", xyz);


			//生成点云
			//if (waitKey(0) != 32) break;
	 if(waitKey(10)==32)
	 {
		vector<Vector4d, Eigen::aligned_allocator<Vector4d>> pointcloud;
		double fx = 2.762165790037839e+02*2, fy = 2.762317738515432e+02*2, u0 = 1.765880468329375e+02*2, v0 = 1.272320444598781e+02*2;
		// 间距
		double baseline = 65;  // (注意此处的间距为双目相机左右光圈的间距)
		double doffs = 4.1;	// 代表两个相机主点在x方向上的差距, doffs = |u1 - u0|
		// 相机坐标系下的点云
		PointCloud<PointXYZRGB>::Ptr cloud(new PointCloud<PointXYZRGB>);

		for (int row = 0; row < disp8.rows; row++)
		{
			for (int col = 0; col < disp8.cols; col++)
			{
				ushort d = disp8.ptr<ushort>(row)[col];

				if (d == 0)
					continue;
				PointXYZRGB p;

				// depth			
				p.z = fx * baseline / (d + doffs); // Zc = baseline * f / (d + doffs)
				p.x = (col - u0) * p.z / fx; // Xc向右，Yc向下为正
				p.y = (row - v0) * p.z / fy;


				p.y = -p.y;  // 为便于显示，绕x轴三维旋转180°
				p.z = -p.z;

				// RGB
				p.b = rectifyImageL.ptr<uchar>(row)[col * 3];
				p.g = rectifyImageL.ptr<uchar>(row)[col * 3 + 1];
				p.r = rectifyImageL.ptr<uchar>(row)[col * 3 + 2];

				cloud->points.push_back(p);
			}
		}

		cloud->height = disp8.rows;
		cloud->width = disp8.cols;
		cloud->points.resize(cloud->height * cloud->width);

		visualization::CloudViewer viewer("Cloud Viewer");
		viewer.showCloud(cloud);
		viewer.runOnVisualizationThreadOnce(viewerOneOff);

		while (!viewer.wasStopped())
		{
			int user_data = 9;
		}
	 }


		if (waitKey(10) == 27) break;

	} //wheil
	return 0;
}