摄像头棋盘标定（源代码）

#include <stdio.h>

#include <stdlib.h>

#include <string.h>

// OpenCV

#include <cxcore.h>

#include <cv.h>

#include <highgui.h>

#include <cvaux.h>

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;

int image_height = 480;

bool verbose = false;

const int ChessBoardSize_w = 7;

const int ChessBoardSize_h = 7;

// Calibration stuff

bool calibration_done = false;

const CvSize ChessBoardSize = cvSize(ChessBoardSize_w,ChessBoardSize_h);

//float SquareWidth = 21.6f; //实际距离 毫米单位 在A4纸上为两厘米

float SquareWidth = 24; //投影实际距离 毫米单位 (我量的实际尺寸为24mm每个方格)

const int NPoints = ChessBoardSize_w*ChessBoardSize_h;

const int NImages = 20; //Number of images to collect

CvPoint2D32f corners[NPoints*NImages];

int corner_count[NImages] = {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;

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(argv[1]) == 1 && isdigit(argv[1][0])))

capture = cvCaptureFromCAM( argc == 2 ? argv[1][0] - '0' : 0 );

else if( argc == 2 )

capture = cvCaptureFromAVI( argv[1] );

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)

{

while (captured_frames < NImages)

{

current_frame_rgb = cvQueryFrame( capture );

//current_frame_rgb = cvLoadImage( "c:\\BoardStereoL3.jpg" );

//cvCopy(chessBoard_Img,current_frame_rgb);

if( !current_frame_rgb )

break;

cvCopy(current_frame_rgb,current_frame_rgb2);

cvCvtColor(current_frame_rgb, current_frame_gray, CV_BGR2GRAY);

//cvThreshold(current_frame_gray,current_frame_gray,Thresholdness,255,CV_THRESH_BINARY);

//cvThreshold(current_frame_gray,current_frame_gray,150,255,CV_THRESH_BINARY_INV);

/*

int pos = 1;

IplConvKernel* element = 0;

const int element_shape = CV_SHAPE_ELLIPSE;

element = cvCreateStructuringElementEx( pos*2+1, pos*2+1, pos, pos, element_shape, 0 );

cvDilate(current_frame_gray,current_frame_gray,element,1);

cvErode(current_frame_gray,current_frame_gray,element,1);

cvReleaseStructuringElement(&element);

*/

find_corners_result = cvFindChessboardCorners(current_frame_gray,

ChessBoardSize,

&corners[captured_frames*NPoints],

&corner_count[captured_frames],

0);

cvDrawChessboardCorners(current_frame_rgb2, ChessBoardSize, &corners[captured_frames*NPoints], NPoints, find_corners_result);

cvShowImage("Window 0",current_frame_rgb2);

cvShowImage("grid",chessBoard_Img);

if(find_corners_result==1)

{

cvWaitKey(2000);

cvSaveImage("e:\\hardyinCV.jpg",current_frame_rgb2);

captured_frames++;

}

//cvShowImage("result",current_frame_gray);

intrinsics->data.fl[0] = 256.8093262; //fx

intrinsics->data.fl[2] = 160.2826538; //cx

intrinsics->data.fl[4] = 254.7511139; //fy

intrinsics->data.fl[5] = 127.6264572; //cy

intrinsics->data.fl[1] = 0;

intrinsics->data.fl[3] = 0;

intrinsics->data.fl[6] = 0;

intrinsics->data.fl[7] = 0;

intrinsics->data.fl[8] = 1;

distortion_coeff->data.fl[0] = -0.193740; //k1

distortion_coeff->data.fl[1] = -0.378588; //k2

distortion_coeff->data.fl[2] = 0.028980; //p1

distortion_coeff->data.fl[3] = 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);

IplImage* img = cvCloneImage(current_frame_rgb);

// [fx 0 cx; 0 fy cy; 0 0 1].

//cvUndistort2(current_frame_rgb,current_frame_rgb,intrinsics,distortion_coeff);

cvUndistort2(current_frame_rgb,img,intrinsics,distortion_coeff);

cvShowImage("result",img);

float intr[3][3] = {0.0};

float dist[4] = {0.0};

float tranv[3] = {0.0};

float rotv[3] = {0.0};

for ( int i = 0; i < 3; i++)

{

for ( int j = 0; j < 3; j++)

{

intr[i][j] = ((float*)(intrinsics->data.ptr + intrinsics->step*i))[j];

}

dist[i] = ((float*)(distortion_coeff->data.ptr))[i];

tranv[i] = ((float*)(translation_vectors->data.ptr))[i];

rotv[i] = ((float*)(rotation_vectors->data.ptr))[i];

}

dist[3] = ((float*)(distortion_coeff->data.ptr))[3];

printf("-----------------------------------------\n");

printf("INTRINSIC MATRIX: \n");

printf("[ %6.4f %6.4f %6.4f ] \n", intr[0][0], intr[0][1], intr[0][2]);

printf("[ %6.4f %6.4f %6.4f ] \n", intr[1][0], intr[1][1], intr[1][2]);

printf("[ %6.4f %6.4f %6.4f ] \n", intr[2][0], intr[2][1], intr[2][2]);

printf("-----------------------------------------\n");

printf("DISTORTION VECTOR: \n");

printf("[ %6.4f %6.4f %6.4f %6.4f ] \n", dist[0], dist[1], dist[2], dist[3]);

printf("-----------------------------------------\n");

printf("ROTATION VECTOR: \n");

printf("[ %6.4f %6.4f %6.4f ] \n", rotv[0], rotv[1], rotv[2]);

printf("TRANSLATION VECTOR: \n");

printf("[ %6.4f %6.4f %6.4f ] \n", tranv[0], tranv[1], tranv[2]);

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 float[NImages*NPoints*3];

// for now, assuming we're 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,

quality, min_distance, 0, 3, 0, 0.04 );

cvFindCornerSubPix( image, corners, count,

cvSize(win_size,win_size), cvSize(-1,-1),

cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03));

cvReleaseImage( &eig );

cvReleaseImage( &temp );

return 1;

}

void makeChessBoard()

{

CvScalar e;

e.val[0] =255;

e.val[1] =255;

e.val[2] =255;

cvSet(chessBoard_Img,e,0);

for(int i = 0;i<ChessBoardSize.width+1;i++)

for(int j = 0;j<ChessBoardSize.height+1;j++)

{

int w =(image_width)/2/(ChessBoardSize.width);

int h = w; //(image_height)/2/(ChessBoardSize.height);

int ii = i+1;

int iii = ii+1;

int jj =j+1;

int jjj =jj+1;

int s_x = image_width/6;

if((i+j)%2==1)

cvRectangle( chessBoard_Img, cvPoint(w*i+s_x,h*j+s_x),cvPoint(w*ii-1+s_x,h*jj-1+s_x), CV_RGB(0,0,0),CV_FILLED, 8, 0 );

}

}