Camera Calibration摄像机棋盘标定(单副图)源程序

#include "stdafx.h"
#include <cv.h>
#include <string>
#include <fstream>
#include <iomanip>
#include "cvut/cvut.h"

using namespace std;
using namespace cvut;

int main()
{
 ifstream fin("calibration.txt");
 /************************************************************************/
 /*    read images, then find corners and subpix                       */
 /************************************************************************/
 CvSize board_size = cvSize(5,7); //棋盘标定板上的内角点个数分布 5*7
 CvSize image_size;
 CvPoint2D32f *cornersBuffer = new CvPoint2D32f[board_size.width * board_size.height];
 Seq<CvPoint2D32f> cornersSeq;
 string strFileName = "image//chess3.jpg";
 string str ;
 getline(fin,str);
 //cout << str << endl;
 Image<uchar> imageChess(str);
 ofstream fout("cali_result.txt",ios_base::out);
 if(!fout)
 {
  cout << "cannot open the file" << endl;
  exit(-1);
 }
 image_size.width = imageChess.width;
 image_size.height = imageChess.height;
 cout << "从" << strFileName << "图像标定摄像机..." << endl;
 fout << "从" << strFileName << "图像标定摄像机..." << endl;
 cout << "图像大小： " << '(' << image_size.width << ',' << image_size.height << ')' << endl;
 fout << "图像大小： " << '(' << image_size.width << ',' << image_size.height << ')' << endl;
 cout << "提取角点..." << endl;
 int count = 0; //
 //提取角点
 if(0 == cvFindChessboardCorners(imageChess.cvimage,board_size,cornersBuffer,&count,1))
 {
  cout << "cann't find corners..." << endl;
  return -1;
 }

 Image<uchar> image_gray(imageChess.size(),8,1);
 rgb2gray(imageChess,image_gray);
 //亚像素角点精确化
 cvFindCornerSubPix(image_gray.cvimage,cornersBuffer,count,cvSize(11,11),
  cvSize(-1,-1),cvTermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS,20,0.1));
 cornersSeq.push_back(cornersBuffer,count);
 //显示角点
 cvDrawChessboardCorners(imageChess.cvimage,board_size,cornersBuffer,count,1);
 imageChess.show("corners",100,100);
 int nTotal = cornersSeq.length();
 fout << "----corners is : " << endl;
 for (int i = 0; i < nTotal; i++)
 {
  cout << '(' << cornersSeq[i].x << ',' << cornersSeq[i].y << ')' << endl;
  fout << '(' << cornersSeq[i].x << ',' << cornersSeq[i].y << ')' << endl;
 }
 delete[] cornersBuffer;
 cout << "角点提取完成.." << endl;
 cvWaitKey();
 imageChess.close();
 //标定..
 cout << "开始标定摄像机..." << endl << endl;
 CvSize square_size = cvSize(10,10);  /* 实际测量得到的定标板上每个棋盘格的大小 */
 CvMat *objectMat = cvCreateMat(3,board_size.height*board_size.width,CV_32FC1);   //数目要相等
 CvMat *cornersMat = cvCreateMat(2,cornersSeq.cvseq->total,CV_32FC1);
 CvMat *intrinsic_mat = cvCreateMat(3,3,CV_32FC1);
 CvMat *distortion_mat = cvCreateMat(1,4,CV_32FC1);
 CvMat *rotation_mat = cvCreateMat(1,3,CV_32FC1);
 CvMat *translation_mat = cvCreateMat(1,3,CV_32FC1);
 CvMat *corner_count = cvCreateMat(1,1,CV_32SC1);
 //initialize
 CV_MAT_ELEM(*corner_count,int,0,0) = cornersSeq.cvseq->total;
 cout << "the number of corner is: " << CV_MAT_ELEM(*corner_count,int, 0, 0) << endl << endl;
 fout << "total: " << CV_MAT_ELEM(*corner_count,int, 0, 0) << endl << endl;
 cvZero(intrinsic_mat);
 cvZero(distortion_mat);
 cvZero(rotation_mat);
 cvZero(translation_mat);
 //初始化标定板上的三维坐标
 int i,j;
 for(i = 0; i < board_size.height; i++)
  for(j = 0; j < board_size.width; j++)
   //假定标定板放在世界坐标系中的z=0的平面上 要注意坐标轴..
  {
   cvmSet(objectMat,0,i*board_size.width+j,i*square_size.width); //x         o------->y
   cvmSet(objectMat,1,i*board_size.width+j,j*square_size.height); //y        |x
   cvmSet(objectMat,2,i*board_size.width+j,0); //z
  }
  //图像角点 矩阵方式表示
  for (i = 0; i < cornersSeq.cvseq->total; i++)
  {
   cvmSet(cornersMat,0,i,cornersSeq[i].x);
   cvmSet(cornersMat,1,i,cornersSeq[i].y);
  }
  //标定
  cvCalibrateCamera2(objectMat,cornersMat,corner_count,
   image_size,
   intrinsic_mat,
   distortion_mat,
   rotation_mat,
   translation_mat,
   0);

  cout << "标定完成..." << endl << endl;
  cout << "/t-------标定结果---------" << endl << endl;
  fout << "/t-------标定结果---------" << endl;

  //输出并保存结果
  cout << "intrinsic_matrix is:(K) ";
  fout << "intrinsic_matrix is:(K) ";
  for (i = 0; i < 3; i++)
  {
   cout << endl;
   fout << endl;
   for (j = 0; j < 3; j++)
   {
    cout << setw(6) << cvmGet(intrinsic_mat,i,j) << ' ';
    fout << setw(6) << cvmGet(intrinsic_mat,i,j) << ' ';
   }
  }
  cout << endl;
  fout << endl;
  cout << "distoration_matrix is:(k1, k2, p1, p2) " << endl;
  fout << "distoration_matrix is:(k1, k2, p1, p2) " << endl;
  for (i = 0; i < 4; i++)
  {
   cout << setw(6) << cvmGet(distortion_mat,0,i) << ' ';
   fout << setw(6) << cvmGet(distortion_mat,0,i) << ' ';
  }
  cout << endl << endl;
  fout << endl << endl;
  cout << "rotation_matrix is :(R) " << endl;
  fout << "rotation_matrix is :(R) " << endl;
  for(i = 0; i < 3; i++)
  {
   cout << setw(6) << cvmGet(rotation_mat,0,i) << ' ';
   fout << setw(6) << cvmGet(rotation_mat,0,i) << ' ';
  }
  cout << endl << endl;
  fout << endl << endl;
  cout << "translation_matrix is :(T) " << endl;
  fout << "translation_matrix is :(T) " << endl;
  for (i = 0; i < 3; i++)
  {
   cout << cvmGet(translation_mat, 0, i) << ' ';
   fout << cvmGet(translation_mat, 0, i) << ' ';
  }
  cout << endl << endl;
  CvMat *rotation = cvCreateMat(3,3,CV_32FC1);
  cvZero(rotation);
  //转换成矩阵表示
  cvRodrigues2(rotation_mat,rotation);
  cout << "rotation's matrix format: ";
  fout << "rotation's matrix format: ";
  for (i = 0; i < 3; i++)
  {
   cout << endl;
   fout << endl;
   for(j = 0; j < 3; j++)
   {
    cout << setw(6) << cvmGet(rotation,i,j) << ' ';
    fout << cvmGet(rotation,i,j) << ' ';
   }
  }
  cout << endl << endl;
  fout << endl << endl;
  fout.close();
  return 0;
}