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;
}