Opencv攝像頭標定（參數+校正），但cvFindChessboardCorners不太穩定，必須標版

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

using namespace std;

int n_boards=0;//圖像數目   canshu
 const int board_dt=20;//等20幀每棋盤視圖  xin
 //int sn_board=0;//成功找到角點的圖像數目
 int board_w;//圖像的角點行列數   canshu
 int board_h; //canshu
int main(int argc ,char * argv[])
{

 /*讀入圖像*/
 
CvCapture*capture;  //xin

 if(argc!=4)//xin
{
	printf("ERROR:Wrong number of input parameters\n");//xin
	return -1;                       //xin
}
board_w=atoi(argv[1]);      //canshu
board_h=atoi(argv[2]);      //canshu
n_boards=atoi(argv[3]);     //canshu

int board_n=board_h*board_w;//每幅圖像的角點數
CvSize board_size=cvSize(board_w,board_h);//每幅圖像的角點數
capture=cvCreateCameraCapture(0);//xin
//assert(capture);              //xin

if(!capture) { printf("\nCouldn't open the camera\n"); return -1;}
cvNamedWindow("MyCalib");
cvNamedWindow("Raw Video");

 //allocate storage
 CvMat*object_points     = cvCreateMat(board_n*n_boards,3,CV_32FC1);
 CvMat*image_points      =cvCreateMat(board_n*n_boards,2,CV_32FC1);
 CvMat*point_counts      =cvCreateMat(n_boards,1,CV_32SC1);
 CvMat * camera_matrix   =cvCreateMat(3,3,CV_32FC1);//攝像機矩陣（內參數矩陣）    分配矩陣空間（行，列，類型）類型為矩陣元素類型
 CvMat * distortion_coeffs=cvCreateMat(4,1,CV_32FC1);//畸變系數

 CvPoint2D32f*corners=new CvPoint2D32f[board_n];//一幅圖像的角點數組
 int corner_count;
 int successes=0;     //xin//   本文 successes就是 sn_board
 int step,frame=0;    //xin

 IplImage*image=cvQueryFrame(capture); //圖像的大小                   //  xin
 IplImage *gray_image=cvCreateImage(cvGetSize(image),8,1);//創建頭並分配數據//tong SourceImgGray
 
 //亞像素subpixel
 //capture corner views  loop  until  we've got n_boards
 //成功捕獲（找到所有角點on the board）

 while (successes<n_boards)
 {   //skip every board_dt frames to allow user to move chessboard
	if(frame++%board_dt==0)
	{
		int found=cvFindChessboardCorners(image,board_size,corners,&corner_count,CV_CALIB_CB_ADAPTIVE_THRESH|CV_CALIB_CB_FILTER_QUADS);

		//精確到亞像素
		cvCvtColor(image,gray_image,CV_BGR2GRAY);            //xin 
		cvFindCornerSubPix(image,corners,corner_count,cvSize(15,15),cvSize(-1,-1),cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,30,0.1));
	    cvDrawChessboardCorners(image,board_size,corners,corner_count,found); //繪制棋盤角點。found
		//cvShowImage("MyCalib",image);


		//如果得到一個好板，則把把它加入到我們的數據中
		if(corner_count==board_n){   //xin
			 cvShowImage( "Calibration", image ); //show in color if we did collect the image
			step=successes*board_n;   //xin
			for(int i=step,j=0;j<board_n;++i,++j){   //xinjia   canshu  i
				CV_MAT_ELEM(*image_points,float,i,0)  =corners[j].x;//CV_MAT_ELEM用來訪問矩陣每個元素的宏，這個宏只對單通道矩陣有效，多通道會報錯
                CV_MAT_ELEM(*image_points,float,i,1)  =corners[j].y;//(要訪問的矩陣，矩陣元素的類型，所要訪問元素的行數，列數)
                                         //上兩句是將檢測到的點以坐標形式存儲
	            CV_MAT_ELEM(*object_points,float,i,0) =float(j/board_w);
                CV_MAT_ELEM(*object_points,float,i,1) =float(j%board_w);
	                                    //上兩句是將檢測到的點的數目，以行列形式存儲
                CV_MAT_ELEM(*object_points,float,i,2) =0.0f;
			}
			 CV_MAT_ELEM(*point_counts,int,successes,0)=board_n;
			 successes++;
             printf("Collected our %d of %d needed chessboard images\n",successes,n_boards);
       }
       else
         cvShowImage( "Calibration", gray_image );//Show Gray if we didn't collect the image
      }//end skip board_dt between chessboard capture
	
	
	
	
	//暫停模塊handle pause/unpause and ESC    //xin
	 int c = cvWaitKey(15);
     if(c == 'p'){  
       c = 0;
       while(c != 'p' && c != 27){
            c = cvWaitKey(250);
       }
     }
     if(c == 27)
        return 0;
    image = cvQueryFrame( capture ); //Get next image
    cvShowImage("Raw Video", image);
  } //END COLLECTION WHILE LOOP.
   cvDestroyWindow("Calibration");
    printf("\n\n*** CALLIBRATING THE CAMERA...");


	 //重新賦值，又重新分配了矩陣空間
  CvMat * object_points0= cvCreateMat(board_n*successes,3,CV_32FC1); 
  CvMat * image_points0=cvCreateMat(board_n*successes,2,CV_32FC1);
  CvMat * point_counts0=cvCreateMat(successes,1,CV_32SC1);
	
  //transfer THE points INTO THE correct size matrices
   for (int i=0;i<successes*board_n;i++)//board_n為每幅圖像的角點數*sn_board為成功找到角點的圖像數目
    {
    CV_MAT_ELEM(*image_points0,float,i,0)=CV_MAT_ELEM(*image_points,float,i,0);
    CV_MAT_ELEM(*image_points0,float,i,1)=CV_MAT_ELEM(*image_points,float,i,1);
    CV_MAT_ELEM(*object_points0,float,i,0)=CV_MAT_ELEM(*object_points,float,i,0);
    CV_MAT_ELEM(*object_points0,float,i,1)=CV_MAT_ELEM(*object_points,float,i,1);
    CV_MAT_ELEM(*object_points0,float,i,2)=0.0f;
    }
  for (int i=0;i<successes;i++)
   {
    CV_MAT_ELEM(*point_counts0,int,i,0)=CV_MAT_ELEM(*point_counts,int,i,0);
   }
   cvReleaseMat(&object_points);//世界坐標點
   cvReleaseMat(&point_counts); //每張圖像中的角點數
   cvReleaseMat(&image_points); //圖像坐標點
   //使用cvReleaseMat函數之后，系統將釋放剛才載入矩陣的內存空間（也即這個矩陣已經不在內存中了）


   // At this point we have all of the chessboard corners we need.
   
   
   // Initialize the intrinsic matrix such that the two focal
  // lengths have a ratio of 1.0
  //
    CV_MAT_ELEM( *camera_matrix, float, 0, 0 ) = 1.0f;   // xin
    CV_MAT_ELEM( *camera_matrix, float, 1, 1 ) = 1.0f;   //xin

  //CALIBRATE THE CAMERA!
  cvCalibrateCamera2(                                   //xin
      object_points0, image_points0,                    //xin
      point_counts0,  cvGetSize( image ),               
      camera_matrix, distortion_coeffs,
      NULL, NULL,0  //CV_CALIB_FIX_ASPECT_RATIO
  );

   // SAVE THE INTRINSICS AND DISTORTIONS     xin
   printf("......保存內部參數與畸變參數\n");
   cvSave("camera_matrix1111.xml",camera_matrix);
   cvSave("distortion_coeffs.xml",distortion_coeffs);

   // EXAMPLE OF LOADING THESE MATRICES BACK IN:      xin
  CvMat *intrinsic = (CvMat*)cvLoad("Intrinsics.xml");
  CvMat *distortion = (CvMat*)cvLoad("Distortion.xml");


  // Build the undistort map which we will use for all 
  // subsequent frames.建立不失真的圖map
  //
  IplImage* mapx = cvCreateImage( cvGetSize(image), IPL_DEPTH_32F, 1 );
  IplImage* mapy = cvCreateImage( cvGetSize(image), IPL_DEPTH_32F, 1 );
  cvInitUndistortMap(intrinsic,distortion,mapx, mapy);

  // Just run the camera to the screen, now showing the raw and
  // the undistorted image.
  //
  cvNamedWindow( "Undistort" );
  while(image) {
    IplImage *t = cvCloneImage(image);
    cvShowImage( "Raw Video", image ); // Show raw image
    cvRemap( t, image, mapx, mapy );     // Undistort image
    cvReleaseImage(&t);
    cvShowImage("Undistort", image);     // Show corrected image

  //Handle pause/unpause and ESC
    int c = cvWaitKey(15);
    if(c == 'p'){ 
       c = 0;
       while(c != 'p' && c != 27){
            c = cvWaitKey(250);
       }
    }
     if(c == 27)
        break;
    image = cvQueryFrame( capture );
  } 

  return 0;
}