标定相机参数

这是借鉴别人的程序，主体都是别人写的，我只做了整理，程序主要完成了9幅棋盘格图像的角点标定，然后利用9组标定的角点信息来计算相机的内参、外参，并最终给出校正后的图像。需要声明的是对9幅图像读取的过程，定义了char filename[]={0},另filename依次加1,2，...9,来实现ascii码中数字1向字母1的转换，这是作者的灵活之处，膜拜。如果需要程序使用的图片，告诉我，再次感谢原作者的慷慨。

#include<iostream>
#include<highgui.h>
#include "opencv2/core/core.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/highgui/highgui.hpp"
using namespace std;
using namespace cv;

//！！！！！需要设置的参数！！！！！/
//设置一 图像尺寸
int image_width = 640;
int image_height = 480;//待标定图片的大小
//设置二 图像中X方向上的角点个数
const int ChessBoardSize_w = 8;
//设置三 图像中Y方向上的角点个数
const int ChessBoardSize_h = 6;
//图片中可标定的角点区域和个数
const CvSize  ChessBoardSize = cvSize(ChessBoardSize_w, ChessBoardSize_h);
const int NPoints = ChessBoardSize_w*ChessBoardSize_h;//单张图像中所有的角点个数
//设置四 待标定的图片数目
const int NImages = 9;
//设置五 待标定的图片格式
char filename[] = "01.tif";//图片依次必须命名为01.tif, 02.tif,……09.tif,必须带上序号0;
//图片必须放在程序子目录中！
//设置六 设置棋盘格子的边长，单位为mm
float    SquareWidth = 24.6;
///需要设置的参数结束，以下设定标定中使用的函数及变量///

//求取棋盘格上点的世界坐标所需要的函数InitCorners3D()
void InitCorners3D(CvMat *Corners3D, CvSize ChessBoardSize, int Nimages, float SquareSize);
int corner_count[NImages] = { 0 };
//标定的结果保存
CvMat *intrinsics;//内参数
CvMat *distortion_coeff;//畸变参数
CvMat *rotation_vectors;//旋转向量
CvMat *translation_vectors;//平移向量
CvMat *object_points;
CvMat *point_counts;
CvMat *image_points;
CvMat *image_points2;

int main()
{
CvPoint2D32f corners[NPoints*NImages];//所有的角点坐标9*8*6
//当前图像序号
int captured_frames = 0;
for (captured_frames = 0; captured_frames<NImages; captured_frames++)
{
//依次读取每个图像，且进行相同的处理，共9次；
filename[1] = (char)(captured_frames + 49);
IplImage *chessBoard_Img = cvLoadImage(filename, 1);//原图
//灰度图
IplImage *current_frame_gray = cvCreateImage(cvSize(chessBoard_Img->width,chessBoard_Img->height), chessBoard_Img->depth, 1);// chessBoard_Img->nChannels);
if (chessBoard_Img == NULL)
{
cout<<"load image failed!"<<endl;
}
cvCvtColor(chessBoard_Img, current_frame_gray, CV_BGR2GRAY);//原图灰度化
//对原图进行复制
IplImage *current_frame_rgb = cvCreateImage(cvSize(chessBoard_Img->width, chessBoard_Img->height), chessBoard_Img->depth, 3);
cvCopy(chessBoard_Img, current_frame_rgb, 0);//用复制后的新图做角点标定
int find_corners_result;//找到的角点数目
find_corners_result =     
cvFindChessboardCorners(current_frame_gray,
ChessBoardSize,
&corners[captured_frames*NPoints],
&corner_count[captured_frames],
CV_CALIB_CB_ADAPTIVE_THRESH);
//通过迭代来发现具有亚像素精度的角点位置
cvFindCornerSubPix(current_frame_gray, //当前图像
&corners[captured_frames*NPoints],//检测到的角点
NPoints, //角点的个数
cvSize(5, 5), cvSize(-1, -1), //不忽略corner临近的像素进行精确估计，
cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 30, 0.01));
//迭代次数（iteration）或者最小精度（epsilon）
//绘制检测到的棋盘角点
cvDrawChessboardCorners(current_frame_rgb, ChessBoardSize, 
&corners[captured_frames*NPoints],
NPoints,
find_corners_result);
char cali_img[15] = "cali_";
strcat(cali_img, filename);
cvSaveImage(cali_img, current_frame_rgb);
cvNamedWindow("result", CV_WINDOW_AUTOSIZE);
cout << "The NO. " << captured_frames + 1 << " Calibrated Image." << endl;
cvShowImage("result", current_frame_rgb);
cvWaitKey(50);
}
//定义存储相机参数的几个矩阵尺寸
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);
image_points2 = cvCreateMat(NImages*NPoints, 2, CV_32FC1);
//把2维点转化成三维点（object_points输出量），
InitCorners3D(object_points, ChessBoardSize, NImages, SquareWidth);//存储所有9*54个角点的三维世界坐标
//把corners 复制到 image_points中，两者都是存储角点坐标的二维矩阵
cvSetData(image_points, corners, sizeof(CvPoint2D32f));
//把corner_count 复制到 point_counts中，两者都表示每个图像中多少个角点
cvSetData(point_counts, &corner_count, sizeof(int));

//计算内参
cvCalibrateCamera2(object_points,
image_points,
point_counts,//指定不同视图里点的数目
cvSize(image_width, image_height),
intrinsics,
distortion_coeff,
rotation_vectors,
translation_vectors,
0);
//四个内参矩阵
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];
//旋转向量转换为旋转矩阵
double R_matrix[9];
CvMat pr_vec;
CvMat pR_matrix;
//分配矩阵空间
cvInitMatHeader(&pr_vec, 1, 3, CV_64FC1, rotv, CV_AUTOSTEP);
cvInitMatHeader(&pR_matrix, 3, 3, CV_64FC1, R_matrix, CV_AUTOSTEP);
cvRodrigues2(&pr_vec, &pR_matrix, 0);

//输出标定结果
cout << "-----------------------------------------\n ";
cout << "INTRINSIC MATRIX:  \n";
cout << intr[0][0] << "; " << intr[0][1] << "; " << intr[0][2] << endl;
cout << intr[1][0] << "; " << intr[1][1] << "; " << intr[1][2] << endl;
cout << intr[2][0] << "; " << intr[2][1] << "; " << intr[2][2] << endl;
cout << "----------------------------------------- \n";
cout<<"DISTORTION VECTOR:  \n";
for (int i = 0; i < 4; i++){
cout << dist[i] << "; ";
}
cout << endl;
cout << "----------------------------------------- \n";
cout << "ROTATION VECTOR(IMAGE1):  \n";
cout << rotv[0] << "; " << rotv[1] << "; " << rotv[2] << endl;
cout<<"TRANSLATION VECTOR(IMAGE1):  \n";
cout << tranv[0] << "; " << tranv[1] << "; " << tranv[2] << endl;
cout << "----------------------------------------- \n";
cout<<"ROTATION MATRIX(IMAGE1):  \n";
cout << R_matrix[0] << ", " << R_matrix[1] << ", " << R_matrix[2] << endl;
cout << R_matrix[3] << ", " << R_matrix[4] << ", " << R_matrix[5] << endl;
cout << R_matrix[6] << ", " << R_matrix[7] << ", " << R_matrix[8] << endl;
cout << "----------------------------------------- \n";

//评价标定结果
double total_err = 0.0;
double err = 0.0;
double point_cousum = 0;
cout << "Error of every single image:" << endl;
for (int i = 0; i<NImages; i++)
{
//提取单张图像的参数 object_matrix, rotation_matrix, translation_matrix
CvMat *object_matrix = cvCreateMat(NPoints, 3, CV_32FC1);
CvMat *image_matrix = cvCreateMat(NPoints, 2, CV_32FC1);
CvMat *project_image_matrix = cvCreateMat(NPoints, 2, CV_32FC1);
CvMat *rotation_matrix_1 = cvCreateMat(1, 3, CV_32FC1);
CvMat *translation_matrix_1 = cvCreateMat(1, 3, CV_32FC1);
CvMat *rotation_matrix = cvCreateMat(3, 1, CV_32FC1);
CvMat *translation_matrix = cvCreateMat(3, 1, CV_32FC1);
double err = 0;
cvGetRows(object_points, object_matrix, i*NPoints, (i + 1)*NPoints, 1);
cvGetRow(rotation_vectors, rotation_matrix_1, i);
cvReshape(rotation_matrix_1, rotation_matrix, 0, 3);
cvGetRow(translation_vectors, translation_matrix_1, i);
cvReshape(translation_matrix_1, translation_matrix, 0, 3);
cvGetRows(image_points, project_image_matrix, i*NPoints, (i + 1)*NPoints, 1);

//反向投影，投影到的点保存在image_matrix中
cvProjectPoints2(object_matrix, rotation_matrix, translation_matrix,
intrinsics, distortion_coeff, image_matrix, 0, 0, 0, 0, 0);
err = cvNorm(image_matrix, project_image_matrix, CV_L2, 0);
total_err = err*err;
point_cousum = point_cousum + (point_counts->data.ptr + point_counts->step*i)[0];
cout << "Error of NO. " << i + 1 << "image= " << sqrt(err*err / (point_counts->data.ptr + point_counts->step*i)[0])<<endl;
}
cout << "Total error= " << sqrt(total_err / point_cousum)<<endl;

//以下为图像的校正结果显示
IplImage     *srcimage; //彩色图像
IplImage     *grayimage;//灰度图像
IplImage     *result_image;//角点图像
srcimage = cvCreateImage(cvSize(image_width, image_height), IPL_DEPTH_8U, 3);
grayimage = cvCreateImage(cvSize(image_width, image_height), IPL_DEPTH_8U, 1);
result_image = cvCreateImage(cvSize(image_width, image_height), IPL_DEPTH_8U, 1);
cvNamedWindow("Corrected image", 1);
//显示校正后的图片
cout << "Show Corrected image ." << endl;
for (int i = 0; i<NImages; i++)
{//加载图片
cout << "The NO." << i + 1 << " corrected image ." << endl;
char filename[] = "01.tif";
filename[1] = (char)(i + 49);
if ((srcimage = cvLoadImage(filename, 1)) != 0)
{//色彩转换
cvCvtColor(srcimage, grayimage, CV_BGR2GRAY);
cvUndistort2(grayimage, result_image, intrinsics, distortion_coeff);
cvShowImage("Corrected image", result_image);
char corrected[20] = "correct_";
strcat(corrected, filename);
cvSaveImage(corrected, result_image);
cvWaitKey(50);
}
}
cvWaitKey(0);
cvReleaseMat(&intrinsics);
cvReleaseMat(&distortion_coeff);
cvReleaseMat(&rotation_vectors);
cvReleaseMat(&translation_vectors);
cvReleaseMat(&point_counts);
cvReleaseMat(&object_points);
cvReleaseMat(&image_points);
cvDestroyAllWindows();
return 0;
}


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