opencv中摄像机的模型与标定

/*一次完成标定：我们通过程序来完成如下功能，
1）首先寻找用户指定维数的棋盘，
2）捕捉用户需要的许多完整图像(即能找到棋盘的所有角点)，计算摄像机内参数和畸变参数。
3）进入显示模式，以显示矫正后的摄像机图像
*/

#define _CRT_SECURE_NO_WARNINGS
#include <string>
#include <iostream>
#include <cv.h>
#include <highgui.h>

using namespace std;

int main()
{
int cube_length = 7;

CvCapture* capture;

capture = cvCreateCameraCapture(0);

if (capture == 0)
{
printf("无法捕获摄像头设备！\n\n");
return 0;
}
else
{
printf("捕获摄像头设备成功！！\n\n");
}

IplImage* frame = NULL;
cvNamedWindow("摄像机帧截取窗口", 1);
printf("按“C”键截取当前帧并保存为标定图片...\n按“Q”键退出截取帧过程...\n\n");

int number_image = 1;
char *str1;
str1 = ".jpg";
char filename[20] = "";

while (true)
{
frame = cvQueryFrame(capture);
if (!frame)
break;
cvShowImage("摄像机帧截取窗口", frame);

if (cvWaitKey(10) == 'c')
{
sprintf_s(filename, "%d.jpg", number_image);
cvSaveImage(filename, frame);
cout << "成功获取当前帧，并以文件名" << filename << "保存...\n\n";
printf("按“C”键截取当前帧并保存为标定图片...\n按“Q”键退出截取帧过程...\n\n");
number_image++;
}
else if (cvWaitKey(10) == 'q')
{
printf("截取图像帧过程完成...\n\n");
cout << "共成功截取" << --number_image << "帧图像！！\n\n";
break;
}
}

cvReleaseImage(&frame);
cvDestroyWindow("摄像机帧截取窗口");

IplImage * show;
cvNamedWindow("RePlay", 1);

int number_image_copy = number_image;

//cvFindChessboardCorners()各个参数
CvSize board_size = cvSize(7, 7);
int board_width = board_size.width;
int board_height = board_size.height;
int total_per_image = board_width*board_height;
CvPoint2D32f * image_points_buf = new CvPoint2D32f[total_per_image];

CvMat * image_points = cvCreateMat(number_image*total_per_image, 2, CV_32FC1);
CvMat * object_points = cvCreateMat(number_image*total_per_image, 3, CV_32FC1);
CvMat * point_counts = cvCreateMat(number_image, 1, CV_32SC1);
CvMat * intrinsic_matrix = cvCreateMat(3, 3, CV_32FC1);
CvMat * distortion_coeffs = cvCreateMat(5, 1, CV_32FC1);

int count;//记录角点数目
int found;//存储cvFindChessboardCorners()函数的返回值
int step;
int successes = 0;

int a = 1;
while (a <= number_image_copy)
{
sprintf_s(filename, "%d.jpg", a);
show = cvLoadImage(filename, -1);

/*1）cvFindChessboardCorners()定位棋盘的角点，该函数返回的值仅仅是近似值
参数1：包含棋盘的单幅图像(8位灰度单通道图像)。
参数2：表示棋盘的每行和没列有多少个角点，该数值是内角点个数，因此对一个标准象棋棋盘，正确值应该是cvSize(7,7)
参数3：存储角点位置的数组指针。该数组必须事先分配空间，至少必须大于棋盘所有的角点数，角点位置的每个数字都是以像素坐标保存的。
参数4：此变量可选，如果不为NULL，则它是一个指向所记录角点数目的整型指针。
参数5：用来定义额外的滤波长以有助于寻找棋盘角点。所有变量都可以单独或者以逻辑或的方式组合使用
***********如果函数成功找到所有的角点，那么返回非0，反之，返回0.******************
*/
found = cvFindChessboardCorners(show, board_size, image_points_buf, &count,
CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS);
if (found == 0)
{
cout << "第" << a << "帧图片无法找到棋盘格所有角点!\n\n";
cvNamedWindow("RePlay", 1);
cvShowImage("RePlay", show);
cvWaitKey(0);
}
else
{
cout << "第" << a << "帧图像成功获得" << count << "个角点...\n";

cvNamedWindow("RePlay", 1);

IplImage * gray_image = cvCreateImage(cvGetSize(show), 8, 1);
cvCvtColor(show, gray_image, CV_BGR2GRAY);
cout << "获取源图像灰度图过程完成...\n";

//2）由于cvFindChessboardCorners()返回的仅仅是近似值，必须使用另外的函数cvFindCornerSubPix()来计算角点精确位置
cvFindCornerSubPix(gray_image, image_points_buf, count, cvSize(11, 11), cvSize(-1, -1),
cvTermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
cout << "灰度图亚像素化过程完成...\n";

/*3）绘制棋盘角点
参数一：是欲绘制的图像。因为角点是以颜色圆圈表示的、该图像必须是8位的彩色图像。在大多数场合下，它是
cvFindChessboardCorners()所使用图像的复制品（但必须预先转换为3通道图像）
参数二、三：跟cvFindChessboardCorners()函数一样。
参数四：count是一个等于角点数目的整数。
参数五：表示是否所有的棋盘模式都被成功的找到。这可以设置为cvFindChessboardCorners()函数的返回值。
*/
cvDrawChessboardCorners(show, board_size, image_points_buf, count, found);
cout << "在源图像上绘制角点过程完成...\n\n";
cvShowImage("RePlay", show);

cvWaitKey(0);
}

if (total_per_image == count)
{
step = successes*total_per_image;
for (int i = step, j = 0; j<total_per_image; ++i, ++j)
{
CV_MAT_ELEM(*image_points, float, i, 0) = image_points_buf[j].x;
CV_MAT_ELEM(*image_points, float, i, 1) = image_points_buf[j].y;
CV_MAT_ELEM(*object_points, float, i, 0) = (float)(j / cube_length);
CV_MAT_ELEM(*object_points, float, i, 1) = (float)(j%cube_length);
CV_MAT_ELEM(*object_points, float, i, 2) = 0.0f;
}
CV_MAT_ELEM(*point_counts, int, successes, 0) = total_per_image;
successes++;
}
a++;
}

cvReleaseImage(&show);
cvDestroyWindow("RePlay");

cout << "*********************************************\n";
cout << number_image << "帧图片中，标定成功的图片为" << successes << "帧...\n";
cout << number_image << "帧图片中，标定失败的图片为" << number_image - successes << "帧...\n\n";
cout << "*********************************************\n\n";

cout << "按任意键开始计算摄像机内参数...\n\n";

CvCapture* capture1;
capture1 = cvCreateCameraCapture(0);
IplImage * show_colie;
show_colie = cvQueryFrame(capture1);

CvMat * object_points2 = cvCreateMat(successes*total_per_image, 3, CV_32FC1);
CvMat * image_points2 = cvCreateMat(successes*total_per_image, 2, CV_32FC1);
CvMat * point_counts2 = cvCreateMat(successes, 1, CV_32SC1);

for (int i = 0; i<successes*total_per_image; ++i)
{
CV_MAT_ELEM(*image_points2, float, i, 0) = CV_MAT_ELEM(*image_points, float, i, 0);
CV_MAT_ELEM(*image_points2, float, i, 1) = CV_MAT_ELEM(*image_points, float, i, 1);
CV_MAT_ELEM(*object_points2, float, i, 0) = CV_MAT_ELEM(*object_points, float, i, 0);
CV_MAT_ELEM(*object_points2, float, i, 1) = CV_MAT_ELEM(*object_points, float, i, 1);
CV_MAT_ELEM(*object_points2, float, i, 2) = CV_MAT_ELEM(*object_points, float, i, 2);
}

for (int i = 0; i<successes; ++i)
{
CV_MAT_ELEM(*point_counts2, int, i, 0) = CV_MAT_ELEM(*point_counts, int, i, 0);
}

cvReleaseMat(&object_points);
cvReleaseMat(&image_points);
cvReleaseMat(&point_counts);

CV_MAT_ELEM(*intrinsic_matrix, float, 0, 0) = 1.0f;
CV_MAT_ELEM(*intrinsic_matrix, float, 1, 1) = 1.0f;

/*4）标定函数cvCalibrateCamera2()：该函数将做大量的分解工作以提供我们所需要的信息。具体来说，我们会得到摄像机内参数矩阵。畸变系数、旋转向量、和平移向量。
前两个构成了摄像机的内参数，后两个构成了物体（如棋盘）位置和方向的摄像机外参数。畸变系数(k1,K2，P1，P2，和k3)来自于径向和切向畸变方程。它们有助于矫正这些畸变。
摄像内参数矩阵也许是最有用的结果，因为它可以让我们将3D坐标转换为2D图像坐标。

参数1：object_points是一个N*3的矩阵，包含物体的每K个点在每M个图像上的物理坐标，(即N=K*M)。这些点位于物体的坐标平面上：(在不同的图像中使用同一个物体很正常，所以
N个点实际上是同一个物体的K个点的M次重复列表)。
参数2：image_points,它是一个N*2矩阵，包含参数一所提供的所有点的像素坐标。如果使用棋盘进行标定，那么这个变量简单地由M次调用cvFindChessboardCorners()的返回值构成
参数3：point_counts表示每个图像的点的个数，以M*1矩阵形式提供。
参数4：Image_size是以像素衡量的图像尺寸，图像点就是从该图像中提取

参数4和5：intrinsic_matrix和distortion_coeffs构成了摄像机的内参数。内参数矩阵完全定义了理想摄像机模型的摄像机行为，而畸变系数则更多表征摄像机的非理想行为。摄像机矩阵
总是3*3的，而畸变系数总是5个所以distortion_coeffs变量是一个指向5*1矩阵的指针(记录顺序为k1,k2,p1,p2,k3).

接下来两个变量将概括外参数值信息，也就是，它们说明与在每幅图像中相对于摄像机来说的标定物体(即棋盘)的位置。物体位置由旋转和平移表征。
旋转rotation_vectors由M个三元素向量行列为M*3的矩阵(M为图像的个数)，每个向量的长度表示逆时针旋转的角度。每个旋转向量可以通过调用cvRodirigues2()转换为3*3的旋转矩阵

*/
cvCalibrateCamera2(object_points2, image_points2, point_counts2, cvGetSize(show_colie),
intrinsic_matrix, distortion_coeffs, NULL, NULL, 0);

cout << "摄像机内参数矩阵为：\n";
cout << CV_MAT_ELEM(*intrinsic_matrix, float, 0, 0) << "    " << CV_MAT_ELEM(*intrinsic_matrix, float, 0, 1)
<< "    " << CV_MAT_ELEM(*intrinsic_matrix, float, 0, 2)
<< "\n\n";
cout << CV_MAT_ELEM(*intrinsic_matrix, float, 1, 0) << "    " << CV_MAT_ELEM(*intrinsic_matrix, float, 1, 1)
<< "    " << CV_MAT_ELEM(*intrinsic_matrix, float, 1, 2)
<< "\n\n";
cout << CV_MAT_ELEM(*intrinsic_matrix, float, 2, 0) << "    " << CV_MAT_ELEM(*intrinsic_matrix, float, 2, 1)
<< "          " << CV_MAT_ELEM(*intrinsic_matrix, float, 2, 2)
<< "\n\n";

cout << "畸变系数矩阵为：\n";
cout << CV_MAT_ELEM(*distortion_coeffs, float, 0, 0) << "    " << CV_MAT_ELEM(*distortion_coeffs, float, 1, 0)
<< "    " << CV_MAT_ELEM(*distortion_coeffs, float, 2, 0)
<< "    " << CV_MAT_ELEM(*distortion_coeffs, float, 3, 0)
<< "    " << CV_MAT_ELEM(*distortion_coeffs, float, 4, 0)
<< "\n\n";

cvSave("Intrinsics.xml", intrinsic_matrix);
cvSave("Distortion.xml", distortion_coeffs);

cout << "摄像机矩阵、畸变系数向量已经分别存储在名为Intrinsics.xml、Distortion.xml文档中\n\n";

CvMat * intrinsic = (CvMat *)cvLoad("Intrinsics.xml");
CvMat * distortion = (CvMat *)cvLoad("Distortion.xml");

IplImage * mapx = cvCreateImage(cvGetSize(show_colie), IPL_DEPTH_32F, 1);
IplImage * mapy = cvCreateImage(cvGetSize(show_colie), IPL_DEPTH_32F, 1);

/*5）矫正---
cvInitUndistortMap()函数计算畸变映射，该映射将图中的每个点与其映射位置关联。cvRemap()表示在任意图像上应用该映射

前两个变量是摄像机内参数矩阵和畸变系数，全部是来自函数cvCalibrateCamera2()的期望形式，生成的畸变映射由两个单独的32位
单通道矩阵所表示。第一个给出点被映射的x值，第二个给出的y值。
*/
cvInitUndistortMap(intrinsic, distortion, mapx, mapy);

cvNamedWindow("原始图像", 1);
cvNamedWindow("非畸变图像", 1);

cout << "按‘E’键退出显示...\n\n";

while (show_colie)
{
IplImage * clone = cvCloneImage(show_colie);
cvShowImage("原始图像", show_colie);

cvRemap(clone, show_colie, mapx, mapy);//

cvReleaseImage(&clone);
cvShowImage("非畸变图像", show_colie);

if (cvWaitKey(10) == 'e')
{
break;
}

show_colie = cvQueryFrame(capture1);
}

return 0;
}