双目规则物体测量

这是一个基于双目立体视觉做的规则物体测量的demo，实现了对规则物体尺寸测量的工程，精度在2mm左右。

#define _CRT_SECURE_NO_WARNINGS

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

using namespace cv;
using namespace std;

//int num = 50 size = 3;

//从TXT中读取参数至mat
bool read_file_to_matrix（string fileName Mat& matrix）
{
	ifstream file（fileName.c_str（））;
	if （!file）
	{
		cerr << “error unable to open input file: “ << fileName << endl;
		return -1;//read failed
	}

	for （int i = 0; i		for （int j = 0; j			file >> matrix.at（i j）;
		}

	return 0;
}

Size imagesize = Size（320 240）;
void init（Mat& Q Mat rmap[2][2]）
{
	Mat ML（3 3 CV_64F） DL（1 5 CV_64F）;
	Mat MR（3 3 CV_64F） DR（1 5 CV_64F）;
	Mat R（3 3 CV_64F） T（3 1 CV_64F） E（3 3 CV_64F） F（3 3 CV_64F）;

	read_file_to_matrix（“ML.txt“ ML）;
	read_file_to_matrix（“DL.txt“ DL）;
	read_file_to_matrix（“MR.txt“ MR）;
	read_file_to_matrix（“DR.txt“ DR）;
	read_file_to_matrix（“R.txt“ R）;
	read_file_to_matrix（“T.txt“ T）;

	//Bouguet校正
	Mat R1 R2 P1 P2;
	Rect roi1 roi2;
	// 双目校正，计算R1 R2 P1 P2 roi1 roi2
	stereoRectify（ML DL MR DR imagesize R T R1 R2 P1 P2 Q
		CALIB_ZERO_DISPARITY 0 imagesize &roi1 &roi2）;
	//计算映射表
	initUndistortRectifyMap（ML DL R1 P1 imagesize CV_16SC2 rmap[0][0] rmap[0][1]）;
	initUndistortRectifyMap（MR DR R2 P2 imagesize CV_16SC2 rmap[1][0] rmap[1][1]）;
}

void oncontrol（int void*）
{

}

Point cal_intersection（Vec2f line1 Vec2f line2）
{
	Point point（0 0）;
	float r1（line1[0]） the1（line1[1]） r2（line2[0]） the2（line2[1]）;
	point.x = r1 * sin（the2） - r2 * sin（the1）;
	point.x = point.x / sin（the2 - the1）;
	point.y = r1 * cos（the2） - r2 * cos（the1）;
	point.y = point.y / sin（the1 - the2）;

	return point;
}

vector tmeasure（Point pointlPoint pointrMat Q vector& points）
{
	double disparity = pointl.x - pointr.x;
	double baseline = 1.0/Q.ptr（3）[2];
	double focal = Q.ptr（2）[3];
	//cout << disparity<<““ << baseline<<““ << focal;
	double Z = baseline*focal / disparity;
	double X = Z*pointr.x / focal;
	double Y = Z*pointr.y / focal;
	cout << X << ““ << Y << ““ << Z << endl;
	points.push_back（X）;
	points.push_back（Y）;
	points.push_back（Z）;
	return points;
}

double cal_distance（vector& wp1 vector& wp2）
{
	double dis = 0;
	for （int i = 0; i<3; i++）
		dis += pow（wp1[i] - wp2[i] 2）;
	dis = sqrt（dis）;
	return dis;
}

int main（）
{
	Mat Q rmap[2][2];
	init（Q rmap）; 
	//int low = 50high=100;
	int thre = 80;

	/*char trackbarname[50];
	namedWindow（“bars“WINDOW_AUTOSIZE）;
	sprintf（trackbarname “thre“ thre）;
	createTrackbar（“thre“ “bars“ &thre 255 oncontrol）;*/
	/*sprintf（trackbarname “high“ high）;
	createTrackbar（“low“ “bars“ &high 255 oncontrol）;*/
	Mat l = imread（

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件     157696  2017-03-24 20:48  规则物体测量\Debug\规则物体测量.exe

     文件     851396  2017-03-24 20:48  规则物体测量\Debug\规则物体测量.ilk

     文件    3321856  2017-03-24 20:48  规则物体测量\Debug\规则物体测量.pdb

     文件     114176  2017-04-01 21:55  规则物体测量\Debug\规则物体测量Srv.exe

     文件      41472  2017-03-16 09:53  规则物体测量\Release\规则物体测量.exe

     文件    1323008  2017-03-16 09:53  规则物体测量\Release\规则物体测量.pdb

     文件     550841  2017-03-24 20:48  规则物体测量\规则物体测量\Debug\materialmeasure.obj

     文件     658432  2017-03-24 20:48  规则物体测量\规则物体测量\Debug\vc120.idb

     文件    1355776  2017-03-24 20:48  规则物体测量\规则物体测量\Debug\vc120.pdb

     文件       2877  2017-03-24 20:48  规则物体测量\规则物体测量\Debug\规则物体测量.log

     文件        506  2017-03-24 20:48  规则物体测量\规则物体测量\Debug\规则物体测量.tlog\cl.command.1.tlog

     文件      13064  2017-03-24 20:48  规则物体测量\规则物体测量\Debug\规则物体测量.tlog\CL.read.1.tlog

     文件        304  2017-03-24 20:48  规则物体测量\规则物体测量\Debug\规则物体测量.tlog\CL.write.1.tlog

     文件       2600  2017-03-24 20:48  规则物体测量\规则物体测量\Debug\规则物体测量.tlog\link.command.1.tlog

     文件       7142  2017-03-24 20:48  规则物体测量\规则物体测量\Debug\规则物体测量.tlog\link.read.1.tlog

     文件        260  2017-03-24 20:48  规则物体测量\规则物体测量\Debug\规则物体测量.tlog\link.write.1.tlog

     文件        155  2017-03-24 20:48  规则物体测量\规则物体测量\Debug\规则物体测量.tlog\规则物体测量.lastbuildstate

     文件         54  2017-03-13 19:58  规则物体测量\规则物体测量\DL.txt

     文件         52  2017-03-13 19:58  规则物体测量\规则物体测量\DR.txt

     文件         88  2017-03-13 19:58  规则物体测量\规则物体测量\E.txt

     文件         98  2017-03-13 19:58  规则物体测量\规则物体测量\F.txt

     文件      13459  2017-03-13 19:54  规则物体测量\规则物体测量\ll2.jpg

     文件      13487  2017-03-13 19:54  规则物体测量\规则物体测量\ll3.jpg

     文件      13338  2017-03-13 19:54  规则物体测量\规则物体测量\ll4.jpg

     文件       8369  2017-04-01 21:58  规则物体测量\规则物体测量\materialmeasure.cpp

     文件         44  2017-03-13 19:58  规则物体测量\规则物体测量\ML.txt

     文件         44  2017-03-13 19:58  规则物体测量\规则物体测量\MR.txt

     文件         69  2017-03-13 19:58  规则物体测量\规则物体测量\Q.txt

     文件        100  2017-03-13 19:58  规则物体测量\规则物体测量\R.txt

     文件    2125019  2017-03-16 09:53  规则物体测量\规则物体测量\Release\materialmeasure.obj

............此处省略30个文件信息