摄像机标定

主要是学习OPENCV一书中的例程，例程有部分错误，已修改过来了：

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

int n_boards = 0;     //will be set by input list
const int board_dt = 20;    //wait 20 frames per chessboard view
int board_w;
int board_h;

int main(int argc, char** argv)
{
 if (argc != 4)
 {
  printf("ERROR: Wrong number of input parameters\n");
  return -1;
 }

 board_w = atoi(argv[1]);
 board_h = atoi(argv[2]);
 n_boards = atoi(argv[3]);
 int board_n = board_w * board_h;
 CvSize board_sz = cvSize(board_w, board_h);
 CvCapture* capture = cvCreateCameraCapture(0);

 if (!capture)
 {
  printf("不能从摄影像头获取图像\n");
  return -1;
 }
// assert (capture);
 
 cvNamedWindow("calibration");
 //allocate storage
 CvMat* image_points = cvCreateMat(n_boards * board_n, 2, CV_32FC1);
 CvMat* object_points = cvCreateMat(n_boards * board_n, 3, CV_32FC1);
 CvMat* point_counts = cvCreateMat(n_boards, 1, CV_32SC1);
 CvMat* intrinsic_matrix = cvCreateMat(3, 3, CV_32FC1);
 CvMat* distortion_coeffs = cvCreateMat(5, 1, CV_32FC1);

 CvPoint2D32f* corners = new CvPoint2D32f[board_n];
 int corner_count;
 int successes = 0;
 int step;
 int frame =0;

 IplImage* image = cvQueryFrame(capture);
 IplImage* gray_image = cvCreateImage(cvGetSize(image), 8, 1);

 //CAPTURE CORNER VIEWS LOOP UNTIL WE'VE GOT n_boards
 //SUCCESSFUL CAPTURES (ALL CORNERS ON THE BOARD ARE FOUND)
 //
 while(successes < n_boards)
 {
  //skip every board_dt frames to allow user to move chessboard
  if (frame++ % board_dt == 0)
  {
   //find chessboard corners:
   int found = cvFindChessboardCorners(image, board_sz, corners, &corner_count,
    CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS);

   //get subpixel accuracy on those corners
   cvCvtColor(image, gray_image, CV_RGB2GRAY);
   cvFindCornerSubPix(gray_image, corners, corner_count, cvSize(11, 11), cvSize(-1, -1),
    cvTermCriteria(CV_TERMCRIT_EPS + CV_CALIB_CB_FILTER_QUADS, 30, 0.1));

   //draw it
   cvDrawChessboardCorners(image, board_sz, corners, corner_count, found);
   cvShowImage("calibration", image);

   //if we got a good board, add it to our data
   if (corner_count == board_n)
   {
    step = successes * board_n;
    for (int i = step, j = 0; j < board_n; ++i, ++j)
    {
     CV_MAT_ELEM(*image_points, float, i, 0) = corners[j].x;
     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++;
   }
  }   // end skip board_dt between chessboard capture  
  
  //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)
  {
   return 0;
   image = cvQueryFrame(capture);  //get next image
  }
 }        //end collection while loop

 // allocate matrices according to how many chessboards found
 CvMat* object_points2 = cvCreateMat(successes * board_n, 3, CV_32FC1);
 CvMat* image_points2 = cvCreateMat(successes * board_n, 2, CV_32FC1);
 CvMat* point_counts2 = cvCreateMat(successes, 1, CV_32SC1);

 //transfer the points into ther correct size matrices
 //below, we weite out the details in the next two loops.we could instead have written:
 //image_points->rows = object_points->rows = siccesses*board_n; point_counts->rows = successes;

 for (int i = 0; i < successes * board_n; ++i)
 {
  CV_MAT_ELEM(*image_points2, float, i, 0) = CV_MAT_ELEM(*image_points, float, i, 0);
  CV_MAT_ELEM(*image_points2, float, i, 1) = CV_MAT_ELEM(*image_points, float, i, 1);
  CV_MAT_ELEM(*object_points2, float, i, 0) = CV_MAT_ELEM(*image_points, float, i, 0);
  CV_MAT_ELEM(*object_points2, float, i, 1) = CV_MAT_ELEM(*image_points, float, i, 1);
  CV_MAT_ELEM(*object_points2, float, i, 2) = CV_MAT_ELEM(*image_points, float, i, 2);
 }
 for (i = 0; i < successes; ++i)
 {
  CV_MAT_ELEM(*point_counts2, int, i, 0) = CV_MAT_ELEM(*point_counts, int, i, 0);
 }
 cvReleaseMat(&object_points);
 cvReleaseMat(&image_points);
 cvReleaseMat(&point_counts);
 
 // at this point we have all of the chessboard corners we need
 //initialize the intrinsic matrix such that the two focal
 //lengths have aratio of 1.0
 //
 CV_MAT_ELEM(*intrinsic_matrix, float, 0, 0) = 1.0f;
 CV_MAT_ELEM(*intrinsic_matrix, float, 1, 1) = 1.0f;

 //calibrate the camera!
 cvCalibrateCamera2(object_points2, image_points2, point_counts2, cvGetSize(image),
      intrinsic_matrix, distortion_coeffs, NULL, NULL, 0);

 //save the intrinsics and distortions
 cvSave("intrinsics.xml", intrinsic_matrix);
 cvSave("distortion.xml", distortion_coeffs);

 //example of loading these matrices back in :
 CvMat* intrinsic = (CvMat*)cvLoad("intrinsics.xml");
 CvMat* distortion = (CvMat*)cvLoad("distortion.xml");

 //build the undistort map that we will use for all
 //subsequent frames.
 //
 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("calibration", 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;

}