棋盘格的角点标定与相机参数标定

这个程序是对26幅图像的处理过程，使用了一种新的方法实现对规则命名图像的循环读取，一大收获，程序如下：

// CamCalib.cpp : 定义控制台应用程序的入口点。
//
//#include "stdafx.h"
#include<iostream>
#include <cv.h>
#include <highgui.h>
#include <cxcore.h>
#include<string.h>
using namespace std;
//！！！！！需要设置的参数！！！！！/
//设置一 图像尺寸
int image_width = 640;
int image_height = 480;//待标定图片的大小
//设置二 图像中X方向上的角点个数
const int ChessBoardSize_w = 6;
//设置三 图像中Y方向上的角点个数
const int ChessBoardSize_h = 8;
//图片中可标定的角点区域和个数
const CvSize  ChessBoardSize = cvSize(ChessBoardSize_w, ChessBoardSize_h);
const int NPoints = ChessBoardSize_w*ChessBoardSize_h;//单张图像中所有的角点个数
//设置四 待标定的图片数目
const int NImages = 27;
//设置五 待标定的图片格式
int corner_count[NImages] = { 0 };
char file[10];
//图片依次必须命名为1.tif, 2.tif, ……26.tif;
//设置六 设置棋盘格子的边长，单位为mm
float    SquareWidth = 24.6;
///需要设置的参数结束，以下设定标定中使用的函数及变量///
//求取棋盘格上点的世界坐标所需要的函数InitCorners3D()
void InitCorners3D(CvMat *Corners3D, CvSize ChessBoardSize, int Nimages, float SquareSize);
//标定的结果保存
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()
{
IplImage     *current_frame_rgb; //连接棋盘格角点图像
IplImage     *currentsamall_frame_rgb; //连接棋盘格角点图像（缩小大小后的，便于显示）
IplImage     *current_frame_gray;//灰度图像，用来提取角点
IplImage     *chessBoard_Img;//原始载入图像
CvPoint2D32f corners[NPoints*NImages];//所有的角点坐标
//创建图像
chessBoard_Img = cvCreateImage(cvSize(image_width, image_height), IPL_DEPTH_8U, 3);//原图
current_frame_gray = cvCreateImage(cvSize(image_width, image_height), IPL_DEPTH_8U, 1);//灰度图
current_frame_rgb = cvCreateImage(cvSize(image_width, image_height), IPL_DEPTH_8U, 3);//彩色标定图
currentsamall_frame_rgb = cvCreateImage(cvSize(image_width / 2, image_height / 2), IPL_DEPTH_8U, 3);//彩色缩小标定图
//当前图像序号
int captured_frames = 0;
for (captured_frames = 0; captured_frames < NImages; captured_frames++)
{//依次读取每个图像，且进行相同的处理，共27次；
sprintf(file, "%d.tif", captured_frames);
chessBoard_Img = cvLoadImage(file, 1);
if (chessBoard_Img == NULL)
{
cout<<"load image failed!";
}
cvCvtColor(chessBoard_Img, current_frame_gray, CV_BGR2GRAY);//原图灰度化
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);
cvSaveImage("Agular dot image.bmp", current_frame_rgb);
cvResize(current_frame_rgb, currentsamall_frame_rgb);
cvNamedWindow("result", CV_WINDOW_AUTOSIZE);
cout << "The NO. " << captured_frames <<"Calibrated Image :"<< endl;
cvShowImage("result", currentsamall_frame_rgb);
cvWaitKey(100);
}
   //定义存储相机参数的几个矩阵尺寸
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";
printf("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;
printf("TRANSLATION VECTOR(IMAGE1):  \n");
cout << tranv[0] << "; " << tranv[1] << "; "<<tranv[2]<<endl;
cout<<"----------------------------------------- \n";
printf("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]);
}
cout<<endl<<"Total error= "<<sqrt(total_err / point_cousum);

//以下为图像的校正
IplImage     *srcimage; //彩色图像
IplImage     *grayimage;//灰度图像
IplImage     *result_image;//角点图像
IplImage     *samllresult_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);
samllresult_image = cvCreateImage(cvSize(image_width / 2, image_height / 2), IPL_DEPTH_8U, 1);
cvNamedWindow("Corrected image", 1);
//显示校正后的图片
cout << "Show Corrected image :" << endl;
for (int i = 0; i<NImages; i++)
{//加载图片
cout << "The NO." << i  << " corrected image :" << endl;
sprintf(file, "%d.tif", i);
if ((srcimage = cvLoadImage(file, 1)) != 0)
{//色彩转换
cvCvtColor(srcimage, grayimage, CV_BGR2GRAY);
cvUndistort2(grayimage, result_image, intrinsics, distortion_coeff);
cvResize(result_image, samllresult_image);
cvShowImage("Corrected image", samllresult_image);
cvSaveImage("Corrected img.tif", result_image);
cvWaitKey(200);
}
}

system("pause");
cvReleaseMat(&intrinsics);
cvReleaseMat(&distortion_coeff);
cvReleaseMat(&rotation_vectors);
cvReleaseMat(&translation_vectors);
cvReleaseMat(&point_counts);
cvReleaseMat(&object_points);
cvReleaseMat(&image_points);
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 (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);
}