摄像头标定程序用于标定多幅棋盘图像.docVIP

/#include stdafx.h#include #include #include / OpenCV#include #include #include #include void InitCorners3D(CvMat *Corners3D, CvSize ChessBoardSize, int Nimages, float SquareSize);void makeChessBoard();int myFindChessboardCorners( const void* image, CvSize pattern_size, CvPoint2D32f* corners, int* corner_count=NULL, int flags=CV_CALIB_CB_ADAPTIVE_THRESH ); inline int drawCorssMark(IplImage *dst,CvPoint pt)/* Function: main_loop Description: 绘制一个十字标记 Calls: Called By: Input: RGB image, pt Output: Return: Others: 需要检查坐标是否越界 to do list*/ const int cross_len = 4;CvPoint pt1,pt2,pt3,pt4;pt1.x = pt.x;pt1.y = pt.y - cross_len;pt2.x = pt.x;pt2.y = pt.y + cross_len;pt3.x = pt.x - cross_len;pt3.y = pt.y;pt4.x = pt.x + cross_len;pt4.y = pt.y; cvLine(dst,pt1,pt2,CV_RGB(0,255,0),2,CV_AA, 0 );cvLine(dst,pt3,pt4,CV_RGB(0,255,0),2,CV_AA, 0 ); return 0; /* declarations for OpenCV */IplImage *current_frame_rgb,grid;IplImage *current_frame_gray;IplImage *chessBoard_Img; int Thresholdness = 120; int image_width = 640;/320;int image_height = 480;/240; bool verbose = false; const int ChessBoardSize_w = 7;const int ChessBoardSize_h = 7;/ Calibration stuffboolcalibration_done = false;const CvSize ChessBoardSize = cvSize(ChessBoardSize_w,ChessBoardSize_h);/float SquareWidth = 21.6f; /实际距离 毫米单位 在A4纸上为两厘米float SquareWidth = 17; /投影实际距离 毫米单位 200 const int NPoints = ChessBoardSize_w*ChessBoardSize_h;const int NImages = 10; /Number of images to collect CvPoint2D32f cornersNPoints*NImages;int corner_countNImages = 0;int captured_frames = 0; CvMat *intrinsics;CvMat *distortion_coeff;CvMat *rotation_vectors;CvMat *translation_vectors;CvMat *object_points;CvMat *point_counts;CvMat *image_points;int find_corners_result =0 ; void on_mouse( int event, int x, int y, int flags, void* param ) if( event = CV_EVENT_LBUTTONDOWN ) /calibration_done = true; int main(int argc, char *argv) CvFont font; int i; char filename100; cvInitFont( &font, CV_FONT_VECTOR0,5, 5, 0, 7, 8); 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); / Function to fill in the real-world points of the checkerboard InitCorners3D(object_points, ChessBoardSize, NImages, SquareWidth); CvCapture* capture = 0; if( argc = 1 | (argc = 2 & strlen(argv1) = 1 & isdigit(argv10) capture = cvCaptureFromCAM( argc = 2 ? argv10 - 0 : 0 ); else if( argc = 2 ) capture = cvCaptureFromAVI( argv1 ); if( !capture ) fprintf(stderr,Could not initialize capturing.n); return -1; / Initialize all of the IplImage structures current_frame_rgb = cvCreateImage(cvSize(image_width, image_height), IPL_DEPTH_8U, 3); IplImage *current_frame_rgb2 = cvCreateImage(cvSize(image_width, image_height), IPL_DEPTH_8U, 3); current_frame_gray = cvCreateImage(cvSize(image_width, image_height), IPL_DEPTH_8U, 1); chessBoard_Img = cvCreateImage(cvSize(image_width, image_height), IPL_DEPTH_8U, 3); current_frame_rgb2-origin = chessBoard_Img-origin = current_frame_gray-origin = current_frame_rgb-origin = 1; makeChessBoard(); cvNamedWindow( result, 0); cvNamedWindow( Window 0, 0); cvNamedWindow( grid, 0); cvMoveWindow( grid, 100,100); cvSetMouseCallback( Window 0, on_mouse, 0 ); cvCreateTrackbar(Thresholdness,Window 0,&Thresholdness, 255,0); while (!calibration_done) i=0;while (captured_frames data.fl0 = 256.8093262; /fxintrinsics-data.fl2 = 160.2826538; /cxintrinsics-data.fl4 = 254.7511139; /fyintrinsics-data.fl5 = 127.6264572; /cy intrinsics-data.fl1 = 0; intrinsics-data.fl3 = 0; intrinsics-data.fl6 = 0; intrinsics-data.fl7 = 0; intrinsics-data.fl8 = 1; distortion_coeff-data.fl0 = -0.193740; /k1distortion_coeff-data.fl1 = -0.378588; /k2distortion_coeff-data.fl2 = 0.028980; /p1distortion_coeff-data.fl3 = 0.008136; /p2 cvWaitKey(40);find_corners_result = 0; /if (find_corners_result !=0) printf(n); cvSetData( image_points, corners, sizeof(CvPoint2D32f);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); / fx 0 cx; 0 fy cy; 0 0 1.cvUndistort2(current_frame_rgb,current_frame_rgb,intrinsics,distortion_coeff);cvShowImage(result,current_frame_rgb); float intr33 = 0.0;float dist4 = 0.0;float tranv3 = 0.0;float rotv3 = 0.0; for ( int i = 0; i 3; i+)for ( int j = 0; j data.ptr + intrinsics-step*i)j;disti = (float*)(distortion_coeff-data.ptr)i;tranvi = (float*)(translation_vectors-data.ptr)i;rotvi = (float*)(rotation_vectors-data.ptr)i;dist3 = (float*)(distortion_coeff-data.ptr)3; printf(-n);printf(INTRINSIC MATRIX: n);printf( %6.4f %6.4f %6.4f n, intr00, intr01, intr02);printf( %6.4f %6.4f %6.4f n, intr10, intr11, intr12);printf( %6.4f %6.4f %6.4f n, intr20, intr21, intr22);printf(-n);printf(DISTORTION VECTOR: n);printf( %6.4f %6.4f %6.4f %6.4f n, dist0, dist1, dist2, dist3);printf(-n);printf(ROTATION VECTOR: n);printf( %6.4f %6.4f %6.4f n, rotv0, rotv1, rotv2);printf(TRANSLATION VECTOR: n);printf( %6.4f %6.4f %6.4f n, tranv0, tranv1, tranv2);printf(-n); cvWaitKey(0); calibration_done = true; exit(0); 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 floatNImages*NPoints*3; / for now, assuming were row-scanning 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); int myFindChessboardCorners( const void* image, CvSize pattern_size, CvPoint2D32f* corners, int* corner_count, int flags ) IplImage* eig = cvCreateImage( cvGetSize(image), 32, 1 );IplImage* temp = cvCreateImage( cvGetSize(image), 32, 1 );double quality = 0.01;double min_distance = 5;int win_size =10; int count = pattern_size.width * pattern_size.height;cvGoodFeaturesToTrack( image, eig, temp, corners, &count