www.pudn.com > opencv-Polargeometry.zip > main.cpp, change:2016-11-08,size:13128b


//包含的头文件 
#include<iostream> //标准输入输出流  
#include "cvut.h" //摄像机标定 
#include <fstream>//文件流 
#include <string>//字符串 
#include "cvutSeq.h"//摄像机标定.序列 
//包含的命名空间 
using namespace std;//与标准输入输出流对应 
using namespace cvut;//与摄像机标定对应 
 
//定义全局变量,便于在回调函数中使用 
CvMat* fundamental_matrix = cvCreateMat(3, 3, CV_32F);//定义基本矩阵 
int num1 = 0; int num2 = 0;//定义两张图的选点数 
Seq<CvPoint> points_seq1;//选点序列 
Seq<CvPoint> points_seq2; 
IplImage *img1 = cvLoadImage("IMG_5084.JPG", 1); 
IplImage *img2 = cvLoadImage("IMG_5097.JPG", 1); 
CvMat* R1 = cvCreateMat(3, 2, CV_32F);//定义相机外参数 
CvMat* T1 = cvCreateMat(3, 1, CV_32F); 
CvMat* R2 = cvCreateMat(3, 2, CV_32F); 
CvMat* T2 = cvCreateMat(3, 1, CV_32F); 
double fx = 1969.82;//定义相机内部参数,并赋值 
double fy = 1971.52; 
double cx = 329.079; 
double cy = 233.369; 
                                   /* 鼠标回调函数 */ 
void on_mouse(int event, int x, int y, int flags, void* img) 
{ 
 
	CvFont font;//描述字体属性的变量 
	//初始化:水平方向字体的缩放系数h_scale为0.5;垂直方向v_scale为0.5;;字体的倾斜程度,shear为0表示不倾斜;thichness, line_type表示线型的宽度和线条的类型; 
	cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 0.5, 0.5, 0, 1, CV_AA); 
	if (event == CV_EVENT_LBUTTONDOWN) 
	{ 
		IplImage *timg = cvCloneImage((IplImage *)img);//定义处理图 
		CvPoint pt = cvPoint(x, y);//定义鼠标捕获点 
		char temp[16]; 
		sprintf(temp, "(%d,%d)", x, y);//把格式化的数据写入字符串 
		cvPutText(timg, temp, pt, &font, CV_RGB(250, 0, 0));//显示坐标 
		cvCircle(timg, pt, 2, cvScalar(255, 0, 0, 0), CV_FILLED, CV_AA, 0);//显示点 
		cvShowImage("result", timg); 
		if (x < img1->width)//判断所选点在图1 
		{ 
			num1++;//计数图1选点数 
			points_seq1.push_back(pt);//入栈图1序列 
			CvMat* point1 = cvCreateMat(2, num1, CV_32F);//定义点阵 
		    for (int i = 0; i < points_seq1.cvseq->total; i++)//点“出栈” 
			{ 
				cvmSet(point1, 0, i, points_seq1[i].x); 
				cvmSet(point1, 1, i, points_seq1[i].y); 
				//cout << '(' << cvmGet(point1, 0, i) << ',' << cvmGet(point1, 1, i) << ')' << '\t'; 
			} 
			//cout << endl;//显示坐标 
			//int e2x; int e2y;//计算极点坐标 
			//e2x = (cvmGet(fundamental_matrix, 1, 1)*cvmGet(fundamental_matrix, 2, 0) - cvmGet(fundamental_matrix, 2, 1)*cvmGet(fundamental_matrix, 1, 0)) / (cvmGet(fundamental_matrix, 1, 0)*cvmGet(fundamental_matrix, 0, 1) - cvmGet(fundamental_matrix, 0, 0)*cvmGet(fundamental_matrix, 1, 1)); 
			//e2y = (cvmGet(fundamental_matrix, 0, 0)*cvmGet(fundamental_matrix, 2, 1) - cvmGet(fundamental_matrix, 2, 0)*cvmGet(fundamental_matrix, 0, 1)) / (cvmGet(fundamental_matrix, 1, 0)*cvmGet(fundamental_matrix, 0, 1) - cvmGet(fundamental_matrix, 0, 0)*cvmGet(fundamental_matrix, 1, 1)); 
			CvMat* obpt1 = cvCreateMat(2, num1, CV_32F);//定义世界坐标系下点阵,与相机标定时一致认为在Z=0平面上 
			for (int i = 0; i < num1; i++)//计算各点对应世界坐标 
			{ 
				double A = (cvmGet(point1, 0, i) - cx) / fx; 
				double B = (cvmGet(point1, 1, i) - cy) / fy; 
				double C = A*cvmGet(R1, 2, 0) - cvmGet(R1, 0, 0); 
				double D = cvmGet(R1, 0, 1) - A*cvmGet(R1, 2, 1); 
				double E = cvmGet(T1, 0, 0) - A*cvmGet(T1, 2, 0); 
				double F = B*cvmGet(R1, 2, 0) - cvmGet(R1, 1, 0); 
				double G = cvmGet(R1, 1, 1) - B*cvmGet(R1, 2, 1); 
				double H = cvmGet(T1, 1, 0) - B*cvmGet(T1, 2, 0);//定义的一些中间变量 
				cvmSet(obpt1, 0, i, (D*H-E*G)/(F*D-C*G)); 
				cvmSet(obpt1, 1, i, (C*H - E*F) / (F*D - C*G));//计算世界坐标系下X,Y 
			} 
			//计算极线和对应点 
			for (int i = 0; i < num1; i++) 
			{ 
			    CvRNG rng = cvRNG(cvGetTickCount());//cvGetTickCount() 
				int color_r = cvRandInt(&rng) / (1024 * 1024 * 16);//cvRandInt输出32位结果,转换到0-255 
				int color_g = cvRandInt(&rng) / (1024 * 1024 * 16); 
				int color_b = cvRandInt(&rng) / (1024 * 1024 * 16); 
				CvScalar color = cvScalar(color_r, color_g, color_b);//随机产生颜色   
				int x0 = cvmGet(point1, 0, i); int y0 = cvmGet(point1, 1, i);//获得每点坐标,准备画圈 
				CvPoint mn = cvPoint(x0, y0); 
				cvCircle(timg, mn, 3, color);//把选点用圆圈画出来 
				//由基本矩阵和选点坐标计算极线斜率 
				double a; double b; double c; 
				a = x0*cvmGet(fundamental_matrix, 0, 0) + y0*cvmGet(fundamental_matrix, 0, 1) + cvmGet(fundamental_matrix, 0, 2); 
				b = x0*cvmGet(fundamental_matrix, 1, 0) + y0*cvmGet(fundamental_matrix, 1, 1) + cvmGet(fundamental_matrix, 1, 2); 
				c = x0*cvmGet(fundamental_matrix, 2, 0) + y0*cvmGet(fundamental_matrix, 2, 1) + cvmGet(fundamental_matrix, 2, 2); 
				int x1 = 0 + img1->width; int y1 = -c / b; 
				int x2 = img1->width + img2->width; int y2 = -(c + img2->width * a) / b; 
				cvLine(timg, cvPoint(x1, y1), cvPoint(x2, y2), color);//画极线 
				//计算对应点坐标 
				int u; int v; 
				u = cx + fx*(cvmGet(T2, 0, 0) + cvmGet(obpt1, 0, i)*cvmGet(R2, 0, 0) + cvmGet(obpt1, 1, i)*cvmGet(R2, 0, 1)) / (cvmGet(T2, 2, 0) + cvmGet(obpt1, 0, i)*cvmGet(R2, 2, 0) + cvmGet(obpt1, 1, i)*cvmGet(R2, 2, 1)); 
				v = cy + fy*(cvmGet(T2, 1, 0) + cvmGet(obpt1, 0, i)*cvmGet(R2, 1, 0) + cvmGet(obpt1, 1, i)*cvmGet(R2, 1, 1)) / (cvmGet(T2, 2, 0) + cvmGet(obpt1, 0, i)*cvmGet(R2, 2, 0) + cvmGet(obpt1, 1, i)*cvmGet(R2, 2, 1)); 
				CvPoint uv = cvPoint(u + img1->width, v); 
				cvCircle(timg, uv, 3, color);//画对应点 
			} 
		} 
		else//否则点在图2.相应处理与上述类似 
		{ 
		    num2++; 
			points_seq2.push_back(pt); 
			CvMat* point2 = cvCreateMat(2, num2, CV_32F); 
			for (int i = 0; i < points_seq2.cvseq->total; i++) 
			{ 
				cvmSet(point2, 0, i, points_seq2[i].x - img1->width); 
				cvmSet(point2, 1, i, points_seq2[i].y); 
				//cout << '(' << cvmGet(point2, 0, i) << ',' << cvmGet(point2, 1, i) << ')' << '\t'; 
			} 
			//cout << endl; 
			CvMat* obpt2 = cvCreateMat(2, num2, CV_32F); 
			for (int i = 0; i < num2; i++) 
			{ 
				double A = (cvmGet(point2, 0, i) - cx) / fx; 
				double B = (cvmGet(point2, 1, i) - cy) / fy; 
				double C = A*cvmGet(R2, 2, 0) - cvmGet(R2, 0, 0); 
				double D = cvmGet(R2, 0, 1) - A*cvmGet(R2, 2, 1); 
				double E = cvmGet(T2, 0, 0) - A*cvmGet(T2, 2, 0); 
				double F = B*cvmGet(R2, 2, 0) - cvmGet(R2, 1, 0); 
				double G = cvmGet(R2, 1, 1) - B*cvmGet(R2, 2, 1); 
				double H = cvmGet(T2, 1, 0) - B*cvmGet(T2, 2, 0); 
				cvmSet(obpt2, 0, i, (D*H - E*G) / (F*D - C*G)); 
				cvmSet(obpt2, 1, i, (C*H - E*F) / (F*D - C*G)); 
            } 
			//int e1x; int e1y; 
			//e1x = (cvmGet(fundamental_matrix, 1, 1)*cvmGet(fundamental_matrix, 0, 2) - cvmGet(fundamental_matrix, 1, 2)*cvmGet(fundamental_matrix, 0, 1)) / (cvmGet(fundamental_matrix, 0, 1)*cvmGet(fundamental_matrix, 1, 0) - cvmGet(fundamental_matrix, 0, 0)*cvmGet(fundamental_matrix, 1, 1)); 
			//e1y = (cvmGet(fundamental_matrix, 0, 0)*cvmGet(fundamental_matrix, 1, 2) - cvmGet(fundamental_matrix, 0, 2)*cvmGet(fundamental_matrix, 1, 0)) / (cvmGet(fundamental_matrix, 0, 1)*cvmGet(fundamental_matrix, 1, 0) - cvmGet(fundamental_matrix, 0, 0)*cvmGet(fundamental_matrix, 1, 1)); 
			for (int i = 0; i < num2; i++) 
			{ 
				int x0 = cvmGet(point2, 0, i) + img1->width; int y0 = cvmGet(point2, 1, i); 
				CvPoint mn = cvPoint(x0, y0); 
				CvRNG rng = cvRNG(cvGetTickCount()); 
			    int color_r = cvRandInt(&rng) / (1024 * 1024 * 16); 
				int color_g = cvRandInt(&rng) / (1024 * 1024 * 16); 
				int color_b = cvRandInt(&rng) / (1024 * 1024 * 16); 
				CvScalar color = cvScalar(color_r, color_g, color_b);   
				cvCircle(timg, mn, 3, color); 
				double a; double b; double c; 
				a = cvmGet(point2, 0, i)*cvmGet(fundamental_matrix, 0, 0) + y0*cvmGet(fundamental_matrix, 1, 0) + cvmGet(fundamental_matrix, 2, 0); 
				b = cvmGet(point2, 0, i)*cvmGet(fundamental_matrix, 0, 1) + y0*cvmGet(fundamental_matrix, 1, 1) + cvmGet(fundamental_matrix, 2, 1); 
				c = cvmGet(point2, 0, i)*cvmGet(fundamental_matrix, 0, 2) + y0*cvmGet(fundamental_matrix, 1, 2) + cvmGet(fundamental_matrix, 2, 2); 
			    int x1 = 0; int y1 = -c / b; 
				int x2 = img1->width; int y2 = -(c + img1->width * a) / b; 
				cvLine(timg, cvPoint(x1, y1), cvPoint(x2, y2), color); 
				int u; int v; 
				u = cx + fx*(cvmGet(T1, 0, 0) + cvmGet(obpt2, 0, i)*cvmGet(R1, 0, 0) + cvmGet(obpt2, 1, i)*cvmGet(R1, 0, 1)) / (cvmGet(T1, 2, 0) + cvmGet(obpt2, 0, i)*cvmGet(R1, 2, 0) + cvmGet(obpt2, 1, i)*cvmGet(R1, 2, 1)); 
				v = cy + fy*(cvmGet(T1, 1, 0) + cvmGet(obpt2, 0, i)*cvmGet(R1, 1, 0) + cvmGet(obpt2, 1, i)*cvmGet(R1, 1, 1)) / (cvmGet(T1, 2, 0) + cvmGet(obpt2, 0, i)*cvmGet(R1, 2, 0) + cvmGet(obpt2, 1, i)*cvmGet(R1, 2, 1)); 
				CvPoint uv = cvPoint(u , v); 
				cvCircle(timg, uv, 3, color); 
			} 
		} 
		cvShowImage("result", timg);//显示结果 
    } 
} 
 
 
 
                                          /* 主函数 */ 
int main() 
{ 
	Image<uchar> view1("IMG_5084.JPG");//读取相应图片 
	Image<uchar> view2("IMG_5097.JPG");//读取相应图片 
	CvSize board_size = cvSize(5, 7);//定义每张棋盘图检测角点尺寸为5*7 
	int count1;//图1角点数目 
	int count2;//图2角点数目 
	//定义并转为一张灰度图 
	Image<uchar> view_gray1(view1.size(), 8, 1); 
    rgb2gray(view1, view_gray1); 
	Image<uchar> view_gray2(view2.size(), 8, 1); 
	rgb2gray(view2, view_gray2); 
	CvPoint2D32f * image_points_buf = new CvPoint2D32f[board_size.width*board_size.height];//坐标值为浮点类型的二维点,用来存放每张图角点坐标 
	Seq<CvPoint2D32f> image_points_seq;//定义模板序列 
	if (0 == cvFindChessboardCorners(view1.cvimage, board_size, image_points_buf, &count1, CV_CALIB_CB_ADAPTIVE_THRESH)) 
	{ 
		cout << "can not find chessboard corners!\n"; 
		exit(1);//角点为0,异常退出 
	} 
	else//正常 
	{ 
		//检测亚像素级角点,使用(11*2+1)×(11*2+1)=23×23大小的搜索窗口;死区的一半尺寸设置为无死区;迭代终止条件最大迭代次数类型和设定的精度(亚像素级精度为像素的十分之一)类型组合 
		cvFindCornerSubPix(view_gray1.cvimage, image_points_buf, count1, cvSize(11, 11), 
			cvSize(-1, -1), cvTermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1)); 
		image_points_seq.push_back(image_points_buf, count1);//检测到的所有角点依顺序入栈进行保存 
		//在图像上显示角点位置 
		cvDrawChessboardCorners(view1.cvimage, board_size, image_points_buf, count1, 1);//patternWasFound设置为1 
		view1.show("calib1");//显示 
		cvWaitKey();//等待按键 
	} 
	if (0 == cvFindChessboardCorners(view2.cvimage, board_size, image_points_buf, &count2, CV_CALIB_CB_ADAPTIVE_THRESH)) 
	{ 
		cout << "can not find chessboard corners!\n"; 
		exit(1);//角点为0,异常退出 
	} 
	else//正常 
	{ 
		//检测亚像素级角点,使用(11*2+1)×(11*2+1)=23×23大小的搜索窗口;死区的一半尺寸设置为无死区;迭代终止条件最大迭代次数类型和设定的精度(亚像素级精度为像素的十分之一)类型组合 
		cvFindCornerSubPix(view_gray2.cvimage, image_points_buf, count2, cvSize(11, 11), 
			cvSize(-1, -1), cvTermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1)); 
		image_points_seq.push_back(image_points_buf, count2);//检测到的所有角点依顺序入栈进行保存 
		//在图像上显示角点位置 
		cvDrawChessboardCorners(view2.cvimage, board_size, image_points_buf, count2, 1);//patternWasFound设置为1 
		view2.show("calib2");//显示 
		cvWaitKey();//等待按键	 
	} 
	delete[]image_points_buf;//释放内存 
	view1.close();//释放当前图片 
	view2.close();//释放当前图片 
	Matrix<double> image1_points(1, count1, 2);//标定点在图像内的坐标矩阵 
	Matrix<double> image2_points(1, count2, 2); 
	//分别把两张图片标定点在图像内亚像素坐标归类 
	for (int i = 0; i<image_points_seq.cvseq->total; i++) 
	{ 
		if (i < count1) 
		{ 
			image1_points(0, i, 0) = image_points_seq[i].x; 
			image1_points(0, i, 1) = image_points_seq[i].y; 
		} 
		else 
		{ 
			image2_points(0, i - count1, 0) = image_points_seq[i].x; 
			image2_points(0, i - count1, 1) = image_points_seq[i].y; 
		} 
	} 
	//计算基本矩阵 
	//method为CV_FM_RANSAC;param1为1.0;param2 为0.99;status为NULL 
	cvFindFundamentalMat(image1_points.get_cols(0, count1 - 1).cvmat,  
		                 image2_points.get_cols(0, count2 - 1).cvmat,  
	                     fundamental_matrix,CV_FM_RANSAC, 1.0, 0.99,  NULL); 
	cout << "基本矩阵为:" << endl;//屏显 
	for (int i = 0; i < fundamental_matrix->rows; i++) 
	{ 
		for (int j = 0; j < fundamental_matrix->cols; j++) 
		{ 
			cout << cvmGet(fundamental_matrix, i, j) << '\t'; 
		} 
		cout << endl; 
	} 
	//为相机外部参数赋值。为简化计算,世界坐标系和图像坐标系映射未考虑畸变 
	cvmSet(R1, 0, 0, -0.912606); 
	cvmSet(R1, 0, 1, 0.0207166); 
	cvmSet(R1, 1, 0, 0.00885129); 
	cvmSet(R1, 1, 1, 0.999482); 
	cvmSet(R1, 2, 0, -0.408745); 
	cvmSet(R1, 2, 1, -0.0246103); 
	cvmSet(R2, 0, 0, -0.898942); 
	cvmSet(R2, 0, 1, 0.0146055); 
	cvmSet(R2, 1, 0, 0.0220261); 
	cvmSet(R2, 1, 1, 0.999687); 
	cvmSet(R2, 2, 0, 0.437514); 
	cvmSet(R2, 2, 1, -0.0203188); 
	cvmSet(T1, 0, 0, 1181.84); 
	cvmSet(T1, 1, 0, -445.459); 
	cvmSet(T1, 2, 0, 11289.8); 
	cvmSet(T2, 0, 0, 1330.92); 
	cvmSet(T2, 1, 0, -492.466); 
	cvmSet(T2, 2, 0, 12094.8); 
	IplImage *combine = cvLoadImage("IMG_combine.JPG", 1);//读入拼接图 
	cvNamedWindow("result", 1);//定义窗口 
	cvSetMouseCallback("result", on_mouse, combine);//调用鼠标回调函数 
	cvShowImage("result", combine);//显示 
    cvWaitKey(0); 
    cvDestroyAllWindows();//销毁窗口 
	cvReleaseImage(&combine);//销毁图片 
	 
	system("pause");//等待按键退出 
	return 0; 
}