reconstruction.cpp

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

#include "stdafx.h"

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

#pragma comment(lib,"cxcore")
#pragma comment(lib,"cv")
#pragma comment(lib,"highgui")

static int mouse_x,mouse_y;
static double rotate_x=0,rotate_y=0,temp_x=0,temp_y=0;
vector<vector<double> > points_2d_vec;
vector<double> colors;


void init();
void display();
void reshape(int w,int h);
void mouse(int button,int state,int x,int y);
void mouse_move(int x,int y);

int main(int argc,char** argv)
{
	IplImage *img1 = 0;
	IplImage *img2 = 0;
	if( (img1 = cvLoadImage( "match_image/1_l.bmp", 1)) == 0 ) {
		cout<<"fail to load file !!!"<<endl;
		return -1;
	}
	if( (img2 = cvLoadImage( "match_image/1_r.bmp", 1)) == 0 ) {
		cout<<"fail to load file !!!"<<endl;
		return -1;
	}

	int i,j;
	double M1[3][4],M2[3][4],F[3][3];
	for (i=0;i<3;i++) {
		for (j=0;j<4;j++) {
			M1[i][j]=0.0;
			M2[i][j]=0.0;
			if(i<3)
				F[i][j]=0.0;
		}
	}

	cout<<"calc M1,M2"<<endl;
	calc_M(M1,M2);

	cout<<"find F matrix"<<endl;
	find_F(F,M1,M2);
	
	IplImage* gray1 = cvCreateImage( cvGetSize(img1), 8,1);
	IplImage* gray2 = cvCreateImage( cvGetSize(img1), 8,1);
	IplImage* temp1 = cvCreateImage( cvGetSize(img1), IPL_DEPTH_SIGN|16, 1 );
	IplImage* temp2 = cvCreateImage( cvGetSize(img1), IPL_DEPTH_SIGN|16, 1 );
	IplImage* img1_contour = cvCreateImage( cvGetSize(img1), 8, 1 );
	IplImage* img2_contour = cvCreateImage( cvGetSize(img1), 8, 1 );
	CvMemStorage* contour_st1 = cvCreateMemStorage(0);
	CvSeq* contour1=cvCreateSeq(CV_8U,sizeof(CvSeq),sizeof(CvPoint),contour_st1);
	CvMemStorage* contour_st2 = cvCreateMemStorage(0);
	CvSeq* contour2=cvCreateSeq(CV_8U,sizeof(CvSeq),sizeof(CvPoint),contour_st2);
	
	// 图像处理
	cout<<"image process"<<endl;
	//left
	cvCvtColor( img1, gray1, CV_BGR2GRAY );
	cvEqualizeHist(gray1,gray1);
	cvCanny(gray1,gray1,0.1,10.0);

	//right
	cvCvtColor( img2, gray2, CV_BGR2GRAY );
	cvEqualizeHist(gray2,gray2);
 	cvCanny(gray2,gray2,0.1,10.0);

	cvFindContours(gray1,contour_st1,&contour1,sizeof(CvContour),CV_RETR_LIST,CV_LINK_RUNS);
	cvFindContours(gray2,contour_st2,&contour2,sizeof(CvContour),CV_RETR_LIST,CV_LINK_RUNS);
		
	for (;contour2;contour2=contour2->h_next) {
		cvDrawContours(img2_contour,contour2,cvScalar(255.0,255.0,255.0),cvScalar(255.0,255.0,255.0),0);
	}
	cvDilate(gray2,gray2,NULL,2);

	double p1[2],p2[2],p[3];
	
	//开始匹配
	cout<<"start match"<<endl;
	for (;contour1;contour1=contour1->h_next) {
		double color[3]={0.0,0.0,0.0};
		cvDrawContours(img1_contour,contour1,cvScalar(255.0,255.0,255.0),cvScalar(255.0,255.0,255.0),0);
		vector<double> points_3d;
		for (i=0;i<contour1->total;i++) {
			p1[0]=((CvPoint*)cvGetSeqElem(contour1,i))->x;
			p1[1]=((CvPoint*)cvGetSeqElem(contour1,i))->y;

			//点匹配!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
			if (!match(p1,p2,img1,img2,F,color,img2_contour))
				continue;

			cvCircleAA(img2_contour,cvPoint(p2[0],p2[1]),1,255);
			point_reconstruct(M1,M2,p1,p2,p);

			points_3d.push_back(p[0]);
			points_3d.push_back(p[1]);
			points_3d.push_back(p[2]);
		}
		if (points_3d.size()!=0) {
			colors.push_back(color[0]/(points_3d.size()/3));
			colors.push_back(color[1]/(points_3d.size()/3));
			colors.push_back(color[2]/(points_3d.size()/3));

			points_2d_vec.push_back(points_3d);
		}
	}
	
	cvReleaseMemStorage(&contour_st1);
	cvReleaseMemStorage(&contour_st2);
	cvNamedWindow("img",1);
	cvShowImage("img",img2_contour);
	cvWaitKey();


	//立体显示
	glutInit(&argc,argv);
	glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB);
	glutInitWindowSize(800,800);
	glutInitWindowPosition(100,100);
	glutCreateWindow("hello");
	init();
	glutDisplayFunc(display);
	glutReshapeFunc(reshape);
	glutMouseFunc(mouse);
	glutMotionFunc(mouse_move);
	glutMainLoop();
	return 0;
}

void init() {
	glEnable(GL_POINT_SMOOTH);
	glHint(GL_POINT_SMOOTH_HINT,GL_DONT_CARE);
	glPointSize(2.5);
	glClearColor(1.0,1.0,1.0,0.0);
	glShadeModel(GL_FLAT);
}

void display() {
	glClear(GL_COLOR_BUFFER_BIT);
	glPushMatrix();
	glRotatef(temp_x+rotate_x,1.0,0.0,0.0);
	glRotatef(temp_y+rotate_y,0.0,1.0,0.0);
	glTranslatef(0.0,0.0,-2.0);

	
	glBegin(GL_POINTS);
for (int i=0;i<points_2d_vec.size();i++) {
			glColor3ub((int)colors[i*3+0],(int)colors[i*3+1],(int)colors[i*3+2]);
			for (int j=0;j<points_2d_vec[i].size()/3;j+=3)
				glVertex3d(points_2d_vec[i][3*j]+13,points_2d_vec[i][3*j+1]-10,points_2d_vec[i][3*j+2]);
		
	}
glEnd();
	
	glPopMatrix();
	glutSwapBuffers();
}

void reshape(int w,int h) {
	glViewport(0,0,(GLsizei)w,(GLsizei)h);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	glOrtho(-5.0,5.0,-5.0,5.0,-50.0,50.0);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
}

void mouse(int button,int state,int x,int y) {
	switch(button) {
	case GLUT_LEFT_BUTTON:
		if (state==GLUT_DOWN) {
			mouse_x=x;
			mouse_y=y;
		}
		if (state==GLUT_UP) {
			rotate_x+=temp_x;
			rotate_y+=temp_y;
			temp_x=temp_y=0;
			rotate_x=(rotate_x>360?rotate_x-360:rotate_x);
			rotate_y=(rotate_y>360?rotate_y-360:rotate_y);
			rotate_x=(rotate_x<-360?rotate_x+360:rotate_x);
			rotate_y=(rotate_y<-360?rotate_y+360:rotate_y);
		}
		break;
	case GLUT_RIGHT_BUTTON:
		if (state==GLUT_DOWN)
			exit(1);
		break;
	default:
		break;
	}
}

void mouse_move(int x,int y) {
	temp_x=(mouse_y-y)/10.0;
	temp_y=(mouse_x-x)/10.0;
	if (temp_x>360.0)
		temp_x-=360.0;
	if (temp_y>360.0)
		temp_y-=360.0;
	glutPostRedisplay();
}