three_dimensional_reconstruction.cpp

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

#include "three_dimensional_reconstruction.h"
#include "glut.h"
#include "cvaux.h"
#include "ltga.h"
#include <vector>
#include <string>
#include <iostream>
#include <algorithm>
#include <fstream>

using namespace std;

#pragma comment(lib,"glut32")
#pragma comment(lib,"cvaux")

//#define POINTS_SHOW
#define LINES_SHOW
//#define SURFACE_SHOW

double rotate_x=0,rotate_x_buf=0,rotate_y=0,rotate_y_buf=0,rotate_step=0.5;
double translate_z=-10.0,translate_buf=0.0,translate_step=0.2;
int mouse_x,mouse_y;
bool left_down = false;
bool right_down = false;
static GLuint texName;

vector<vector<double> >* points=NULL;
vector<vector<int> >* depth=NULL;
vector<vector<int> > points_index(0,vector<int>(3));

LTGA ltga;
unsigned char* texture_data=NULL;

bool cmp(vector<int>& v1,vector<int>& v2) {
	if (v1[0]<v2[0]) {
		return true;
	} else if (v1[0] > v2[0]) {
		return false;
	} else {
		return v1[1]<v2[1]?true:false;
	}
}

void points_reconstruction(double& focal,
						   double& baseline,
						   vector<vector<int> >& depth,
						   vector<vector<double> >& points3d) 
{
	if (depth.size() == 0)
		return;
	double x_min = baseline * depth[0][0] / depth[0][2];
	double x_max = baseline * depth[0][0] / depth[0][2];
	double y_min = baseline * depth[0][1] / depth[0][2];
	double y_max = baseline * depth[0][1] / depth[0][2];
	double z_min = baseline * focal / depth[0][2];
	double z_max = baseline * focal / depth[0][2];
	int i;
	for (i=0;i<depth.size();i++) {
		points3d[i][0] = baseline * depth[i][0] / depth[i][2];
		points3d[i][1] = baseline * depth[i][1] / depth[i][2];
		points3d[i][2] = baseline * focal / depth[i][2];
		if (points3d[i][0]<x_min) {
			x_min = points3d[i][0];
		}if (points3d[i][0]>x_max) {
			x_max = points3d[i][0];
		}if (points3d[i][1]<y_min) {
			y_min = points3d[i][1];
		}if (points3d[i][1]>y_max) {
			y_max = points3d[i][1];
		}if (points3d[i][2]<z_min) {
			z_min = points3d[i][2];
		}if (points3d[i][2]>z_max) {
			z_max = points3d[i][2];
		}
	}
	for (i=0;i<points3d.size();i++) {
		points3d[i][0] -= (x_max+x_min)/2;
		points3d[i][1] -= (y_max+y_min)/2;
		points3d[i][2] -= (z_max+z_min)/2;
	}
// 	ofstream fout("temp.txt");
// 	for (i=0;i<points3d.size();i++) {
// 		fout<<points3d[i][0]<<" "<<points3d[i][1]<<" "<<points3d[i][2]<<endl;
// 	}
//	cout<<points3d.size()<<endl;
//	cout<<depth.size()<<endl;
//	exit(1);
}

void triangulation(vector<vector<double> >& points3d,
				   vector<vector<int> >& depth,
				   vector<int>& points_count) 
{
	int i,j,m,count=0;
	bool finish = false;
	vector<int> index(3);

	sort(depth.begin(),depth.end(),cmp);
// 	ofstream fout("temp.txt");
// 	for (i=0;i<depth.size();i++) {
// 		fout<<depth[i][0]<<" "<<depth[i][1]<<" "<<depth[i][2]<<endl;
// 	}
// 	exit(1);

	for (i=0;i<points_count.size()-1;) {
		//该极线中是否有匹配点，没有则下移一条极线
		while (points_count[i] <= 0) {
			i++;
			if (i == points_count.size()-1) {
				finish = true;
				break;
			}
		}
		m = i+1;
		while (points_count[m] <= 0) {
			m++;
			if (m == points_count.size()) {
				finish = true;
				break;
			}
		}
		if (finish)
			break;
		
		//三角化
		for (j=0;j<points_count[i]-1;j++) {
			index[0]=count+j;
			index[1]=count+j+1;
			if (points_count[m]>j) {
				index[2]=count+points_count[i]+j;
			} else {
				index[2]=count+points_count[i]+points_count[m]-1;
			}
			points_index.push_back(index);
		}
		
		for (j=0;j<points_count[m]-1;j++) {
			index[0]=count+points_count[i]+j;
			if (points_count[i]>j+1) {
				index[1]=count+j+1;
			} else {
				index[1]=count+points_count[i]-1;
			}
			index[2]=count+points_count[i]+j+1;
			points_index.push_back(index);
		}
		count+=points_count[i];
		i=m;
	}
}

void three_dimensional_show(vector<vector<double> >& points3d,vector<vector<int> >* dep,int* argc,char** argv) {
	points = &points3d;
	init(argc,argv);
	if (dep != NULL) {
		depth = dep;
		string texture_image = "image/1_l.bmp";
		init_texture(texture_image);
	}
//	init_light();
	glutMainLoop();
}

void display() {
 	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
 	glPushMatrix();

	glTranslatef(0.0,0.0,translate_z+translate_buf);
	glRotatef(rotate_x+rotate_x_buf,1.0,0.0,0.0);
	glRotatef(rotate_y+rotate_y_buf,0.0,1.0,0.0);
 	
	glColor3f(1.0,0.0,0.0);

// 	glBegin(GL_TRIANGLES);
// 	for (int i=0;i<points_index.size();i++) {
//  		glTexCoord2i((*depth)[points_index[i][0]][0],
//  					(*depth)[points_index[i][0]][1]);
// 		glVertex3d((*points)[points_index[i][0]][0],
// 					(*points)[points_index[i][0]][1],
// 					(*points)[points_index[i][0]][2]);
// 
//  		glTexCoord2i((*depth)[points_index[i][1]][0],
//  					(*depth)[points_index[i][1]][1]);
// 		glVertex3d((*points)[points_index[i][1]][0],
// 					(*points)[points_index[i][1]][1],
// 					(*points)[points_index[i][1]][2]);
// 
//  		glTexCoord2i((*depth)[points_index[i][2]][0],
//  					(*depth)[points_index[i][2]][1]);
// 		glVertex3d((*points)[points_index[i][2]][0],
// 					(*points)[points_index[i][2]][1],
// 					(*points)[points_index[i][2]][2]);
// 	}
// 	glEnd();

	for (int i=0;i<points_index.size();i++) {
#ifdef POINTS_SHOW
		glBegin(GL_POINTS);
#endif
#ifdef LINES_SHOW
		glBegin(GL_LINE_LOOP);
#endif
#ifdef SURFACE_SHOW
		glBegin(GL_TRIANGLES);
#endif
		glVertex3d((*points)[points_index[i][0]][0],
					(*points)[points_index[i][0]][1],
					(*points)[points_index[i][0]][2]);
		glVertex3d((*points)[points_index[i][1]][0],
					(*points)[points_index[i][1]][1],
					(*points)[points_index[i][1]][2]);
		glVertex3d((*points)[points_index[i][2]][0],
					(*points)[points_index[i][2]][1],
					(*points)[points_index[i][2]][2]);
		glEnd();
	}

	glPopMatrix();
	glutSwapBuffers();
}

void init_light() {
	GLfloat mat_specular[] = {1.0,1.0,1.0,1.0};
	GLfloat mat_shininess[] = {5.5};
	GLfloat light_position[] = {20.0,20.0,20.0,1.0};
	GLfloat white_light[] = {1.0,1.0,1.0,1.0};
	GLfloat lmodel_ambient[] = {0.1,0.1,0.1,1.0};
//	glClearColor(0.0,0.0,0.0,0.0);
//	glShadeModel(GL_SMOOTH);
	glMaterialfv(GL_FRONT,GL_SPECULAR,mat_specular);
	glMaterialfv(GL_FRONT,GL_SHININESS,mat_shininess);
	glLightfv(GL_LIGHT0,GL_POSITION,light_position);
	glLightfv(GL_LIGHT0,GL_DIFFUSE,white_light);
	glLightfv(GL_LIGHT0,GL_SPECULAR,white_light);
	glLightModelfv(GL_LIGHT_MODEL_AMBIENT,lmodel_ambient);
	glEnable(GL_LIGHTING);
	glEnable(GL_LIGHT0);
}

void init_texture(string& texture_image) {
	ltga.LoadFromFile("image/1_l.tga");
	texture_data=ltga.GetPixels();
	glPixelStorei(GL_UNPACK_ALIGNMENT,1);
	glGenTextures(1,&texName);
	glBindTexture(GL_TEXTURE_2D,texName);
	
	glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_WRAP_S,GL_REPEAT);
	glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_WRAP_T,GL_REPEAT);
	glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_NEAREST);
	glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_NEAREST);
//	glTexEnvf(GL_TEXTURE_ENV,GL_TEXTURE_ENV_MODE,GL_DECAL);
	glTexImage2D(GL_TEXTURE_2D,0,GL_RGBA,256,256,0,GL_RGBA,GL_UNSIGNED_BYTE,texture_data);
	glEnable(GL_TEXTURE_2D);
//	glTexEnvf(GL_TEXTURE_ENV,GL_TEXTURE_ENV_MODE,GL_DECAL);
	glBindTexture(GL_TEXTURE_2D,texName);
}

void init(int* argc,char** argv) {
	glutInit(argc,argv);
	glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
	glutInitWindowSize(400,400);
	glutInitWindowPosition(100,100);
	glutCreateWindow("reconstruction");
//	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);
	glutDisplayFunc(display);
	glutReshapeFunc(reshape);
	glutKeyboardFunc(keyboard_control);
	glutSpecialFunc(special_control);
	glutMouseFunc(mouse);
	glutMotionFunc(mouse_move);

//	init_texture();
//	init_light();
	glEnable(GL_DEPTH_TEST);
}

void reshape(int w,int h) {
	glViewport(0,0,(GLsizei)w,(GLsizei)h);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(30.0,1.0,0.0001,1000000000000000.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) {
			left_down = true;
			mouse_x=x;
			mouse_y=y;
		}
		else if (state==GLUT_UP) {
			left_down = false;
			rotate_x += rotate_x_buf;
			rotate_y += rotate_y_buf;
			rotate_x_buf = 0;
			rotate_y_buf = 0;
		}
		break;
	case GLUT_RIGHT_BUTTON:
		if (state == GLUT_DOWN) {
			right_down = true;
			mouse_x = x;
		}
		else if (state == GLUT_UP) {
			right_down = false;
			translate_z += translate_buf;
			translate_buf = 0.0;
		}
	default:
		break;
	}
	glutPostRedisplay();
}

void mouse_move(int x,int y) {
	if (left_down) {
		rotate_x_buf = -(mouse_y-y)*rotate_step;
		rotate_y_buf = -(mouse_x-x)*rotate_step;
		if (rotate_x > 360.0)
			rotate_x -= 360.0;
		else if (rotate_x < -360.0)
			rotate_x += 360.0;
		if (rotate_y > 360.0)
			rotate_y -= 360.0;
		else if (rotate_y < -360.0)
			rotate_y += 360.0;
	}
	else if (right_down) {
		translate_buf = -(mouse_x-x)*translate_step;
	}
	glutPostRedisplay();
}

void keyboard_control(unsigned char key,int a,int b) {
	if (key == 0x1B)
		exit(1);
	glutPostRedisplay();
}

void special_control(int key,int x,int y) {
	if (key == GLUT_KEY_LEFT)
		rotate_x+=rotate_step;
	else if (key == GLUT_KEY_RIGHT)
		rotate_y-=rotate_step;
	else if (key == GLUT_KEY_UP)
		rotate_x-=rotate_step;
	else if (key == GLUT_KEY_DOWN)
		rotate_x+=rotate_step;
	glutPostRedisplay();
}