simplemain.cpp

基础矩阵类的程序。使用OpenCV开发。

//this is a simple method which computes and visualizes the fundamental matrix
//input is two files with the point correspondences
//two corresponding images
//the routine compute the fundamental matrix between the cameras and 
//then visualizes the fundamental matrix
//this routine uses the STL vector 
//to run this open cv must be installed 
//http://www.cs.ucf.edu/~lspencer/vid_app.pdf is a great tutorial to get opencv up and running
#include <cvcam.h>
#include <cv.h>
#include "highgui.h"
#include <stdio.h>
#include <vector>
#include <math.h>

using namespace std;


int main() {
	
	
	
	
	char pointsList1[1000];
	char pointsList2[1000];
	char image1[1000];
	char image2[1000];
	
	
	sprintf(pointsList1,"%s","left.txt");
	sprintf(pointsList2,"%s","right.txt");
	sprintf(image1,"left.pgm");
	sprintf(image2,"right.pgm");
	
	
	FILE* inputFiles[2];
	
	
	//declare useful typedefs
	typedef vector <CvPoint2D32f> CVPOINT2D32F_VECTOR;
	std::vector <CVPOINT2D32F_VECTOR> allPoints;
	
	
	
	//open input files
	inputFiles[0] = fopen(pointsList1,"r");
	if ( inputFiles[0] == NULL ) {
		printf("%s not found\n",pointsList1);
		return -1;
	}
	
	inputFiles[1] = fopen(pointsList2,"r");
	if ( inputFiles[1] == NULL ) {
		printf("%s not found\n",pointsList2);
		return -1;
	}
	
	
	
	//read in images
	IplImage* inputImage1 = cvLoadImage(image1);
	if ( inputImage1 == NULL ) {
		printf("%s not found\n",image1);
		return -1;
	}
	
	IplImage* inputImage2 = cvLoadImage(image2);
	if ( inputImage2 == NULL ) {
		printf("%s not found\n",image2);
		return -1;
	}
	
	//assume that images are the same size
	CvSize imageSize = cvGetSize(inputImage1);
	
	//first read in the point pairs
	for (int i = 0; i < 2; i++) {
		
		CVPOINT2D32F_VECTOR points;
		CvPoint2D32f temp;
		double x,y;
		while ( fscanf(inputFiles[i],"%lf %lf",&x,&y) == 2 ) {
			
			temp.x =x;
			temp.y =y;
			
			points.push_back(temp);
		}
		allPoints.push_back(points);
	}
	
	//make sure that they are equal length
	if ( allPoints[0].size() != allPoints[1].size() ) {
		printf("input files must have same number of points\n");
	}
	
	
	//transfer the vector of points to the appropriate opencv matrix structures
	int i1,i2;
	i2 =0;
	int numPoints = allPoints[0].size();
	CvMat* points1;
	CvMat* points2;
	CvMat* status;
	CvMat* fundMatr;
	points1 = cvCreateMat(2,numPoints,CV_32F);
	points2 = cvCreateMat(2,numPoints,CV_32F);
	status = cvCreateMat(1,numPoints,CV_32F);
	
	for ( i1 = 0; i1 < numPoints; i1++) {
		
		cvSetReal2D(points1,0,i1,allPoints[0][i1].x/1); 
		cvSetReal2D(points1,1,i1,allPoints[0][i1].y/1); 
		
		cvSetReal2D(points2,0,i1,allPoints[1][i1].x/1); 
		cvSetReal2D(points2,1,i1,allPoints[1][i1].y/1); 
	}
	
	//create the output fundamental matrix
	fundMatr = cvCreateMat(3,3,CV_32F);
	
	//see opencv manual for other options in computing the fundamental matrix	
	int num = cvFindFundamentalMat(points1,points2,fundMatr,CV_FM_8POINT,1.0,0.9999,status);
	
	if( num == 1 )
	{
		printf("Fundamental matrix was found\n");
		
		
	}
	else
	{
		printf("Fundamental matrix was not found\n");
		return -1;
		
	}
	
	
	//now visualize the fundamental matrix
	
	int numOutputPoints;
	
	
	CvMat* corrLines;
	corrLines= cvCreateMat(3,numPoints,CV_32F);
	
	
	//specify which direction to compute epipolar lines
	int startImage = 2;
	cvComputeCorrespondEpilines( points2,
		startImage,//means points are in image 1
		fundMatr,
		corrLines);
	
	CvMat* a = cvCreateMat(3,1,CV_32F);
	CvMat* b = cvCreateMat(3,1,CV_32F);
	CvMat* c = cvCreateMat(3,1,CV_32F);
	CvMat* d = cvCreateMat(3,1,CV_32F);
	
	//create output window
	char windowName[100];
	strcpy(windowName,"Output Window");
	cvNamedWindow(windowName,CV_WINDOW_AUTOSIZE);
	
	//for all the points set the point and corresponding epipolar line
	//and determine where the epipolar line intersects the image plane
	//then display all this info
	CvMat* epiLine = cvCreateMat(1,3,CV_32F);
	for (  i1 = 0; i1 < numPoints; i1++) {
		
		for (i2 = 0; i2 < 3; i2++) {
			cvmSet(epiLine,0,i2,cvmGet(corrLines,i2,i1));
		}
		
		
		CvPoint epipolarLinePoint1, epipolarLinePoint2;
		
		int  i4;
		
		CvMat* a = cvCreateMat(3,1,CV_32F);
		CvMat* b = cvCreateMat(3,1,CV_32F);
		CvMat* c = cvCreateMat(3,1,CV_32F);
		CvMat* d = cvCreateMat(3,1,CV_32F);
		
		
		for ( i4 = 0; i4 < 3; i4++) {
			
			cvSetReal2D(a,i4,0,cvGetReal2D(epiLine,0,i4)/cvGetReal2D(epiLine,0,2));		
		}
		
		
		if (abs(cvGetReal2D(epiLine,0,0)) > abs(cvGetReal2D(epiLine,0,1)) ){
			
			double ylim = imageSize.height;
			
			
			cvSetReal2D(b,0,0,0);
			cvSetReal2D(b,1,0,1);
			cvSetReal2D(b,2,0,0);
			
			
			cvCrossProduct(a,b,c);
			for ( i4 = 0; i4 < 3; i4++) {
				cvSetReal2D(c,i4,0,cvGetReal2D(c,i4,0)/cvGetReal2D(c,2,0));		
			}
			
			cvSetReal2D(b,0,0,0);
			cvSetReal2D(b,1,0,-1.0/ylim);			
			cvSetReal2D(b,2,0,1);
			cvCrossProduct(a,b,d);
			for ( i4 = 0; i4 < 3; i4++) {
				cvSetReal2D(d,i4,0,cvGetReal2D(d,i4,0)/cvGetReal2D(d,2,0));		
			}
			
		}
		else  { 
			double xlim = imageSize.width;
			cvSetReal2D(b,0,0,1);
			cvSetReal2D(b,1,0,0);
			cvSetReal2D(b,2,0,0);
			
			cvCrossProduct(a,b,c);
			for ( i4 = 0; i4 < 3; i4++) {
				cvSetReal2D(c,i4,0,cvGetReal2D(c,i4,0)/cvGetReal2D(c,2,0));		
			}
			
			cvSetReal2D(b,0,0,-1.0/xlim);
			cvSetReal2D(b,1,0,0);
			cvSetReal2D(b,2,0,1);
			cvCrossProduct(a,b,d);
			for ( i4 = 0; i4 < 3; i4++) {
				cvSetReal2D(d,i4,0,cvGetReal2D(d,i4,0)/cvGetReal2D(d,2,0));		
			}
			
		}
		
		
		epipolarLinePoint1.x = cvmGet(c,0,0);
		epipolarLinePoint1.y = cvmGet(c,1,0);
		
		epipolarLinePoint2.x = cvmGet(d,0,0);
		epipolarLinePoint2.y = cvmGet(d,1,0);
		
		cvCircle(inputImage2,cvPoint(cvmGet(points2,0,i1),cvmGet(points2,1,i1)),5,CV_RGB(255,255,0),1);
		
		cvShowImage(windowName,inputImage2);
		cvWaitKey(0);
		
		
		cvLine(inputImage1,epipolarLinePoint1,epipolarLinePoint2,CV_RGB(0,255,0));
		cvShowImage(windowName,inputImage1);
		cvWaitKey(0);
	}
	
	return 1;
	
}