function.cpp

基于OpenCV的计算机视觉技术实现.rar

#include "stdafx.h"

#include "cv.h"
#include "cxcore.h"
#include "highgui.h"
#include <iostream>
#include <cmath>
#include <vector>
#include <algorithm>
using namespace std;

#define IMAGE_HEIGHT 288
#define IMAGE_WIDTH 384
#define SIZE 19

/********************************************************************
	calculate fundamental matrix
*********************************************************************/
void find_F(double F_data[3][3],double M1_data[3][4],double M2_data[3][4]) {
	CvMat F=cvMat(3,3,CV_64FC1,F_data);
	double M11_data[3][3]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
	double M21_data[3][3]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
	double mx_data[3][3]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
	double m1_data[3]={0.0,0.0,0.0};
	double m2_data[3]={0.0,0.0,0.0};
	double m_data[3]={0.0,0.0,0.0};
	CvMat M11=cvMat(3,3,CV_64FC1,M11_data);
	CvMat M21=cvMat(3,3,CV_64FC1,M21_data);
	CvMat mx=cvMat(3,3,CV_64FC1,mx_data);
	CvMat m1=cvMat(3,1,CV_64FC1,m1_data);
	CvMat m2=cvMat(3,1,CV_64FC1,m2_data);
	CvMat m=cvMat(3,1,CV_64FC1,m_data);

	int i,j;
	for (i=0;i<3;i++) {
		for (j=0;j<3;j++) {
			M11_data[i][j]=M1_data[i][j];
			M21_data[i][j]=M2_data[i][j];
		}
		m1_data[i]=M1_data[i][3];
		m2_data[i]=M2_data[i][3];
	}

	cvInvert(&M11,&M11);

	cvMatMul(&M21,&M11,&M21);
	cvMatMul(&M21,&m1,&m1);
	for (i=0;i<3;i++) {
		m1_data[i]=-m1_data[i];
	}
	cvAdd(&m2,&m1,&m);
	mx_data[0][1]=-m_data[2];
	mx_data[0][2]=m_data[1];
	mx_data[1][0]=m_data[2];
	mx_data[1][2]=-m_data[0];
	mx_data[2][0]=-m_data[1];
	mx_data[2][1]=m_data[0];
	cvMatMul(&mx,&M21,&F);
		for(i=0;i<3;i++) {
		for(j=0;j<3;j++) {
			cout<<F_data[i][j]<<"  ";
		}
		cout<<endl;
	}
	cout<<endl;
}

/********************************************************************
	 find the match point
*********************************************************************/
bool match(double point_src[2],double point_dst[2],
			IplImage* img_src,IplImage* img_dst,double F_matrix[3][3],
			double colors[3],IplImage* img_temp) {
	
	if (point_src[0]<=SIZE/2 || point_src[1]<=SIZE/2
		|| point_src[0]>=IMAGE_WIDTH-SIZE/2 || point_src[1]>=IMAGE_HEIGHT-SIZE/2) {
		return false;
	}
	double point1_data[3]={point_src[0],point_src[1],1.0};
	CvMat point1=cvMat(3,1,CV_64FC1,point1_data);
	double line_data[3]={0.0,0.0,0.0};
	CvMat line=cvMat(3,1,CV_64FC1,line_data);
	CvMat F=cvMat(3,3,CV_64FC1,F_matrix);

	//计算极线方程
	cvMatMul(&F,&point1,&line);
	double cos_value=-1;
	double temp_cos=-1;

	//建立源向量
	double vec_src[SIZE*SIZE*3];
	int num=0;
	
	for (int i=-SIZE/2;i<=SIZE/2;i++) {
		for (int j=-SIZE/2;j<=SIZE/2;j++) {
			uchar* temp=&((uchar*)(img_src->imageData + img_src->widthStep*((int)point1_data[1]+i)))[((int)point1_data[0]+j)*3];
			vec_src[num++]=temp[0];  //blue
			vec_src[num++]=temp[1];  //green
			vec_src[num++]=temp[2];  //red
		}
	}

	//遍历极线
	double dx,dy;
	double x=-line_data[2]/line_data[0],y=-line_data[2]/line_data[1];
	if (fabs(line_data[0])<0.001) {
		x=SIZE/2+1;
		dx=1.0;
		dy=0.0;
	}
	else if (fabs(line_data[1])<0.001) {
		y=SIZE/2+1;
		dx=0.0;
		dy=1.0;
	}
	else if (fabs(line_data[0]/line_data[1])<1.0) {
		x=SIZE/2+1;
		y=(-line_data[2]-x*line_data[0])/line_data[1];
		dx=1.0;
		dy=-line_data[0]/line_data[1];
		while (x<=SIZE/2 || x>=IMAGE_WIDTH-SIZE/2 || y<=SIZE/2 || y>=IMAGE_HEIGHT-SIZE/2) {
			x+=dx;
			y+=dy;
		}
	}
	else {
		x=SIZE/2+1;
		y=(-line_data[2]-x*line_data[0])/line_data[1];
		dx=-line_data[1]/line_data[0];
		dy=1.0;
		while (x<=SIZE/2 || x>=IMAGE_WIDTH-SIZE/2 || y<=SIZE/2 || y>=IMAGE_HEIGHT-SIZE/2) {
			x+=dx;
			y+=dy;
		}
	}
	for (;x>SIZE/2 && x<IMAGE_WIDTH-SIZE/2 && y>SIZE/2 && y<IMAGE_HEIGHT-SIZE/2;x+=dx,y+=dy) {			
		if(0!=((uchar*)(img_temp->imageData + img_temp->widthStep*((int)y)))[(int)x])
			continue;
		
		//建立向量
		double vec[SIZE*SIZE*3];
		int i,j,num=0;
		for (i=-SIZE/2;i<=SIZE/2;i++) {
			for (j=-SIZE/2;j<=SIZE/2;j++) {
				uchar* temp=&((uchar*)(img_dst->imageData + img_dst->widthStep*((int)y+i)))[((int)x+j)*3];
				vec[num++]=temp[0];  //blue
				vec[num++]=temp[1];  //green
				vec[num++]=temp[2];  //red
			}
		}
		//计算夹角（余弦定理）
		double temp1=0.0,temp2=0.0,temp3=0.0;
		for (i=0;i<SIZE*SIZE*3;i++) {
			temp1+=vec_src[i]*vec[i];
			temp2+=vec_src[i]*vec_src[i];
			temp3+=vec[i]*vec[i];
		}
		temp_cos=temp1/sqrt(temp2*temp3);
		if (temp_cos>cos_value) {
			cos_value=temp_cos;
			point_dst[0]=x;
			point_dst[1]=y;
		}
	}

	//判断匹配精度
	if (cos_value<0.9) {
	//	cout<<"no match point!"<<endl;
		return false;
	}

	//计算颜色
	uchar* temp1=&((uchar*)(img_src->imageData + img_src->widthStep*((int)point1_data[1])))[((int)point1_data[0])*3];
	colors[0]+=temp1[2];
	colors[1]+=temp1[1];
	colors[2]+=temp1[0];

	return true;
}

/********************************************************************
	 calculate the M matrix of the two camera
*********************************************************************/
void calc_M(double M1_data[3][4],double M2_data[3][4]) {
	double a1_d[3][4]={816.664612,0,192.000000,0,
    0,816.664612,145.000000,0,
    0,0,1,0};
	double b1_d[4][4]={0.000000, 0.019645, 0.999809,-0.912897,
 -1.000000, 0.000000, 0.000000,-12.365179,
 0.000000, -0.999809, 0.019645,-0.128571,
    0,0,0,1};
	CvMat a1=cvMat(3,4,CV_64FC1,a1_d);
	CvMat b1=cvMat(4,4,CV_64FC1,b1_d);

	double a2_d[3][4]={816.664612,0,192.000000,0,
    0,816.664612,145.000000,0,
    0,0,1,0};
	double b2_d[4][4]={0.000000, 0.019645, 0.999809,1.287993,
 -1.000000, 0.000000, 0.000000,-12.365179,
 0.000000, -0.999809, 0.019645,-0.128571,
    0,0,0,1};
	CvMat a2=cvMat(3,4,CV_64FC1,a2_d);
	CvMat b2=cvMat(4,4,CV_64FC1,b2_d);

	for (int i=0;i<3;i++) {
		for (int j=0;j<4;j++) {
			M1_data[i][j]=0.0;
			M2_data[i][j]=0.0;
		}
	}
	CvMat M1=cvMat(3,4,CV_64FC1,M1_data);
	CvMat M2=cvMat(3,4,CV_64FC1,M2_data);
	cvMatMul(&a1,&b1,&M1);
	cvMatMul(&a2,&b2,&M2);
}

/********************************************************************
	 calculate the 3-dimention point from two 2-dimention points
*********************************************************************/
void point_reconstruct(double M1[3][4],double M2[3][4],
					   double p1[2],double p2[2],double p[3]) {
	double src1_data[4][3];
	src1_data[0][0]=p1[0]*M1[2][0]-M1[0][0];
	src1_data[0][1]=p1[0]*M1[2][1]-M1[0][1];
	src1_data[0][2]=p1[0]*M1[2][2]-M1[0][2];
	src1_data[1][0]=p1[1]*M1[2][0]-M1[1][0];
	src1_data[1][1]=p1[1]*M1[2][1]-M1[1][1];
	src1_data[1][2]=p1[1]*M1[2][2]-M1[1][2];
	src1_data[2][0]=p2[0]*M2[2][0]-M2[0][0];
	src1_data[2][1]=p2[0]*M2[2][1]-M2[0][1];
	src1_data[2][2]=p2[0]*M2[2][2]-M2[0][2];
	src1_data[3][0]=p2[1]*M2[2][0]-M2[1][0];
	src1_data[3][1]=p2[1]*M2[2][1]-M2[1][1];
	src1_data[3][2]=p2[1]*M2[2][2]-M2[1][2];
	CvMat src1=cvMat(4,3,CV_64FC1,src1_data); 
	double src2_data[4];
	src2_data[0]=M1[0][3]-p1[0]*M1[2][3];
	src2_data[1]=M1[1][3]-p1[1]*M1[2][3];
	src2_data[2]=M2[0][3]-p2[0]*M2[2][3];
	src2_data[3]=M2[1][3]-p2[1]*M2[2][3];
	CvMat src2=cvMat(4,1,CV_64FC1,src2_data);
	CvMat answer=cvMat(3,1,CV_64FC1,p);
	cvSolve(&src1,&src2,&answer,CV_SVD);
}