opencv-python双目测距

双目测距原理

下图是双目摄像头成像示意图，其中P点为目标物体，O1和O2分别为左右摄像头的投影中心，x1和x2分别是目标物体在左右摄像头成像平面上的坐标，f为焦距，T为左右摄像头中心线距离，Z为摄像头投影中心到目标物体的距离。

三维的坐标测距：

通过计算相似三角形我们可以得到：

程序效果

sift算法
匹配效果：

匹配点视差数据（显示不全）：

#include <opencv2/core/core.hpp>  
#include <opencv2/highgui/highgui.hpp>  
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/nonfree/nonfree.hpp>
#include <vector> 
#include <iostream>
#include <algorithm>

using namespace cv;
using namespace std;

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

bool ascendPara(parallax a, parallax b)
{
	return a.paraValue < b.paraValue;
}

int main()
{
	Mat leftImg, rightImg;
	leftImg = imread("limg.jpg");
	rightImg = imread("rimg.jpg");
	imshow("limg", leftImg);
	imshow("rimg", rightImg);

	int minhessian = 1000;//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<2 * 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;
		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 << endl;
	}
	sort(para.begin(), para.end(), ascendPara);
	int idxMedian = int(para.size()/2);
	double paraMedian = para[idxMedian].paraValue;
	vector<parallax>::iterator it;
	double errorRange = 0.005;
	for (it=para.begin(); it!=para.end(); )
	{
		if (it->paraValue<((1 - errorRange)*paraMedian) || it->paraValue>((1 + errorRange)*paraMedian))
			it = para.erase(it);
		else
			it++;
	}
	cout << "Final data..." << endl;
	double paraSum = 0;
	double paraMean;
	for (int i = 0; i < para.size(); i++)
	{
		paraSum = paraSum + para[i].paraValue;
		cout << "No." << i << "\t" << para[i].paraValue << endl;
	}
	paraMean = paraSum / para.size();
	cout << "Parallax is " << paraMean << " pixel." << endl;

	//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;
}

python版本的例子
python 双目测距

有比我写的更好的博客 添加链接描述