opencv关于摄像机标定的一段代码

网上没有现成的，OPENCV也没有提供完整的示例，自己整理了一下，贴出来记录。

但是这里cvFindChessboardCorners非常不稳定，不能工作， 是否要自己写呢？

#include "stdafx.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
// OpenCV
#include <cxcore.h>
#include <cv.h>
#include <highgui.h>
#include <cvaux.h>

void InitCorners3D(CvMat *Corners3D, CvSize ChessBoardSize, int Nimages, float SquareSize);

/* uncomment the following to drop frames to prevent delays */
#define DROP_FRAMES 1
#define MAX_PORTS 3
#define MAX_CAMERAS 1
#define NUM_BUFFERS 8

/* declarations for OpenCV */
IplImage                 *current_frame_rgb,grid;
IplImage                 *current_frame_gray;
IplImage                 *readOnlyImg;

int                       freeze = 0;

int image_width = 640;
int image_height = 480;

bool                      verbose = false;

// Calibration stuff
bool   calibration_done = false;
const CvSize    ChessBoardSize = cvSize(4,4);
float    SquareWidth = 101.6f; //in millimeters
const int    NImages = 20; //Number of images to collect
const   int NPoints = 4*4;

CvMat *intrinsics;
CvMat *distortion_coeff;
CvMat *rotation_vectors;
CvMat *translation_vectors;
CvMat *object_points;
CvMat *point_counts;
CvMat *image_points;

int
main(int argc, char *argv[])
{

  int counter = 0;
  int find_corners_result = 0;
  int captured_frames = 0;

  CvPoint2D32f corners[NImages*NPoints];
  int corner_count[NImages];

  CvFont font;
  cvInitFont( &font, CV_FONT_VECTOR0,5, 5, 0, 7, 8);
  
  intrinsics   = cvCreateMat(3,3,CV_32FC1);
  distortion_coeff  = cvCreateMat(1,4,CV_32FC1);
  rotation_vectors  = cvCreateMat(NImages,3,CV_32FC1);
  translation_vectors  = cvCreateMat(NImages,3,CV_32FC1);

  point_counts   = cvCreateMat(NImages,1,CV_32SC1);

  object_points  = cvCreateMat(NImages*NPoints,3,CV_32FC1);
  image_points   = cvCreateMat(NImages*NPoints,2,CV_32FC1);

  // Function to fill in the real-world points of the checkerboard
  InitCorners3D(object_points, ChessBoardSize, NImages, SquareWidth);

  
    CvCapture* capture = 0;
    
    if( argc == 1 || (argc == 2 && strlen(argv[1]) == 1 && isdigit(argv[1][0])))
        capture = cvCaptureFromCAM( argc == 2 ? argv[1][0] - '0' : 0 );
    else if( argc == 2 )
        capture = cvCaptureFromAVI( argv[1] );

    if( !capture )
    {
        fprintf(stderr,"Could not initialize capturing.../n");
        return -1;
    }
  
  // Initialize all of the IplImage structures
  current_frame_rgb = cvCreateImage(cvSize(image_width, image_height), IPL_DEPTH_8U, 3);
  IplImage *current_frame_rgb2 = cvCreateImage(cvSize(image_width, image_height), IPL_DEPTH_8U, 3);
  current_frame_gray = cvCreateImage(cvSize(image_width, image_height), IPL_DEPTH_8U, 1);
  readOnlyImg   = cvCreateImage(cvSize(image_width, image_height), IPL_DEPTH_8U, 3);
  IplImage *tempimg = cvCreateImage(cvSize(image_width, image_height), IPL_DEPTH_8U, 1);
  CvScalar e;
  e.val[0] =1.0;
  cvSet(tempimg,e,0);

  /* make the window */
  //cvZero(readOnlyImg);
  e.val[0] =255;
  e.val[1] =255;
  e.val[2] =255;
  cvSet(readOnlyImg,e,0);
  for(int i = 0;i<ChessBoardSize.width;i++)
   for(int j = 0;j<ChessBoardSize.height;j++)
   {
    int w =image_width/(ChessBoardSize.width);
    int h = image_height/(ChessBoardSize.height);

  if((i+j)%2==1)
     cvRectangle( readOnlyImg, cvPoint(w*i,h*j),cvPoint(w*(i+1)-1,h*(j+1)-1), CV_RGB(0,0,0),CV_FILLED, 8, 0 );
   }

  cvNamedWindow( "grid", 0);
  cvNamedWindow( "Window 0", 0);
  
  cvResizeWindow( "grid", 500,500);
  cvMoveWindow("Window 0", 100, 100);

  printf("WHEN THE COUNTER HITS 20, YOU'RE DONE /n");

  while (!calibration_done)
  {
    while (captured_frames < NImages)
    {
      if (verbose) printf("%d /n", counter);
      counter++;

 

   current_frame_rgb = cvQueryFrame( capture );
   //current_frame_rgb = cvLoadImage( "c://BoardStereoL3.jpg" );

   if( !current_frame_rgb )
    break;

     cvCvtColor(current_frame_rgb, current_frame_gray, CV_BGR2GRAY);

 //cvMul(current_frame_gray,tempimg,current_frame_gray,1.5);
 //cvAdaptiveThreshold( current_frame_gray, current_frame_gray,255,CV_ADAPTIVE_THRESH_MEAN_C, CV_THRESH_BINARY, 3, 5 );
 
 //cvThreshold( current_frame_gray, current_frame_gray,90,255,CV_THRESH_BINARY_INV);
 //cvThreshold( current_frame_gray, current_frame_gray,90,255,CV_THRESH_BINARY);

 int pos = 1;
 IplConvKernel* element = 0;
 const int element_shape = CV_SHAPE_ELLIPSE;
 element = cvCreateStructuringElementEx( pos*2+1, pos*2+1, pos, pos, element_shape, 0 );
 cvDilate(current_frame_gray,current_frame_gray,element,1);
 cvErode(current_frame_gray,current_frame_gray,element,1);
 cvReleaseStructuringElement(&element);

 cvShowImage("grid",current_frame_gray);
 
    
    find_corners_result = cvFindChessboardCorners(current_frame_gray,
                                          ChessBoardSize,
                                          &corners[captured_frames*NPoints],
                                          &corner_count[captured_frames],
                                          0);

      if (find_corners_result !=0)
      {
    
        cvDrawChessboardCorners(current_frame_rgb, ChessBoardSize, &corners[captured_frames*NPoints], NPoints, find_corners_result);

        if (counter%10 == 0)
        // I do this to give you time to move the checkerboard around between captures
        {
          CvTermCriteria corner_criteria = cvTermCriteria(CV_TERMCRIT_ITER, 100, 0.1);
    
          cvFindCornerSubPix( current_frame_gray,&corners[captured_frames*NPoints],
                              NPoints, cvSize(3,3), cvSize(-1,-1),
                              corner_criteria );
                        
          cvPutText(current_frame_rgb, "FLASH!" , cvPoint(100, 300), &font, CV_RGB(255,0,0));
          printf("%d ... /n", captured_frames+1);
          // NOTE: I put the little "FLASH" thing in here so that you know when it takes the snapshot
          //       I'd gotten used to the little flash when using DirectShow for intrinsic calibration
          captured_frames++;
        }
      }
  
      cvShowImage("Window 0",current_frame_rgb);
   
   
      cvWaitKey(100);

      find_corners_result = 0;
    }    
 
    printf("/n");
    
    cvSetData( image_points, corners, sizeof(CvPoint2D32f));
    cvSetData( point_counts, corner_count, sizeof(int));
      
    cvCalibrateCamera2( object_points,
                        image_points,
                        point_counts,
                        cvSize(640,480),
                        intrinsics,
                        distortion_coeff,
                        rotation_vectors,
                        translation_vectors,
                        0);
                        
  calibration_done = true;                        
  }

  //
  // All this below is used for dumping the parameters to the screen
  //

  float intr[3][3] = {0.0};
  float dist[4] = {0.0};
  float tranv[3] = {0.0};
  float rotv[3] = {0.0};

  for ( int i = 0; i < 3; i++)
  {
    for ( int j = 0; j < 3; j++)
    {
      intr[i][j] = ((float*)(intrinsics->data.ptr + intrinsics->step*i))[j];
    }
      dist[i] = ((float*)(distortion_coeff->data.ptr))[i];
      tranv[i] = ((float*)(translation_vectors->data.ptr))[i];
      rotv[i] = ((float*)(rotation_vectors->data.ptr))[i];
  }
  dist[3] = ((float*)(distortion_coeff->data.ptr))[3];

  printf("-----------------------------------------/n");
  printf("INTRINSIC MATRIX: /n");
  printf("[ %6.4f %6.4f %6.4f ] /n", intr[0][0], intr[0][1], intr[0][2]);
  printf("[ %6.4f %6.4f %6.4f ] /n", intr[1][0], intr[1][1], intr[1][2]);
  printf("[ %6.4f %6.4f %6.4f ] /n", intr[2][0], intr[2][1], intr[2][2]);
  printf("-----------------------------------------/n");
  printf("DISTORTION VECTOR: /n");
  printf("[ %6.4f %6.4f %6.4f %6.4f ] /n", dist[0], dist[1], dist[2], dist[3]);
  printf("-----------------------------------------/n");
  printf("ROTATION VECTOR: /n");
  printf("[ %6.4f %6.4f %6.4f ] /n", rotv[0], rotv[1], rotv[2]);
  printf("TRANSLATION VECTOR: /n");
  printf("[ %6.4f %6.4f %6.4f ] /n", tranv[0], tranv[1], tranv[2]);
  printf("-----------------------------------------/n");

  exit(0);
  cvDestroyAllWindows();
}

void InitCorners3D(CvMat *Corners3D, CvSize ChessBoardSize, int NImages, float SquareSize)
{
  int CurrentImage = 0;
  int CurrentRow = 0;
  int CurrentColumn = 0;
  int NPoints = ChessBoardSize.height*ChessBoardSize.width;
  float * temppoints = new float[NImages*NPoints*3];

  // for now, assuming we're row-scanning
  for (CurrentImage = 0 ; CurrentImage < NImages ; CurrentImage++)
  {
    for (CurrentRow = 0; CurrentRow < ChessBoardSize.height; CurrentRow++)
    {
      for (CurrentColumn = 0; CurrentColumn < ChessBoardSize.width; CurrentColumn++)
      {
    temppoints[(CurrentImage*NPoints*3)+(CurrentRow*ChessBoardSize.width + CurrentColumn)*3]=(float)CurrentRow*SquareSize;
    temppoints[(CurrentImage*NPoints*3)+(CurrentRow*ChessBoardSize.width + CurrentColumn)*3+1]=(float)CurrentColumn*SquareSize;
    temppoints[(CurrentImage*NPoints*3)+(CurrentRow*ChessBoardSize.width + CurrentColumn)*3+2]=0.f;
      }
    }
  }
  (*Corners3D) = cvMat(NImages*NPoints,3,CV_32FC1, temppoints);
}