图像标定 求相机内参外参
下文为使用最小二乘的方法对一幅图像进行标定，求得相机的内参、外参的参数。
Calibration
Process of camera calibration:
1. Load calibration piture andfind the corner by using “cvFindChessboardCorners” Opencv function. Then drawit in the raw piture.
2. Store the corner information,Create the matrix U (160*12) in the formula below.
3. Use to estimate the parameters of M.
Use the formula below to solve IntrinsicParameters and Extrinsic Parameters.
4. The Codelist below:
#include
#include
#include
#include
#include
int n_boards = 1;
const int board_dt = 10;
int board_w = 10;
int board_h = 8;
int x_coordinate[10] = {4,3,2,1,0,0,0,0,0,0};
int y_coordinate[10] = {0,0,0,0,0,1,2,3,4,5};
int main(int argc, char* argv[]) {
int board_n = board_w * board_h;
CvSize board_sz = cvSize( board_w, board_h );
cvNamedWindow( "Calibration" );
//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_32SC1);
CvMat* intrinsic_matrix = cvCreateMat(3,3,CV_32FC1);
CvMat* distortion_coeffs = cvCreateMat(4,1,CV_32FC1);
IplImage* image = 0;
IplImage* gray_image = 0; //for subpixel
CvPoint2D32f* corners = new CvPoint2D32f[ board_n ];
int corner_count;
image = cvLoadImage( "picture1.jpg",1);
//求角点
if(gray_image == 0 && image) //We'll need this for subpixel accurate stuff
gray_image = cvCreateImage(cvGetSize(image),8,1);
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 ) {
for(int i = 0; i < board_h; ++i)
for( int j = 0; j < board_w; ++j ) {
int k = i*board_w+j;
CV_MAT_ELEM(*image_points, float,k,0) = corners[k].x;
CV_MAT_ELEM(*image_points, float,k,1) = corners[k].y;
CV_MAT_ELEM(*object_points,float,k,0) = x_coordinate[j];
CV_MAT_ELEM(*object_points,float,k,1) = y_coordinate[j];
CV_MAT_ELEM(*object_points,float,k,2) = (8-i);
}
}
//构造P矩阵
CvMat* U_matrix = cvCreateMat(2*board_n,12,CV_32FC1);
float Even_line[12];
float Odd_line[12];
memset(Even_line,0.0f,sizeof(float)*12);
memset(Odd_line,0.0f,sizeof(float)*12);
Even_line[3] = Odd_line[7] = 1;
for(int i = 0,j = 0; i < 2*board_n; i+=2,j++){
Even_line[0] = Odd_line[4] = CV_MAT_ELEM(*object_points,float,j,0);
Even_line[1] = Odd_line[5] = CV_MAT_ELEM(*object_points,float,j,1);
Even_line[2] = Odd_line[6] = CV_MAT_ELEM(*object_points,float,j,2);
Even_line[11] = -CV_MAT_ELEM(*image_points,float,j,0);
Odd_line[11] = -CV_MAT_ELEM(*image_points,float,j,1);
Even_line[8] = Even_line[11]*Even_line[0];
Even_line[9] = Even_line[11]*Even_line[1];
Even_line[10] = Even_line[11]*Even_line[2];
Odd_line[8] = Odd_line[11]*Odd_line[4];
Odd_line[9] = Odd_line[11]*Odd_line[5];
Odd_line[10] = Odd_line[11]*Odd_line[6];
for(int j = 0;j < 12;j++) CV_MAT_ELEM(*U_matrix,float,i,j) = Even_line[j];
for(int j = 0;j < 12;j++) CV_MAT_ELEM(*U_matrix,float,i+1,j) = Odd_line[j];
}
//计算内参、外参
CvMat* UtU = cvCreateMat(12,12,CV_32FC1);
cvGEMM(U_matrix,U_matrix,1,NULL,1,UtU,CV_GEMM_A_T);
CvMat* UtU_evects = cvCreateMat(12,12,CV_32FC1);
CvMat* UtU_evals = cvCreateMat(1, 12,CV_32FC1);
cvEigenVV(UtU,UtU_evects,UtU_evals,0);
double UtU_evals_min_val;
CvPoint min_loc;
cvMinMaxLoc(UtU_evals,&UtU_evals_min_val,NULL,&min_loc,NULL,NULL);
CvMat* M = cvCreateMat(12,1,CV_32FC1);
for(int i = 0;i < 12;i++) CV_MAT_ELEM(*M,float,i,0) = CV_MAT_ELEM(*UtU_evects,float,min_loc.x,i);
CvMat* A1 = cvCreateMat(3,1,CV_32FC1);
CvMat* A2 = cvCreateMat(3,1,CV_32FC1);
CvMat* A3 = cvCreateMat(3,1,CV_32FC1);
CvMat* b = cvCreateMat(3,1,CV_32FC1);
for(int i = 0;i < 3;i++) CV_MAT_ELEM(*A1,float,i,0) = CV_MAT_ELEM(*M,float,i,0);
for(int i = 4;i < 7;i++) CV_MAT_ELEM(*A2,float,i-4,0) = CV_MAT_ELEM(*M,float,i,0);
for(int i = 8;i < 11;i++) CV_MAT_ELEM(*A3,float,i-8,0) = CV_MAT_ELEM(*M,float,i,0);
for(int i = 3,j = 0;i < 12;i+=4,j++) CV_MAT_ELEM(*b,float,j,0) = CV_MAT_ELEM(*M,float,i,0);
float a3 =cvNorm(A3,NULL,CV_L2,NULL);
float Rho = -1/a3;
CvMat* r1 = cvCreateMat(3,1,CV_32FC1);
CvMat* r2 = cvCreateMat(3,1,CV_32FC1);
CvMat* r3 = cvCreateMat(3,1,CV_32FC1);
for(int i = 0;i < 3;i++)CV_MAT_ELEM(*r3,float,i,0) = Rho*CV_MAT_ELEM(*A3,float,i,0);
float u0 = pow(Rho,2)*(cvDotProduct(A1,A3));
float v0 = pow(Rho,2)*(cvDotProduct(A2,A3));
CvMat* Cross_a1_a3 = cvCreateMat(3,1,CV_32FC1);
CvMat* Cross_a2_a3 = cvCreateMat(3,1,CV_32FC1);
cvCrossProduct(A1,A3,Cross_a1_a3);
cvCrossProduct(A2,A3,Cross_a2_a3);
float cos_theta = -((cvDotProduct(Cross_a1_a3,Cross_a2_a3))/(cvNorm(Cross_a1_a3,NULL,CV_L2,NULL)*cvNorm(Cross_a2_a3,NULL,CV_L2,NULL)));
float theta = acos(cos_theta);
float alpha = pow(Rho,2)*cvNorm(Cross_a1_a3,NULL,CV_L2,NULL)*sin(theta);
float beta = pow(Rho,2)*cvNorm(Cross_a2_a3,NULL,CV_L2,NULL)*sin(theta);
for(int i = 0;i < 3;i++)CV_MAT_ELEM(*r1,float,i,0) = (1/(cvNorm(Cross_a2_a3,NULL,CV_L2,NULL)))*CV_MAT_ELEM(*Cross_a2_a3,float,i,0);
cvCrossProduct(r3,r1,r2);
float K_element[9] = {alpha,-alpha*cos_theta,u0,0,beta/sin(theta),v0,0,0,1};
CvMat* K = cvCreateMat(3,3,CV_32FC1);
for(int i = 0;i < 3;i++)
for(int j = 0;j < 3;j++) CV_MAT_ELEM(*K,float,i,j) = K_element[i*3+j];
CvMat* t = cvCreateMat(3,1,CV_32FC1);
CvMat* K_invert = cvCreateMat(3,3,CV_32FC1);
cvInvert(K,K_invert,CV_LU);
cvGEMM(K_invert,b,1,NULL,NULL,t,0);
for(int i = 0;i < 3;i++) CV_MAT_ELEM(*t,float,i,0) = Rho*CV_MAT_ELEM(*t,float,i,0);
//显示内参、外参
printf("Intrinsic Parameters:\n");
printf("alpha =%15.8f\n",alpha);
printf("beta =%15.8f\n",beta);
printf("theta =%15.8f\n",theta*57.29577951);
printf("u0 =%15.8f\n",u0);
printf("v0 =%15.8f\n",v0);
printf("Extrinsic Parameters:\n");
printf("r1 = [%15.8f %15.8f %15.8f]'\n",CV_MAT_ELEM(*r1,float,0,0),CV_MAT_ELEM(*r1,float,1,0),CV_MAT_ELEM(*r1,float,2,0));
printf("r2 = [%15.8f %15.8f %15.8f]'\n",CV_MAT_ELEM(*r2,float,0,0),CV_MAT_ELEM(*r2,float,1,0),CV_MAT_ELEM(*r2,float,2,0));
printf("r3 = [%15.8f %15.8f %15.8f]'\n",CV_MAT_ELEM(*r3,float,0,0),CV_MAT_ELEM(*r3,float,1,0),CV_MAT_ELEM(*r3,float,2,0));
printf("t = [%15.8f %15.8f %15.8f]'\n",CV_MAT_ELEM(*t,float,0,0),CV_MAT_ELEM(*t,float,1,0),CV_MAT_ELEM(*t,float,2,0));
cvWaitKey(0);
return 0;
}
5 The result list below:
