图像标定 求相机内参外参

下文为使用最小二乘的方法对一幅图像进行标定，求得相机的内参、外参的参数。

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 <cv.h>#include <highgui.h>#include <stdio.h>#include <stdlib.h>#include <math.h>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 STORAGECvMat* 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 subpixelCvPoint2D32f* 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 stuffgray_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 cornerscvCvtColor(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 itcvDrawChessboardCorners(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: