【OpenCV】基于SIFT/SURF算法的双目视差测距（二）

上一篇链接： https://blog.csdn.net/qinchang1/article/details/86934636
上篇主要介绍了：
1.本次双目视差测距的 原理；
2.如何基于SIFT和SURF算法获得 视差估计。

原理回顾

双目摄像头成像满足这个反比例关系。

其中，
1.?(mm)为摄像头投影中心到物体的距离；
2.?(像素)为视差，与?₁−?₂ 成比例关系；
3.?(mm*像素)为常数。

实验步骤：根据这个关系，用已知距离Z和其视差D，计算出摄像头的K值，再使用这个K值去反推之后的测距结果。

计算K值

如前所述，K值是一个常数，是双目摄像头固有的属性，现在以已知目标的距离Z和视差D来计算双目摄像头的固有属性。
已知目标我选取的是识别精度较高的棋盘角点。

棋盘角点数是5*7=35个，使用双目摄像头拍摄了距离从150-1000mm，每隔50mm距离的18张棋盘左右图像。

由于距离跨度较大，统一尺寸的棋盘可能会在近的地方显示不全，远的地方显示模糊，因此准备了大中小三个尺寸的棋盘。
35个棋盘角点左右图像视差均值作为该距离下的棋盘视差D。
每个角点视差值和视差均值输出如下图：


你会很明显地发现，从850mm距离开始，视差值甚至出现了负值。
这是由于在距离较远处的视差值太小导致的，这时候视差值也许小于本身摄像头拍摄的误差，本应该是左图像X坐标大于右图像的，由于两个值太接近，去掉各自的误差后可能会出现右图像X坐标比左图像还大的情况。
从这我们也可以发现双目视差测距的一个明显的缺陷：距离较远的地方很难测量， 若非要测量较远的地方，必须：
1.增加两摄像头的间距；
2.换精度更高的摄像头。

由于距离较远的地方视差值相对误差较大，因此数据分析时去掉800mm以后的数据。
考虑到Z理论值是摄像头成像中心到物体的距离，而实际测量距离未必准确，因此增加误差距离e。

将其转换为求解Z为因变量，1/D为自变量的一次函数，对其进行线性回归。

之后用MATLAB处理实验数据：

这个结果比较令我意外，理论上应该满足视差和距离成反比例关系，但是实际结果和理论却有很大的出入。
考虑到距离较远的地方视差相对误差较大，因此我去掉了600mm以后的部分：

这样稍微好了一些，但是说实话，我觉得这个结果和理想结果相差较大，但我不能想通其原因，考虑到可能是实验前没有进行双目摄像头标定，其他原因暂时也没能想到，欢迎补充。
但是从图中右半部分距离Z和视差D的曲线来看，确实有那么一点接近反比例函数曲线的意思。

多项式拟合

受实验条件的限制，我只能根据这个结果，通过多项式拟合的方式来获得距离Z和视差D的曲线关系。

首先用二次多项式拟合，但是效果不是特别好，偏差也比较大，因此考虑用三次多项式继续拟合，效果勉勉强强。

最后甚至试了四次多项式和五次多项式拟合，其实多余的也就是拟合了距离较近（也就是视差较大）的部分，这部分本身由于距离太近，很小的距离变化就会造成很大的视差变化，因此实际测量的结果会有很大的偏差，参考价值不大，因此去掉200mm以内的数据。

测距实验

最后我决定使用三次多项式拟合的结果进行视差测距。
实验中测量麦片盒子距摄像头的距离，匹配特征点旁显示该特征点的距离，单位mm。
在实验中，我通过调整SURF或SIFT算法的参数，使得特征点基本上集中在目标物体上。
300mm的效果：

400mm的效果：

500mm的效果：

600mm的效果：

700mm的效果：
下面有两张白天拍摄的图像：
350mm：

450mm:

从结果上看，基本上大多数识别点误差都能保持在10mm（1cm）以内。

实验代码

棋盘角点视差获取

#include 
#include 
#include 
#include 
#include 
#include //important
#include 
#include 
#include 

using namespace cv;
using namespace std;

struct parallax
{
	double value;
	double length;
};

int main()
{
	Mat frame;
	double l0 = 150;//the first length
	int n = 18;//number of pictures
	double t = 50;//step size
	Size boardSize(7, 5);//size of corners
	int boardNum = boardSize.width * boardSize.height;//number of  corners
	String s1 = ".jpg";//file suffix
	vector<parallax> pa;

	for (int i = 0; i < n; i++)
	{
		double l = l0 + i*t;
		char num[4];
		sprintf(num, "%03d", int(l/10));
		String s0 = num;
		String s = s0 + s1;
		cout << endl <<"图片"<< s << endl;
		frame = imread(s);
		resize(frame, frame, Size(1280, 480));//set size of image

		Mat leftImage, rightImage;
		leftImage = frame(Rect(0, 0, frame.size().width / 2, frame.size().height));//split left image
		rightImage = frame(Rect(frame.size().width / 2, 0, frame.size().width / 2, frame.size().height));//split right image
		Mat leftGray, rightGray;
		cvtColor(leftImage, leftGray, CV_BGR2GRAY);
		cvtColor(rightImage, rightGray, CV_BGR2GRAY);

		//////find chess board corner///////
		vector<Point2f>leftCorners, rightCorners;
		bool leftResult = findChessboardCorners(leftGray, boardSize, leftCorners, CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE);//find chess board corners
		bool rightResult = findChessboardCorners(rightGray, boardSize, rightCorners, CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE);
		TermCriteria criteria = TermCriteria(TermCriteria::MAX_ITER + TermCriteria::EPS,30,0.1);
		cornerSubPix(leftGray, leftCorners, Size(11, 11), Size(-1, -1), criteria);//find sub-pixel for corners
		cornerSubPix(rightGray, rightCorners, Size(11, 11), Size(-1, -1), criteria);
		drawChessboardCorners(leftImage, boardSize, leftCorners, leftResult);//draw the corners
		drawChessboardCorners(rightImage, boardSize, rightCorners, rightResult);
		String leftWin = s0 + "leftImage";//window name
		String rightWin = s0 + "rightImage";
		imshow(leftWin, leftImage);//left image
		imshow(rightWin, rightImage);//right image

		///////calculate parallax///////
		double sum = 0;
		double mean;
		for (int j = 0; j < boardNum; j++)
		{
			double temp = leftCorners[j].x - rightCorners[j].x;
			sum = sum + temp;
			cout << temp << "\t";
			if (j % 12 == 11)
				cout << endl;
		}
		mean = sum / boardNum;
		cout << "均值：" << mean << endl;
		parallax temp;
		temp.value = mean;
		temp.length = l;
		pa.push_back(temp);
	}

	//display parallx value
	cout << endl;
	for (int i = 0; i < pa.size(); i++)
	{
		cout << "距离Z：\t" << pa[i].length << "\t视差D：\t" << pa[i].value << "\t 1/D：\t" << 1 / pa[i].value << "\t K:" << pa[i].length*pa[i].value << endl;
	}

	waitKey(0);
	return 0;
}

实验数据处理（MATLAB）

clear
x0=[];%D(Parallax)
x=[];%1/D(Parallax reciprocal)
y=[];%Z(Distance)
[P,S]=polyfit(x,y,1);%Linear or multiple function function fitting
[Y,delta]=polyconf(P,x,S);
x1=0:0.001:0.08;
 f=polyval(P,x1);
 figure(1); 
 plot(x,y,'ro',x1,f,'-')
 ylabel('距离/mm');
 xlabel('视差倒数/(1/pixel)');
 hold on;
 grid on
 plot(x,y,'r');
 plot(x,Y+delta,'*g');
 plot(x,Y-delta,'*g');%confidence interval
 legend('数据点','拟合线','原始线','置信区间');
 y0=P(1)*x+P(2);
 figure(2); 
 plot(x0,y);
 ylabel('距离/mm');
 xlabel('视差/pixel');
 hold on;
 grid on
 plot(x0,y0,'r');
 legend('原始','拟合');

视差测距试验

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

using namespace cv;
using namespace std;

const double p0 = 885.3237;
const double p1 = -20.60308;
const double p2 = 0.2518267;
const double p3 = -0.001105757;


class parallax
{
public:
	double leftX;
	double rightX;
	double paraValue;
	double distance;
};

int main()
{
	Mat frame = imread("45cm.jpg");//image path

	Mat leftImg, rightImg;
	leftImg = frame(Rect(0, 0, frame.size().width / 2, frame.size().height));//split left image
	rightImg = frame(Rect(frame.size().width / 2, 0, frame.size().width / 2, frame.size().height));//split right image
	//imshow("limg", leftImg);
	//imshow("rimg", rightImg);

	int minhessian = 5000;//threshold of hessian in SIFT or SURF algorithm 
	vector<KeyPoint>l_keyPoint, r_keyPoint;
	Mat l_descriptor, r_descriptor;

	SurfFeatureDetector detector(minhessian);//define a feature detection class object
	detector.detect(leftImg, l_keyPoint);
	detector.detect(rightImg, r_keyPoint);

	//compute descriptor (feature vector) of key points
	SurfDescriptorExtractor extractor;
	extractor.compute(leftImg, l_keyPoint, l_descriptor);
	extractor.compute(rightImg, r_keyPoint, r_descriptor);

	//FLANN algorithm to match feature vector
	FlannBasedMatcher matcher;
	vector<DMatch>matches;
	matcher.match(l_descriptor, r_descriptor, matches);

	//calculate the max and min distance between key points
	double maxdist = 0; double mindist = 100;
	for (int i = 0; i < l_descriptor.rows; i++)
	{
		double dist = matches[i].distance;
		if (dist < mindist)mindist = dist;
		if (dist > maxdist)maxdist = dist;
	}
	cout << "Matching quantity:" << matches.size() << endl;

	//select the good match points
	vector<DMatch>goodMatches;
	for (int i = 0; i < l_descriptor.rows; i++)
	{
		if (matches[i].distance<1.5 * mindist)
		{
			goodMatches.push_back(matches[i]);
		}
	}
	cout << "Good matching quantity:" << goodMatches.size() << endl;

	//calculate parallax
	vector<parallax>para;
	for (int i = 0; i < goodMatches.size(); i++)
	{
		parallax temp;
		temp.leftX = l_keyPoint[goodMatches[i].queryIdx].pt.x;
		temp.rightX = r_keyPoint[goodMatches[i].trainIdx].pt.x;
		temp.paraValue = temp.leftX - temp.rightX;
		temp.distance =  p3 * pow(temp.paraValue, 3) + p2 * pow(temp.paraValue, 2) + p1 * temp.paraValue + p0;
		para.push_back(temp);
		cout << "No." << i + 1 << ":\t l_X ";
		cout << para[i].leftX << "\t r_X " << para[i].rightX;
		cout << "\t parallax " << para[i].paraValue <<"\t distance "<<para[i].distance<< endl;
	}

	vector<parallax>::iterator it;
	for (it = para.begin(); it != para.end();)
	{
		if (it->paraValue<0)
			it = para.erase(it);
		else
			it++;
	}
	cout << "Final data..." << endl;

	//tag the distance text
	for (int i = 0; i < para.size(); i++)
	{
		cout << "No." << i << "\t" << para[i].paraValue << "\t distance " << para[i].distance << endl;
		char str[20];
		sprintf(str, "%.3f", para[i].distance);
		string text = str;
		Point org(l_keyPoint[goodMatches[i].queryIdx].pt.x, l_keyPoint[goodMatches[i].queryIdx].pt.y);
		putText(leftImg, text, org, 2, 0.5, Scalar(0, 255, 255), 1, 8, 0);
	}
	
	//draw the match image
	Mat matchImg;
	drawMatches(leftImg, l_keyPoint, rightImg, r_keyPoint, goodMatches, matchImg,
		Scalar::all(-1), Scalar::all(-1), vector<char>(), DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS);
	imshow("match", matchImg);

	waitKey(0);
	return 0;
}

如有错误，欢迎指正！