双目测距原理与基于opencv的简单实现

双目测距基本原理：

如图，P是待测物体上的某一点，OR与OT分别是两个相机的光心，点P在两个相机感光器上的成像点分别为P和P’（相机的成像平面经过旋转后放在了镜头前方），

f为相机焦距，B为两相机中心距，Z为我们想求得的深度信息，设点P到点P’的距离为dis，则：

 

根据相似三角形原理：

可得：

 

公式中，焦距f和摄像头中心距B可通过标定得到，因此，只要获得了（即，视差d）的值即可求得深度信息。

 

双目测距实际操作分4个步骤：相机标定——双目校正——双目匹配——计算深度信息。

 

相机标定：摄像头由于光学透镜的特性使得成像存在着径向畸变，可由三个参数k1,k2,k3确定；由于装配方面的误差，传感器与光学镜头之间并非完全平行，因此成像存在切向畸变，可由两个参数p1,p2确定。单个摄像头的定标主要是计算出摄像头的内参（焦距f和成像原点cx,cy、五个畸变参数（一般只需要计算出k1,k2,p1,p2，对于鱼眼镜头等径向畸变特别大的才需要计算k3））以及外参（标定物的世界坐标）。而双目摄像头定标不仅要得出每个摄像头的内部参数，还需要通过标定来测量两个摄像头之间的相对位置（即右摄像头相对于左摄像头的旋转矩阵R、平移向量t）。

 

双目校正：双目校正是根据摄像头定标后获得的单目内参数据（焦距、成像原点、畸变系数）和双目相对位置关系（旋转矩阵和平移向量），分别对左右视图进行消除畸变和行对准，使得左右视图的成像原点坐标一致（CV_CALIB_ZERO_DISPARITY标志位设置时发生作用）、两摄像头光轴平行、左右成像平面共面、对极线行对齐。这样一幅图像上任意一点与其在另一幅图像上的对应点就必然具有相同的行号，只需在该行进行一维搜索即可匹配到对应点。

 

双目匹配：双目匹配的作用是把同一场景在左右视图上对应的像点匹配起来，这样做的目的是为了得到视差图。双目匹配被普遍认为是立体视觉中最困难也是最关键的问题。得到视差数据，通过上述原理中的公式就可以很容易的计算出深度信息。

 

下面是简单的测距代码，代码中没有相机的标定部分，因为标定数据是之前测好的所以直接拿来用了。

 

#include <opencv2\opencv.hpp>  
#include<iostream>
#include <fstream>

using namespace cv;
using namespace std;
//f is 1383.144277、B is 0.21087920785  Z = f * B /d.  f * B = 291.67636947602097445
void mouseHandler(int event, int x, int y, int flags, void* param);
CvMat *disp;  //disparity map
bool left_mouse = false;

void main()
{

		
	IplImage* imageLeft;
	IplImage* imageRight;
	imageLeft = cvLoadImage("image001.pgm"); //读入左侧相机的图像，3通道的
	imageRight = cvLoadImage("image010.pgm");//读入右侧相机的图像  
	//int d = imageLeft->depth;
	int width = imageLeft->width;//图片宽度
	int height = imageLeft->height;//图片高度
	
    //这里是根据图像位数进行灰度值调整，不是必须的，可根据实际情况自行删减
	cvScale(imageLeft,imageLeft,16, 0);  
	cvScale(imageRight,imageRight,16, 0);
    //初始化相机的标定数据，由于不同的相机标定参数不同，所以这里用XX代替
	double M1[3][3]={xx.x00000,xx.000000,xx.xxx000,xx.000000,xx.xx0000,xx.xxx000,xx.000000,xx.000000,xx.000000};
	double M2[3][3]={xx.x00000,xx.000000,xx.xxx000,xx.000000,xx.xx0000,xx.xxx000,xx.000000,xx.000000,xx.000000};
	double D1[5]={x.xxx, x.xxx0, xx.xxx,x.xxx, x.xxx};
	double D2[5]={x.xxx, x.xxx0, xx.xxx,x.xxx, x.xxx};
	CvMat _M1calib = cvMat(3, 3, CV_64F, M1 );
    CvMat _M2calib = cvMat(3, 3, CV_64F, M2 );
    CvMat _D1      = cvMat(1, 5, CV_64F, D1 );
    CvMat _D2      = cvMat(1, 5, CV_64F, D2 );

	double R1[3][3]={xx.x00000,xx.000000,xx.xxx000,xx.000000,xx.xx0000,xx.xxx000,xx.000000,xx.000000,xx.000000};
	double R2[3][3]={xx.x00000,xx.000000,xx.xxx000,xx.000000,xx.xx0000,xx.xxx000,xx.000000,xx.000000,xx.000000};
	double P1[3][4]={xx.x00000,xx.000000,xx.xxx000,xx.000000,xx.xx0000,xx.xxx000,xx.000000,xx.000000,xx.000000,xx.000000,xx.000000,xx.000000};
	double P2[3][4]={xx.x00000,xx.000000,xx.xxx000,xx.000000,xx.xx0000,xx.xxx000,xx.000000,xx.000000,xx.000000,xx.000000,xx.000000,xx.000000};
    CvMat _R1 = cvMat(3, 3, CV_64F, R1);
    CvMat _R2 = cvMat(3, 3, CV_64F, R2);
    CvMat _P1 = cvMat(3, 4, CV_64F, P1);
    CvMat _P2 = cvMat(3, 4, CV_64F, P2); 
	
	CvMat* mx1calib = cvCreateMat( height,width, CV_32F );
    CvMat* my1calib = cvCreateMat( height,width, CV_32F );
    CvMat* mx2calib = cvCreateMat( height,width, CV_32F );
    CvMat* my2calib = cvCreateMat( height,width, CV_32F );
    		//双目校正，分别得到左右两个相机的X坐标重映射矩阵和Y坐标重映射矩阵
	cvInitUndistortRectifyMap(&_M1calib,&_D1,&_R1,&_P1,mx1calib,my1calib);
    cvInitUndistortRectifyMap(&_M2calib,&_D2,&_R2,&_P2,mx2calib,my2calib);
	
	
	CvMat	*img1r,		//rectified left image
			*img2r,		//rectified right image		
			*vdisp,		//scaled disparity map for viewing
			*pair,
			*depthM;
	img1r = cvCreateMat( height,width, CV_8U );		//rectified left image
	img2r = cvCreateMat( height,width, CV_8U );		//rectified right image
	disp  = cvCreateMat( height,width, CV_16S );		//disparity map
	vdisp = cvCreateMat( height,width, CV_8U );
	CvStereoBMState *BMState = cvCreateStereoBMState(); 
	assert(BMState != 0);
	//BMState->preFilterSize = 63;//stereoPreFilterSize; 
	//BMState->preFilterCap = 63;//stereoPreFilterCap;
	BMState->SADWindowSize = 15;// stereoDispWindowSize; //33
	BMState->minDisparity = 0;
	BMState->numberOfDisparities = 48;//stereoNumDisparities; //48
	BMState->textureThreshold = 20;//stereoDispTextureThreshold; //20
	BMState->uniquenessRatio = 15;///stereoDispUniquenessRatio;//15
	BMState->speckleWindowSize = 200;
	BMState->speckleRange = 32;
	BMState->disp12MaxDiff = 2;
	
	IplImage	*img1,	//left image
				*img2;

	img1 = cvCreateImage(cvSize(width,height), IPL_DEPTH_8U, 1);		
	img2 = cvCreateImage(cvSize(width,height), IPL_DEPTH_8U, 1);
	cvCvtColor(imageLeft, img1, CV_BGR2GRAY);
	cvCvtColor(imageRight, img2, CV_BGR2GRAY);
	
	cvRemap( img1, img1r, mx1calib, my1calib );
	cvRemap( img2, img2r, mx2calib, my2calib );
	
	cvFindStereoCorrespondenceBM(img1r, img2r, disp, BMState);
	cvNormalize( disp, vdisp, 0, 256, CV_MINMAX );

	
	//cvNamedWindow( "Rectified", 1);
	//cvNamedWindow( "uDisparity Map",0 );
	cvNamedWindow( "Disparity Map",0 );
	//cvShowImage("Rectified", pair);
	//cvShowImage("uDisparity Map", disp);
	cvShowImage("Disparity Map", vdisp);
	cvSetMouseCallback("Disparity Map", mouseHandler, NULL);
	cvNamedWindow("left",0);
	//cvNamedWindow("right",0);

	cvShowImage("left",imageLeft);
	//cvShowImage("right",imageRight);
	
	cvWaitKey(0);

	cvDestroyWindow("Rectified");
	cvDestroyWindow("Disparity Map");
	cvDestroyWindow("left");
	//cvDestroyWindow("right");
	cvReleaseImage(&imageLeft);
	cvReleaseImage(&imageRight);
}

void mouseHandler(int event, int x, int y, int flags, void *param){

	if (event == CV_EVENT_LBUTTONDOWN)
	{
		cout << "x:" << x<< "y:" << y << endl;
		//l = cvGet2D(stereoFunc->depthM, x, y);
		CvScalar s = cvGet2D( disp, y, x );
		double dep1 = s.val[0];
		double dep = 291.67636947602097445/dep1;
		dep *= 16;
		//int dep2 = cvmGet( vdisp, 1000, 500 );
		//printf("dep1 = %d\n",dep1);
		printf("Distance to this object is: %f m \n", dep);
		left_mouse = true;

	}
	else if (event == CV_EVENT_LBUTTONUP)
	{
		left_mouse = false;
	}
	else if ((event == CV_EVENT_MOUSEMOVE) && (left_mouse == true))
	{
	}
}

最后得到的视差图：

 

测距结果：（实际距离8.521m）