摄像机标定

/*程序将完成如下功能：它先寻找用户指定维数的棋盘，然后捕捉到用户需要的许多完整图像(即能找到棋盘的所有角点)，
计算摄像机内参数和畸变参数，最后进入显示模式，以显示矫正后的摄像机图像
读入棋盘的宽度和高度，读入收集到的不同场景图像，然后标定摄像机*/
#include<opencv2/highgui/highgui.hpp>
#include<opencv2/opencv.hpp>
#include<stdio.h>
#include<stdlib.h>


int n_boards = 0;//Will be set by input list
const int board_dt = 20;//Wait 20 frames per chessboard view
int board_w;
int board_h;


int main(int argc, char* argv[])
{
if (argc != 4) {
printf("ERROR: Wrong number of input parameters\n");
return -1;
}


//我们简单的定义棋盘的每个方块为一个单位，这样棋盘角点的横竖坐标都是整数。则棋盘的宽度就是方块的个数
board_w = atoi(argv[1]);
board_h = atoi(argv[2]);
n_boards = atoi(argv[3]);//棋盘数目
int board_n = board_w*board_h;//每个棋盘上角点的数目
CvSize board_sz = CvSize(board_w, board_h);
CvCapture* capture = cvCreateCameraCapture(0);
assert(capture);


cvNamedWindow("Calibration");
/*1.第一个参数object_points是一个N*3矩阵，包含物体的每k个点在每M个图像上的物理坐标（即N=K*M）,这些点位于物体的坐标平面上。
2.image_points，它是一个N*2的矩阵，包含object_points所提供的所有点的像素坐标。
3.point_counts表示每个图像的点的个数，以M*1矩阵形式提供
4.变量intrisic_matrix和distortion_coeffs构成了摄像机的内参数。摄像机矩阵总是3*3的，而畸变系数总是五个。所以distortion_coeffs变量是一个指向5*1矩阵的指针(记录顺序是k1，k2，p1，p2，k3)
*/
//AllOCATE STORAGE
CvMat* image_points = cvCreateMat(n_boards*board_n, 2, CV_32FC1);
CvMat* object_points = cvCreateMat(n_boards*board_n, 3, CV_32FC1);


CvMat* point_counts = cvCreateMat(n_boards, 1, CV_32FC1);
CvMat* intrinsic_matrix = cvCreateMat(3, 3, CV_32FC1);
CvMat* distortion_coeffs = cvCreateMat(5, 1, CV_32FC1);


CvPoint2D32f* corners = new CvPoint2D32f[board_n];
int corner_count;
int successes = 0;//用来记录第几个棋盘
int step, frame = 0;
IplImage* image = cvQueryFrame(capture);
IplImage* gray_image = cvCreateImage(cvGetSize(image), 8, 1);


/*subpixel  CAPTURE CORNER VIEWS LOOP UNTIL WE HAVE GOT n_boards
SUCCESSFUL CAPTURES(ALL CORNERS ON THE BOARD ARE )*/
while (successes < n_boards)
{
//Skip every board_dt frames to allow user to move chessboard
if (frame++%board_dt == 0)
{
//Find chessboard corners:
int found = cvFindChessboardCorners(image, board_sz, corners,
&corner_count, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS);


//Get Subpixel accuracy on those corners
cvCvtColor(image, gray_image, CV_BGR2GRAY);//将彩色图片转换为灰度图像
cvFindCornerSubPix(gray_image, corners, corner_count,
cvSize(11, 11), cvSize(-1, -1), cvTermCriteria(
CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));


//Draw it 
cvDrawChessboardCorners(image, board_sz, corners,
corner_count, found);
cvShowImage("Calibration", image);


//if we got a good board,add it to our data
if (corner_count == board_n)//通过函数得到的corner_count与棋盘中的角点个数对比
{
step = successes*board_n;//step指向每个棋盘的第一个元素
for (int i = step, j = 0; j < board_n; ++i, ++j)//这里i代表了棋盘的编号：即第几个棋盘
{
CV_MAT_ELEM(*image_points, float, i, 0) = corners[j].x;
CV_MAT_ELEM(*image_points, float, i, 1) = corners[j].y;
CV_MAT_ELEM(*object_points, float, i, 0) = j / board_w;
CV_MAT_ELEM(*object_points, float, i, 1) = j%board_w;
CV_MAT_ELEM(*object_points, float, i, 2) = 0.0f;
}
CV_MAT_ELEM(*point_counts, int, successes, 0) = board_n;
successes++;
}
}//end skip board_dt between chessboard capture


//Hit 'p' to pause(暂停)/unpause,ESC to quit
int c = cvWaitKey(15);
if (c == 'p')
{
c = 0;
while (c != 'p'&&c != 27)
{
c = cvWaitKey(250);
}
}
if (c == 27)
return 0;
image = cvQueryFrame(capture);//Get next image
}//END COLLECTION WHILE LOOP


//ALLOCATE MATRICES ACCORDING TO HOW MANY CHESSBOARDS FOUND
CvMat* object_points2 = cvCreateMat(successes*board_n, 3, CV_32FC1);
CvMat* image_points2 = cvCreateMat(successes*board_n, 2, CV_32FC1);
CvMat* point_counts2 = cvCreateMat(successes, 1, CV_32SC1);
/*TRANSFER THE POINTS INTO THE CORRECT SIZE MATRICES
Below ,we write out the details in the next two loops.we could
instead have written: image_points->rows=object_points->rows=successes*board_n;
point_counts->rows=success*/


for (int i = 0; i < successes*board_n; ++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_points2, float, i, 0);
CV_MAT_ELEM(*object_points2, float, i, 1) = CV_MAT_ELEM(*object_points2, float, i, 1);
CV_MAT_ELEM(*object_points2, float, i, 2) = CV_MAT_ELEM(*object_points2, 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);
/*At this point we have all of the chessboard corners we need. Initialize the intrinsic
matrix such that the two focal lengths have a ratio of 1.0*/


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


//标定摄像机
cvCalibrateCamera2(object_points2, image_points2,
point_counts2, cvGetSize(image), intrinsic_matrix, distortion_coeffs, NULL, NULL, 0);


//SAVE THE INTRINSICS AND DISTORTIONS
cvSave("Intrinsics.xml", intrinsic_matrix);
cvSave("Distortion.xml", distortion_coeffs);
//EXAMPLE OF LOADING THESS MATRICES BACK IN
CvMat *intrinsic = (CvMat*)cvLoad("Intrinsics.xml");
CvMat *distortion = (CvMat*)cvLoad("Distortion.xml");


//Build the undistort map that we will use for all subsequent frames.
IplImage* mapx = cvCreateImage(cvGetSize(image), IPL_DEPTH_32F, 1);
IplImage* mapy = cvCreateImage(cvGetSize(image), IPL_DEPTH_32F, 1);
cvInitUndistortMap(intrinsic, distortion, mapx, mapy);


//Just run the camera to the screen,now showing the raw and the undistorted image
cvNamedWindow("Undistort");
while (image)
{
IplImage *t = cvCloneImage(image);
cvShowImage("Calibration", image);//Show raw image
cvRemap(t, image, mapx, mapy);//Undistort image
cvReleaseImage(&t);
cvShowImage("Undistort", image);//Show corrected image




int c = cvWaitKey(15);
if (c == 'p')
{
c = 0;
while (c != 'p'&&c != 27)
{
c = cvWaitKey(250);
}
}
if (c == 27)
break;
image = cvQueryFrame(capture);
}
return 0;
}