opencv标定程序的代码（完全测试好的）

opencv标定程序的代码（完全测试好的）发布时间：2009-02-22 00:45:42技术类别：软件开发 

这个是我在win32 平台下写的程序，测试完全通过：
不过要注意下面的问题：
（1）opencv库是1.0的，我在vc6.0的平台使用
（2）软件的设置要正确
（3）图片载入的时候，使用的是批处理文件（其实就是建一个txt文件，里面写的和dos下面的操作代码一样的输入）
（4）里面现在还有个问题没有解决：在得到角点坐标的时候，cvFindCornerSubPix里面的搜索区域还不明白
（05）下面是我在网上搜的别人的代:测试通过的：
A.基于计算机视觉技术：http://space.ednchina.com/Upload/2009/2/22/4ca54c82-1ab2-44e2-a456-7f0d87c24b7a.rar


B.

C.

下面是我的代码：

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

#include<stdio.h>
#include"cv.h"
#include"highgui.h"
#include <stdlib.h> 

//函数声明
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<NImages;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<corner_counter[CurrentImage];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; CurrentColumn < 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<NImages;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<NImages;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");
          fprintf(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<NImages;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<NImages;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_coeffs);
   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;j<matrix->cols;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;j<matrix->cols;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;
     
      }
   }
     
}
//*****************************