利用cvFindExtrinsicCameraParams2求取相机外参数

cvFindExtrinsicCameraParams2函数的定义：

1 void cvFindExtrinsicCameraParams2( const CvMat* object_points,
2                                    const CvMat* image_points,
3                                    const CvMat* intrinsic_matrix,
4                                    const CvMat* distortion_coeffs,
5                                    CvMat* rotation_vector,
6                                    CvMat* translation_vector );

  1 #include <cv.h>
  2 #include <highgui.h>
  3 #include <stdio.h>
  4 #include <stdlib.h>
  5 
  6 int n_boards = 0; //Will be set by input list
  7 const int board_dt = 10;
  8 int board_w;
  9 int board_h;
 10 
 11 
 12 void help()
 13 {
 14     printf("Calibration from disk. Call convention:\n\n"
 15                     "Where: board_{w,h} are the # of internal corners in the checkerboard\n"
 16             "       width (board_w) and height (board_h)\n"
 17             "       image_list is space separated list of path/filename of checkerboard\n"
 18             "       images\n\n"
 19             "Hit 'p' to pause/unpause, ESC to quit.  After calibration, press any other key to step through the images\n\n");
 20 }
 21 
 22 int main(int argc, char* argv[])
 23 {
 24   
 25   CvCapture* capture;// = cvCreateCameraCapture( 0 );
 26  // assert( capture );
 27 
 28     if(argc != 4)
 29     {
 30         help();
 31         return -1;
 32     }
 33     help();
 34     board_w = atoi(argv[1]);
 35     board_h = atoi(argv[2]);
 36     int board_n  = board_w * board_h;
 37     CvSize board_sz = cvSize( board_w, board_h );
 38     FILE *fptr = fopen(argv[3],"r");
 39     char names[2048];
 40     //COUNT THE NUMBER OF IMAGES:
 41     while(fscanf(fptr,"%s ",names)==1)
 42     {
 43         n_boards++;
 44     }
 45     rewind(fptr);
 46 
 47   cvNamedWindow( "Calibration" );
 48   //ALLOCATE STORAGE
 49   CvMat* image_points      = cvCreateMat(board_n,2,CV_32FC1);
 50   CvMat* object_points     = cvCreateMat(board_n,3,CV_32FC1);
 51   //CvMat* point_counts      = cvCreateMat(1,CV_32SC1);
 52 
 53 
 54   CvMat *intrinsic = (CvMat*)cvLoad("Intrinsicsr.xml");
 55   CvMat *distortion = (CvMat*)cvLoad("Distortionr.xml");
 56   CvMat* rotation_vector=cvCreateMat(3,1,CV_32FC1);
 57   CvMat* translation_vector=cvCreateMat(3,1,CV_32FC1);
 58   CvMat* rotation_mat=cvCreateMat(3,3,CV_32FC1);
 59   CvMat* jacobian=cvCreateMat(3,1,CV_32FC1);
 60 
 61 
 62 
 63   IplImage* image = 0;// = cvQueryFrame( capture );
 64   IplImage* gray_image = 0; //for subpixel
 65   CvPoint2D32f* corners = new CvPoint2D32f[ board_n ];
 66   int corner_count;
 67   
 68       fscanf(fptr,"%s ",names);
 69       image = cvLoadImage( names);
 70       if(gray_image == 0  && image) //We'll need this for subpixel accurate stuff
 71           gray_image = cvCreateImage(cvGetSize(image),8,1);
 72 
 73       if(!image)
 74           printf("null image\n");
 75 
 76       int found = cvFindChessboardCorners(
 77         image,
 78         board_sz,
 79         corners,
 80         &corner_count, 
 81         CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS
 82               );
 83     
 84       //Get Subpixel accuracy on those corners
 85       cvCvtColor(image, gray_image, CV_BGR2GRAY);
 86       cvFindCornerSubPix(gray_image, corners, corner_count, 
 87               cvSize(11,11),cvSize(-1,-1), cvTermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
 88       //Draw it
 89 
 90       cvDrawChessboardCorners(image, board_sz, corners, corner_count, found);
 91       cvShowImage( "Calibration", image );
 92        if( corner_count == board_n ) 
 93           printf("Found = %d for %s\n",found,names);
 94         for( int i=0; i<board_n; ++i )
 95           {
 96            CV_MAT_ELEM(*image_points, float,i,0) = corners[i].x;
 97            CV_MAT_ELEM(*image_points, float,i,1) = corners[i].y;
 98            CV_MAT_ELEM(*object_points,float,i,0) = i/board_w;
 99            CV_MAT_ELEM(*object_points,float,i,1) = i%board_w;
100            CV_MAT_ELEM(*object_points,float,i,2) = 0.0f;
101           }
102     
103         cvFindExtrinsicCameraParams2(
104                             object_points,image_points,intrinsic,distortion,rotation_vector,translation_vector
105                             );
106         cvRodrigues2(
107           rotation_vector,rotation_mat,jacobian=NULL
108                      );
109 cvSave("Rotationr.xml",rotation_mat);
110 cvSave("Translationr.xml",translation_vector);      
111   }