opencv标定程序

/*标定的过程：
图片的加载
角点的检测
提取角点精确坐标
参数求解
利用参数对图像进行矫正
*/

#include
#include"cv.h"
#include"highgui.h"
#include

//函数声明
void PrintMat(CvMat *matrix,BOOL save_or_show,FILE *fp);
int main(int argc,char **argv)
{
int i=1;
char k=0;
int CurrentImage = 0;
int CurrentRow = 0; //行
int CurrentColumn = 0; //列
int findcorner_result=0;

FILE *fp; //文件指针


int ChessBoardSize_w =6; //角点个数
int ChessBoardSize_h =7;
int width_pixel =1280; //像素
int high_pixel =1024;

float SquareSize=10; //棋盘大小
int NImages=16;
CvSize ChessBoardSize;
CvSize image_pixel;
int NPoints=0;

int *corner_counter;
float *temppoints; //这里可以使用内存动态存储管理。。。
CvPoint2D32f *corners; //存储角点坐标的数组

//单通道灰度图像
IplImage *grayimage=0;
IplImage *srcimage=0; //三通道图像
IplImage *result_image=0; //矫正以后的图像


CvMat *intrinsic_matrix=0; //内部参数矩阵
CvMat *distortion_coeffs=0; //畸变系数
CvMat *rotation_vectors=0; //旋转向量
CvMat *translation_vectors=0;//平移向量
CvMat *points_counts=0; //图片角点数
CvMat *object_points=0; //世界坐标系中角点的坐标
CvMat *image_points=0; //检测到的角点坐标

CvMat *temp_matrix,*rotation_matrix,*translation_matrix;

//*****************************************************
//数据载入、开辟空间
image_pixel = cvSize(width_pixel,high_pixel);
ChessBoardSize = cvSize(ChessBoardSize_w,ChessBoardSize_h);
NPoints = ChessBoardSize_w*ChessBoardSize_h;

corner_counter=calloc(NImages,sizeof(int)); //动态管理，记着释放空间
temppoints =calloc(NImages*NPoints*3,sizeof(float));
corners =calloc(NImages*NPoints,sizeof(CvPoint2D32f));

if((corner_counter==0 )||(temppoints==0 )||( corners==0 ) )
{
return -1;
}
intrinsic_matrix = cvCreateMat(3,3,CV_32FC1); //内参数矩阵
distortion_coeffs = cvCreateMat(1,4,CV_32FC1); //形变参数

rotation_vectors = cvCreateMat(NImages,3,CV_32FC1); //旋转向量
translation_vectors = cvCreateMat(NImages,3,CV_32FC1); //平移向量


points_counts = cvCreateMat(NImages,1,CV_32SC1); //视图数目

object_points = cvCreateMat(NImages*NPoints,3,CV_32FC1);//世界坐标系中角点的坐标
image_points = cvCreateMat(NImages*NPoints,2,CV_32FC1);//检测到

的坐标点坐标

temp_matrix= cvCreateMat(1,3,CV_32FC1);
rotation_matrix = cvCreateMat(3,3,CV_32FC1); //旋转矩阵
translation_matrix = cvCreateMat(3,3,CV_32FC1); //旋转矩阵

grayimage=cvCreateImage(image_pixel,IPL_DEPTH_8U,1); //单通道灰度图像
result_image=cvCreateImage(image_pixel,IPL_DEPTH_8U,1); //校正以后的图像

fp = fopen( "data1.txt", "w+" ); //打开文件,建立一个文件，然后写入数据
fprintf(fp, "坐标数据：\n" ); //写入数据，写入文件
//****************************************************
//图片的加载以及角点的提取
if(argc!=NImages+1)
{
printf("图片加载有误!");
return -1;
}
for(CurrentImage=0;CurrentImage{
//加载图片
if((srcimage=cvLoadImage(argv[CurrentImage+1],1))!=0)
{
//色彩转换
cvCvtColor(srcimage,grayimage,CV_BGR2GRAY);
//角点检测
findcorner_result=cvFindChessboardCorners(grayimage,ChessBoardSize,&corners[CurrentImage*NPoints],
&corner_counter[CurrentImage],CV_CALIB_CB_ADAPTIVE_THRESH);
//画出检测到的点
cvDrawChessboardCorners(srcimage,ChessBoardSize,
&corners[CurrentImage*NPoints],corner_counter[CurrentImage],findcorner_result);
//精确坐标位置
cvFindCornerSubPix(grayimage,&corners[CurrentImage*NPoints],corner_counter[CurrentImage],
cvSize(10,10), cvSize(-1,-1),//这个搜索的范围。。。？
cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,500,0.003)//迭代终止条件
);
cvNamedWindow("image",1);
cvShowImage("image",srcimage);

printf("检测到的角点:%d\n",corner_counter[CurrentImage]);
//fprintf(fp,"检测到的角点:%d\n",corner_counter[CurrentImage]);
//for(i=0;i// {
// printf("第%d个角点 %f %f\n",i,corners[CurrentImage*NPoints+i].x,corners[CurrentImage*NPoints+i].y);
//}
cvWaitKey(0);
printf("按任意键提取下一幅图片角点。。。\n");
//fprintf(fp,"按任意键提取下一幅图片角点。。。\n");

}
}

printf("角点提取结束......\n");

printf("开始定标......\n");
fprintf(fp,"角点提取结束......\n");

fprintf(fp,"开始定标......\n");

//棋盘世界坐标系坐标
for(CurrentImage = 0 ; CurrentImage < NImages ; CurrentImage++)//图片
{
for (CurrentRow = 0; CurrentRow < ChessBoardSize_h; CurrentRow++)//行
{
for (CurrentColumn = 0; CurrentColum

n < ChessBoardSize_w; CurrentColumn++)//列
{
temppoints[(CurrentImage*NPoints*3)+(CurrentRow*ChessBoardSize_w+CurrentColumn)*3]=(float)(CurrentRow*SquareSize);
temppoints[(CurrentImage*NPoints*3)+(CurrentRow*ChessBoardSize_w+CurrentColumn)*3+1]=(float)(CurrentColumn*SquareSize);
temppoints[(CurrentImage*NPoints*3)+(CurrentRow*ChessBoardSize_w+CurrentColumn)*3+2]=0;
}
}
}
//参数求解
*object_points=cvMat(NImages*NPoints,3,CV_32FC1,temppoints);//似乎这里开始有点问题。。。还有就是在标定过程中这个棋盘的边长和标定的精度无关
cvSetData(image_points,corners,sizeof(CvPoint2D32f));//设置image_points矩阵的数据
cvSetData(points_counts,corner_counter,sizeof(int));

/*printf("棋盘坐标");
printf("\n");
PrintMat(object_points);
printf("\n计算机\n");
printf("\n");
PrintMat(image_points);
printf("\n");
printf("\n角点个数\n");
PrintMat(points_counts);
printf("\n");*/

cvCalibrateCamera2(object_points,image_points,points_counts,cvGetSize(grayimage),
intrinsic_matrix,distortion_coeffs,rotation_vectors,
translation_vectors,0
);

printf("\n内部岑数矩阵\n");
fprintf(fp,"\n内部岑数矩阵\n");

PrintMat(intrinsic_matrix,FALSE,NULL);
PrintMat(intrinsic_matrix,TRUE,fp);

printf("\n形变岑数\n");

fprintf(fp,"\n形变岑数\n");
PrintMat(distortion_coeffs,FALSE,NULL);
PrintMat(distortion_coeffs,TRUE,fp);



for(i=0;i{

cvGetRow(rotation_vectors,temp_matrix,i);
cvRodrigues2(temp_matrix,rotation_matrix,0);

printf(" 第%d个图片的旋转向量\n",i);
fprintf(fp," 第%d个图片的旋转向量\n",i);

PrintMat(temp_matrix,FALSE,NULL);
PrintMat(temp_matrix,TRUE,fp);

printf(" 第%d个图片的旋转矩阵\n",i);
fprintf(fp," 第%d个图片的旋转矩阵\n",i);

PrintMat(rotation_matrix,FALSE,NULL);
PrintMat(rotation_matrix,TRUE,fp);
printf("\n");
fprintf(fp,"\n");

}
//cvReleaseMat(&temp_matrix);
printf("平移矩阵\n");
fprintf(fp,"平移矩阵\n");

for(i=0;i{

cvGetRow(translation_vectors,temp_matrix,i);
cvRodrigues2(temp_matrix,translation_matrix,0);

printf(" 第%d个图片的平移向量\n",i);
fprintf(fp," 第%d个图片的平移向量\n",i);
PrintMat(temp_matrix,FALSE,NULL);
PrintMat(temp_matrix,TRUE,fp);

printf(" 第%d个图片的平移矩阵\n",i);
fprintf(fp," 第%d个图片的平移矩阵\n",i);
PrintMat(translation_matrix,FALSE,NULL);
PrintMat(translation_matrix,TRUE,fp);
printf("\n");
fprin

tf(fp,"\n");

}

// PrintMat(translation_vectors,FALSE,NULL);
// PrintMat(translation_vectors,TRUE,fp);

printf("标定结束.....\n");
fprintf(fp,"标定结束.....\n");

printf("按任意键开始误差分析.....\n");
fprintf(fp,"按任意键开始误差分析.....\n");
cvWaitKey(0);
//误差分析
//利用已知的内部参数，使用cvProjectPoints2()计算出世界坐标系中的坐标在图片中的坐标也就是校准后的坐标
//将校准后的坐标和原来求得的坐标进行比较
for(CurrentImage=0;CurrentImage{
CvMat *object_matrix= cvCreateMat(NPoints,3,CV_32FC1);
CvMat *image_matrix= cvCreateMat(NPoints,2,CV_32FC1);
CvMat *project_image_matrix= cvCreateMat(NPoints,2,CV_32FC1);
CvMat *rotation_matrix_1= cvCreateMat(1,3,CV_32FC1);
CvMat *translation_matrix_1= cvCreateMat(1,3,CV_32FC1);
CvMat *rotation_matrix= cvCreateMat(3,1,CV_32FC1);
CvMat *translation_matrix= cvCreateMat(3,1,CV_32FC1);
double err=0;

cvGetRows(object_points,object_matrix,CurrentImage*NPoints,(CurrentImage+1)*NPoints,1);
cvGetRow(rotation_vectors,rotation_matrix_1,CurrentImage);
cvReshape(rotation_matrix_1,rotation_matrix,0,3);
cvGetRow(translation_vectors,translation_matrix_1,CurrentImage);
cvReshape(translation_matrix_1,translation_matrix,0,3);
cvGetRows(image_points,project_image_matrix,CurrentImage*NPoints,(CurrentImage+1)*NPoints,1);

cvProjectPoints2(object_matrix,rotation_matrix,translation_matrix,
intrinsic_matrix,distortion_coeffs,image_matrix,0,0,0,0,0);

err=cvNorm(image_matrix,project_image_matrix,CV_L2,0);

printf("第%d幅图像的误差为%f\n",CurrentImage+1,err);
fprintf(fp,"第%d幅图像的误差为%f\n",CurrentImage+1,err);

}

cvNamedWindow("Undistort_image",1);
//显示校正后的图片
printf("\n");
printf("校正后的图片....\n");
for(CurrentImage=0;CurrentImage{
//加载图片
if((srcimage=cvLoadImage(argv[CurrentImage+1],1))!=0)
{
//色彩转换
cvCvtColor(srcimage,grayimage,CV_BGR2GRAY);
cvUndistort2(grayimage,result_image,intrinsic_matrix,distortion_coeffs);
cvShowImage("Undistort_image",result_image);
printf("按任意键显示下一幅图片。。。\n");
cvWaitKey(0);
}
}

//关闭文件
fclose(fp);
//释放内存
cvWaitKey(0);

free(corner_counter);
free(temppoints);
free(corners);
cvDestroyWindow("Undistort_image");
cvDestroyWindow("Image");

cvReleaseMat(&intrinsic_matrix);
cvReleaseMat(&distortion_co

effs);
cvReleaseMat(&rotation_vectors);
cvReleaseMat(&translation_vectors);
cvReleaseMat(&rotation_matrix);
cvReleaseMat(&translation_matrix);
cvReleaseMat(&points_counts);
cvReleaseMat(&object_points);
cvReleaseMat(&image_points );
cvReleaseMat(&temp_matrix );

cvReleaseImage(&srcimage);
cvReleaseImage(&result_image);
cvReleaseImage(&grayimage);


return -1;
}
/*********************************
函数名： PrintMat(CvMat *matrix)
函数输入：matrix指针 数据类型opencv规定的任意一个
函数作用：在屏幕上打印矩阵
**********************************/
void PrintMat(CvMat *matrix,BOOL save_or_show,FILE *fp)
{
int i=0;
int j=0;
for(i=0;i < matrix->rows;i++)//行
{
if (save_or_show)
{
fprintf(fp,"\n");
}
else
{
printf("\n");
}
switch(matrix->type&0X07)
{
case CV_32F:
case CV_64F:
{
for(j=0;jcols;j++)//列
{
if (save_or_show)
{
fprintf(fp,"%9.3f ",(float)cvGetReal2D(matrix,i,j));
}
else
{
printf("%9.3f ",(float)cvGetReal2D(matrix,i,j));
}

}


break;
}
case CV_8U:
case CV_16U:
{
for(j=0;jcols;j++)
{
printf("%6d ",(int)cvGetReal2D(matrix,i,j));
if (save_or_show)
{
fprintf(fp,"%6d ",(int)cvGetReal2D(matrix,i,j));
}
else
{
printf("%6d ",(int)cvGetReal2D(matrix,i,j));
}
}

break;
}

default:
break;

}
}

}
//*****************************