基于OpenCV+VS2013双目视差测距系统的设计与实现（课设论文）

计算机视觉和机器视觉在人工智能感知领域是一个比较重要的部分。双目视觉又是视觉研究中比较关键的一个分支，利用双目视差原理对物体进行测距这一方向已经有很多科学技术人员参与进来了，也出现了很多研究成果。
本学期的智能机器人课程设计题目是智能机器人视觉之双目视差图，稍微延伸之后，我选择了双目视觉测距系统作为自己本次课设的目标。

---

准备工作：安装OpenCV3.1.0与VS2013 教程链接点我
首先，思考一下，我们正常人的眼睛为什么是两只，而不是一只？为什么韩剧里面男主为女主捐献眼角膜都是成对的捐，而不是给女主一只，自己留一只？难道仅仅是因为独眼龙有损主角形象吗？对此怀有疑问的朋友，可以现在做个试验，找来一支笔和一个笔帽，用眼罩挡住一只眼睛，双手分别拿着笔和笔帽，去将笔帽合在笔上，看看是不是不太好对准呐？这是因为，双目的作用是能让我们判断物体和我们眼睛的距离，也就是深度，正是利用这一点，视觉这一块出现了利用双目视差原理进行测距的研究。
双目测距的流程大概有以下几部分：

（1）图像采集，这一块可以是利用双目摄像头对同时对同一物体进行拍摄，也可以是利用单摄像头在不同的方位对同一物体进行拍摄。
（2）摄像机标定，涉及两个摄像机的相互之间的坐标关系，内参和外参等信息，这块可以参考张正友棋盘标定法，因为我做课设中没有双目摄像机，所以直接省略了这一步。
（3）立体矫正，图像采集之后，需要将图像进行矫正，让相同点在同一行线上。
（4）立体匹配，矫正完成的图像对，借助立体匹配算法进行立体匹配，生成视差图，同时可在这一步利用双目视差原理，计算出对应点距离摄像头的距离。

从上图我们利用三角形相似可以计算出Z的距离。


我们来看下图的一个系统架构：

整个系统就是按照此架构完成的，理解整个系统架构之后，现来获取双目视差图，此处就是一个立体匹配的过程，匹配算法有很多种，这里就不赘述每种算法的优缺点，大家可以自己查询，并且自己试一试实际的效果。我在本系统中使用的是SGBM算法，有点是效果比较好，但是缺点为速度慢。测试图使用OpenCV自带的标准测试图，可以在库里面自己找寻，基本路径是：D:\OpenCV3.1.0\opencv\sources\samples\data\aloeL.jpg 和 aloeR.jpg
立体匹配算法代码如下：

#include <highgui.h>
#include <cv.h>
#include <cxcore.h>
#include <iostream>
using namespace std;
using namespace cv;
int main()
{
	IplImage * img1 = cvLoadImage("D:\\VS2013\\left.png", 0);
	IplImage * img2 = cvLoadImage("D:\\VS2013\\right.png", 0);

	cv::StereoSGBM sgbm;
	int SADWindowSize = 9;
	sgbm.preFilterCap = 63;
	sgbm.SADWindowSize = SADWindowSize > 0 ? SADWindowSize : 3;
	int cn = img1->nChannels;
	int numberOfDisparities = 64;
	sgbm.P1 = 8 * cn*sgbm.SADWindowSize*sgbm.SADWindowSize;
	sgbm.P2 = 32 * cn*sgbm.SADWindowSize*sgbm.SADWindowSize;
	sgbm.minDisparity = 0;
	sgbm.numberOfDisparities = numberOfDisparities;
	sgbm.uniquenessRatio = 10;
	sgbm.speckleWindowSize = 100;
	sgbm.speckleRange = 32;
	sgbm.disp12MaxDiff = 1;
	Mat disp, disp8;
	int64 t = getTickCount();
	sgbm((Mat)img1, (Mat)img2, disp);
	t = getTickCount() - t;
	cout << "Time elapsed:" << t * 1000 / getTickFrequency() << endl;
	disp.convertTo(disp8, CV_8U, 255 / (numberOfDisparities*16.));
	namedWindow("left", 1);
	cvShowImage("left", img1);
	namedWindow("right", 1);
	cvShowImage("right", img2);
	namedWindow("disparity", 1);
	imshow("disparity", disp8);
	waitKey(0);
	//imwrite();
	cvDestroyAllWindows();
	return 0;
}

效果图：

视差图出来之后，我们的工作基本就完成了一大半了，这个时候，我们进行测距工作。由于没有摄像机标定这一环节，通过参考VS2017+OpenCV3.3基于SGBM算法的双目立体视觉、双目测距（双目校正和立体匹配）这篇博客，成功调试成功，附代码如下：

#include <opencv2/opencv.hpp>  
#include <iostream>  
#include <stdio.h>

using namespace std;
using namespace cv;

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

Mat rgbImageL, grayImageL;
Mat rgbImageR, grayImageR;
Mat rectifyImageL, rectifyImageR;

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

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

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


Ptr<StereoSGBM> sgbm = StereoSGBM::create(0, 16, 3);


//事先标定好的相机的参数
//fx 0 cx
//0 fy cy
//0 0  1

Mat cameraMatrixL = (Mat_<double>(3, 3) << 4334.09568, 0, 959.50000,
	0, 4334.09568, 511.50000,
	0, 0, 1.0);
Mat distCoeffL = (Mat_<double>(5, 1) << 0.0, 0.0, 0.0, 0.0, 0.0);

Mat cameraMatrixR = (Mat_<double>(3, 3) << 4489.55770, 0, 801.86552,
	0, 4507.13412, 530.72579,
	0, 0, 1.0);
Mat distCoeffR = (Mat_<double>(5, 1) << 0.0, 0.0, 0.0, 0.0, 0.0);

Mat T = (Mat_<double>(3, 1) << -518.97666, 01.20629, 9.14632);//T平移向量
Mat rec = (Mat_<double>(3, 1) << 0.04345, -0.05236, -0.01810);//rec旋转向量
Mat R;//R 旋转矩阵


static void saveXYZ(const char* filename, const Mat& mat)
{
	const double max_z = 16.0e4;
	FILE* fp = fopen(filename, "wt");
	printf("%d %d \n", mat.rows, mat.cols);
	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 GenerateFalseMap(cv::Mat &src, cv::Mat &disp)
{
	// color map  
	float max_val = 255.0f;
	float map[8][4] = { { 0, 0, 0, 114 }, { 0, 0, 1, 185 }, { 1, 0, 0, 114 }, { 1, 0, 1, 174 },
	{ 0, 1, 0, 114 }, { 0, 1, 1, 185 }, { 1, 1, 0, 114 }, { 1, 1, 1, 0 } };
	float sum = 0;
	for (int i = 0; i<8; i++)
		sum += map[i][3];

	float weights[8]; // relative   weights  
	float cumsum[8];  // cumulative weights  
	cumsum[0] = 0;
	for (int i = 0; i<7; i++) {
		weights[i] = sum / map[i][3];
		cumsum[i + 1] = cumsum[i] + map[i][3] / sum;
	}

	int height_ = src.rows;
	int width_ = src.cols;
	// for all pixels do  
	for (int v = 0; v<height_; v++) {
		for (int u = 0; u<width_; u++) {

			// get normalized value  
			float val = std::min(std::max(src.data[v*width_ + u] / max_val, 0.0f), 1.0f);

			// find bin  
			int i;
			for (i = 0; i<7; i++)
			if (val<cumsum[i + 1])
				break;

			// compute red/green/blue values  
			float   w = 1.0 - (val - cumsum[i])*weights[i];
			uchar r = (uchar)((w*map[i][0] + (1.0 - w)*map[i + 1][0]) * 255.0);
			uchar g = (uchar)((w*map[i][1] + (1.0 - w)*map[i + 1][1]) * 255.0);
			uchar b = (uchar)((w*map[i][2] + (1.0 - w)*map[i + 1][2]) * 255.0);
			//rgb内存连续存放  
			disp.data[v*width_ * 3 + 3 * u + 0] = b;
			disp.data[v*width_ * 3 + 3 * u + 1] = g;
			disp.data[v*width_ * 3 + 3 * u + 2] = r;
		}
	}
}

//立体匹配
void stereo_match(int, void*)
{
	sgbm->setPreFilterCap(63);
	int sgbmWinSize = 5;//根据实际情况自己设定
	int NumDisparities = 416;//根据实际情况自己设定
	int UniquenessRatio = 6;//根据实际情况自己设定
	sgbm->setBlockSize(sgbmWinSize);
	int cn = rectifyImageL.channels();

	sgbm->setP1(8 * cn*sgbmWinSize*sgbmWinSize);
	sgbm->setP2(32 * cn*sgbmWinSize*sgbmWinSize);
	sgbm->setMinDisparity(0);
	sgbm->setNumDisparities(NumDisparities);
	sgbm->setUniquenessRatio(UniquenessRatio);
	sgbm->setSpeckleWindowSize(100);
	sgbm->setSpeckleRange(10);
	sgbm->setDisp12MaxDiff(1);
	sgbm->setMode(StereoSGBM::MODE_SGBM);
	Mat disp, dispf, disp8;
	sgbm->compute(rectifyImageL, rectifyImageR, disp);
	//去黑边
	Mat img1p, img2p;
	copyMakeBorder(rectifyImageL, img1p, 0, 0, NumDisparities, 0, IPL_BORDER_REPLICATE);
	copyMakeBorder(rectifyImageR, img2p, 0, 0, NumDisparities, 0, IPL_BORDER_REPLICATE);
	dispf = disp.colRange(NumDisparities, img2p.cols - NumDisparities);

	dispf.convertTo(disp8, CV_8U, 255 / (NumDisparities *16.));
	reprojectImageTo3D(dispf, xyz, Q, true); //在实际求距离时，ReprojectTo3D出来的X / W, Y / W, Z / W都要乘以16(也就是W除以16)，才能得到正确的三维坐标信息。
	xyz = xyz * 16;
	imshow("disparity", disp8);
	imwrite("disp.jpg", disp8);//保存灰值视差图
	Mat color(dispf.size(), CV_8UC3);
	GenerateFalseMap(disp8, color);//转成彩图
	imshow("disparity", color);
	imwrite("dispcolor.jpg",color);//保存彩色视差图
	saveXYZ("xyz.xls", xyz);
}



//描述：鼠标操作回调*
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;
		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_16SC2, mapLx, mapLy);
	initUndistortRectifyMap(cameraMatrixR, distCoeffR, Rr, Pr, imageSize, CV_16SC2, mapRx, mapRy);

	//  读取图片    
	rgbImageL = imread("E://WorkImage//left_cor.bmp", CV_LOAD_IMAGE_COLOR);//CV_LOAD_IMAGE_COLOR
	rgbImageR = imread("E://WorkImage//right_cor.bmp", -1);


	//  经过remap之后，左右相机的图像已经共面并且行对准了
	remap(rgbImageL, rectifyImageL, mapLx, mapLy, INTER_LINEAR);//INTER_LINEAR
	remap(rgbImageR, rectifyImageR, mapRx, mapRy, INTER_LINEAR);

	//  把校正结果显示出来

	//显示在同一张图上
	Mat canvas;
	double sf;
	int w, h;
	sf = 700. / 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);
	//保存矫正后的图片
	imwrite("rectified.jpg", canvas);

	//  立体匹配    
	namedWindow("disparity", CV_WINDOW_NORMAL);
	//鼠标响应函数setMouseCallback(窗口名称, 鼠标回调函数, 传给回调函数的参数，一般取0)
	setMouseCallback("disparity", onMouse, 0);//disparity
	stereo_match(0, 0);

	waitKey(0);
	return 0;
	
}

效果图：

光标放在需要测量的物体上，右边命令行窗口出现三维坐标，Z坐标既是距离。

附上我的课设论文：点击下载

【点击下方图片直达☟】