初始内参矩阵

相机内参矩阵：

        fx    s    x0
    K =  0    fy   y0
         0    0    1

其中，fx，fy为焦距，一般设二者相等，初始设为图像的长边长。
x0、y0为主点坐标（相对于成像平面），为图像中心点坐标。
s为畸变系数，初始设为0

	int Focalf = max(image.rows, image.cols);//初始焦距

	Mat K (Matx33d (
		Focalf, 0, img1.rows/2,
		0, Focalf, img1.cols/2,
		0, 0, 1)); // 相机矩阵(初始内参)

上面的哪个焦距是很粗糙的，
下面用一个 jhead.exe 来读取图像中的一些信息来计算得到  一个比较准确的焦距：

	system("jhead et000.jpg >et000.tmp");//运行 jhead
	printf("焦距(像数) = %0.3f\n", getfocal(name));//显示焦距

getfocal 函数

float getfocal(  
        const char * name
){  
	//char make[50];//厂商
	//char model[50];//型号
	float focal_mm=0.0f;//焦距（毫米）
	float ccd_width_mm=0.0f;//ccd宽（毫米）
	int res_x=0;//x分辨率
	int res_y=0;//y分辨率
	float focal_pixels=0.0f;//焦距（像数）


	//printf("装入 相机参数 文件 %s...\n", name);  



	FILE * file = fopen(name, "r");  
	if( file == NULL ){  
			printf("错误，没有打开文件\n");  
			return focal_pixels;  
	}  

	char lineHeader[128];
	char* res ;
	int i=0;char*t;
	char tt[60];
	while( 1 ){  
  
		//printf("%d\n",i++);//行号

		// 读这行的第一个字串  
		res= fgets(lineHeader, 128, (FILE*)file);
		if (res == NULL)  
				break; // EOF = End Of File. 文件结束。退出循环。
		//printf("：%s\n",memcpy(lineHeader,  strchr(lineHeader, ':'), 3) );


		//strstr(s1, s2);
		//返回一个指针，指向字符串 s1 中字符串 s2 的第一次出现的位置。

		if ( strstr( lineHeader, "Camera make" )  ){  //厂商

			t=strchr(lineHeader, ':');
			if(t){
				t++;//跳过':'
				memcpy(tt,  t, t-lineHeader);
				//printf("厂商：%s\n",tt);
			}

		}
		else if ( strstr( lineHeader, "Camera model" )  ){  //型号
			t=strchr(lineHeader, ':');
			if(t){
				t++;//跳过':'
				memcpy(tt,  t, t-lineHeader);
				//printf("型号：%s\n",tt);
			}
		}else if ( strstr( lineHeader, "Focal length" )  ){  //焦距（毫米）
			t=strchr(lineHeader, ':');
			if(t){
				t++;//跳过':'
				memcpy(tt,  t, t-lineHeader);
				//printf("焦距：%s\n",tt);
				t=strstr( tt, "mm" );
				if (t)
				{
					char temp[50];int t0=t-tt;
					memcpy(temp,  tt, t0);//复制 mm 前的
					//printf("焦距：%s\n",temp);
					t=temp;t+=t0;*t='\0';//删除尾部乱码
					//printf("焦距：%s\n",temp);
					t=temp;while (*t == ' ' )  {t++;t0--;}//删除行首空格
					memcpy(tt,  t, t0);t=tt;t+=t0;*t='\0';
					//printf("焦距：%smm\n",tt);
					focal_mm=atof(tt);
					//printf("焦距(浮点数)：%0.3fmm\n",focal_mm);

				}
			}
		}else if ( strstr( lineHeader, "CCD width" )  ){  //CCD 宽
			t=strchr(lineHeader, ':');
			if(t){
				t++;//跳过':'
				memcpy(tt,  t, t-lineHeader);
				//printf("焦距：%s\n",tt);
				t=strstr( tt, "mm" );
				if (t)
				{
					char temp[50];int t0=t-tt;
					memcpy(temp,  tt, t0);//复制 mm 前的
					
					t=temp;t+=t0;*t='\0';//删除尾部乱码
					
					t=temp;while (*t == ' ' )  {t++;t0--;}//删除行首空格
					memcpy(tt,  t, t0);t=tt;t+=t0;*t='\0';
					//printf("CCD 宽：%smm\n",tt);
					ccd_width_mm=atof(tt);
					//printf("CCD 宽(浮点数)：%0.3fmm\n",ccd_width_mm);

				}
			}
		}else if ( strstr( lineHeader, "Resolution" )  ){  //分辨率
			t=strchr(lineHeader, ':');
			if(t){
				t++;//跳过':'
				int t0=t-lineHeader;
				memcpy(tt,  t, t0);t=tt;t+=t0;*t='\0';
				//printf("分辨率：%s\n",tt);
				t=strchr(tt, 'x');
				if(t){
					char temp[50];char *p0,*p1;
					p0=tt;p1=t-1;t0=p1-p0;//首尾
					memcpy(temp,  p0, t0);p0=temp;p0+=t0;*p0='\0';
					res_x=atoi(temp);
					//printf("分辨率x(整数)：%d\n",res_x);
					p0=t+1;//首址
					res_y=atoi(p0);
					//printf("分辨率y(整数)：%d\n",res_y);
				}
			}
		}

	}
	int has_focal;
    if (focal_mm == 0 || ccd_width_mm == 0 || res_x == 0) {
	has_focal = 0;
    } else {
	has_focal = 1;
    }
    if (res_x < res_y) {
		// 长宽比是错误的,则交换
		int tmp = res_x;
		res_x = res_y;
		res_y = tmp;
    }
    if (has_focal == 1) {
		// 以像素为单位计算焦距
		focal_pixels = res_x * (focal_mm / ccd_width_mm);
		//printf("焦距(像数) = %0.3f\n", focal_pixels);
    }
	return focal_pixels;
}