std::vector<CvPoint2D32f> findCorners(Image<PixRGB<byte> > &img, int rows, int cols) { int count = 0; std::vector<CvPoint2D32f> corners(rows*cols); Image<byte> in = luminance(img); int result = cvFindChessboardCorners(img2ipl(in), cvSize(rows,cols), &corners[0], &count, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE | CV_CALIB_CB_FILTER_QUADS); // result = 0 if not all corners were found // Find corners to an accuracy of 0.1 pixel if(result != 0) { cvFindCornerSubPix(img2ipl(in), &corners[0], count, cvSize(10,10), //win cvSize(-1,-1), //zero_zone cvTermCriteria(CV_TERMCRIT_ITER,1000,0.01) ); return corners; } else { return std::vector<CvPoint2D32f>(); } }
int cvFindChessBoardCornerGuesses( const void* arr, void*, CvMemStorage*, CvSize pattern_size, CvPoint2D32f* corners, int* corner_count ) { return cvFindChessboardCorners( arr, pattern_size, corners, corner_count, CV_CALIB_CB_ADAPTIVE_THRESH ); }
bool CybCamCalibration::generateDataToCalibration(IplImage *rgb_image) { IplImage *view_gray; int found = 0, count = 0; img_size = cvGetSize(rgb_image); found = cvFindChessboardCorners( rgb_image, board_size, image_points_buf, &count, CV_CALIB_CB_ADAPTIVE_THRESH ); // improve the found corners' coordinate accuracy view_gray = cvCreateImage( cvGetSize(rgb_image), 8, 1 ); cvCvtColor( rgb_image, view_gray, CV_BGR2GRAY ); cvFindCornerSubPix( view_gray, image_points_buf, count, cvSize(11,11), cvSize(-1,-1), cvTermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 )); cvReleaseImage( &view_gray ); if( found ) { cvSeqPush( image_points_seq, image_points_buf ); sprintf( imagename, "image%03d.png", image_points_seq->total - 1 ); cvSaveImage( imagename, rgb_image ); } cvDrawChessboardCorners( rgb_image, board_size, image_points_buf, count, found ); return ((unsigned)image_points_seq->total >= (unsigned)image_count); }
int main(int argc, char* argv[]) { board_w = 5; // Board width in squares board_h = 8; // Board height n_boards = 8; // Number of boards int board_n = board_w * board_h; CvSize board_sz = cvSize( board_w, board_h ); CvCapture* capture = cvCreateCameraCapture( 0 ); assert( capture ); cvNamedWindow( "Calibration" ); // Allocate Sotrage 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( 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 ); // Capture Corner views loop until we've got n_boards // succesful captures (all corners on the board are found) while( successes < n_boards ){ // Skp 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 ){ step = successes*board_n; for( int i=step, j=0; j < board_n; ++i, ++j ){ 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++; } }
// // コーナーを検出する // // 引数: // frameImage : キャプチャ画像用IplImage // grayImage : グレースケール画像用IplImage // corners : コーナーの位置を格納する変数 // // 戻り値: // 0 : コーナーがすべて検出できなかった場合 // 非0 : コーナーがすべて検出された場合 // int findCorners( IplImage *frameImage, IplImage *grayImage, CvPoint2D32f *corners ) { int cornerCount; // 検出したコーナーの数 int findChessboardCornersFlag; // cvFindChessboardCorners用フラグ int findFlag; // コーナーがすべて検出できたかのフラグ IplImage* m_image_binary; IplImage* m_set_image; m_image_binary = cvCreateImage(cvSize(frameImage->width, frameImage->height), IPL_DEPTH_8U, 1); m_set_image = cvCreateImage(cvSize(frameImage->width, frameImage->height), IPL_DEPTH_8U, 3); // cvChessboardCorners用フラグを生成する findChessboardCornersFlag = createFindChessboardCornersFlag(); // 画像をBinaryImageとして変換する。 // コーナーを検出する cvCvtColor( frameImage, grayImage, CV_BGR2GRAY ); // グレースケールから2値に変換する cvThreshold( grayImage, m_image_binary, 128, 255, CV_THRESH_BINARY ); // Convert to 3channel image cvMerge(m_image_binary, m_image_binary, m_image_binary, NULL, m_set_image); findFlag=cvFindChessboardCorners( m_set_image, //m_set_image, //cvSize( CORNER_WIDTH, CORNER_HEIGHT ), board_sz, corners, &cornerCount, findChessboardCornersFlag ); if( findFlag != 0 ) { // コーナーがすべて検出された場合 // 検出されたコーナーの位置をサブピクセル単位にする CvTermCriteria criteria={ CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, MAX_ITERATIONS, EPSILON }; cvFindCornerSubPix( grayImage, corners, cornerCount, cvSize( SEARCH_WINDOW_HALF_WIDTH, SEARCH_WINDOW_HALF_HEIGHT ), cvSize( DEAD_REGION_HALF_WIDTH, DEAD_REGION_HALF_HEIGHT ), cvTermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, MAX_ITERATIONS, EPSILON ) ); } // コーナーの位置を描く cvDrawChessboardCorners( frameImage, board_sz, corners, cornerCount, findFlag ); cvReleaseImage(&m_set_image); cvReleaseImage(&m_image_binary); return findFlag; }
//borrowed from Patrick... int do_calibration() { IplImage* image = NULL; int xc = 12; int yc = 12; int nc = xc*yc; float side_length = 15; CvSize board_sz = cvSize(xc, yc); int ntrials = img_cnt; char filename[128]; CvMat* object_points = cvCreateMat( ntrials * nc, 3, CV_32FC1 ); CvMat* image_points = cvCreateMat( ntrials * nc, 2, CV_32FC1 ); CvMat* point_counts = cvCreateMat( ntrials, 1, CV_32SC1 ); CvPoint2D32f* corners = new CvPoint2D32f[nc]; for(int t=1; t<=ntrials; t++) { // load an image sprintf(filename, "%s/Image%d.jpg", dir_name, t); image = cvLoadImage(filename); if(!image){ printf("Could not load image file: %s\n",filename); exit(0); } int corner_count; int found = cvFindChessboardCorners(image, board_sz, corners, &corner_count, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS); IplImage* gray = cvCreateImage(cvSize(image->width, image->height), IPL_DEPTH_8U, 1); cvCvtColor(image, gray, CV_BGR2GRAY); cvFindCornerSubPix(gray, corners, corner_count, cvSize(5, 5), cvSize(-1, -1), cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 10, 0.01f )); printf("Image: %d, %d / %d\n", t, corner_count, nc); // get data points if(corner_count == nc && found != 0) { for(int c=0; c<nc; c++) { CV_MAT_ELEM( *image_points, float, (t-1)*nc+c, 0 ) = corners[c].x; CV_MAT_ELEM( *image_points, float, (t-1)*nc+c, 1 ) = corners[c].y; CV_MAT_ELEM( *object_points, float, (t-1)*nc+c, 0 ) = (float)(c/xc)*side_length; CV_MAT_ELEM( *object_points, float, (t-1)*nc+c, 1 ) = (float)(c%xc)*side_length; CV_MAT_ELEM( *object_points, float, (t-1)*nc+c, 2 ) = 0.0f; } CV_MAT_ELEM( *point_counts, int, t-1, 0 ) = nc; } else printf("Bad board! How did this happen?\n"); }
void Calibrate(){ CvPoint2D32f* corners = new CvPoint2D32f[ board_n ]; int corner_count; int successes = 0; int step, frame = 0; IplImage *image = cvQueryFrame( capture ); /*(IplImage *image = cvQueryFrame( capture ); cvSetImageROI(image, cvRect(300, 300, 600, 600)); tmp = cvCreateImage(cvGetSize(image), image->depth, image->nChannels); cvCopy(image, tmp, NULL); cvResetImageROI(image); image = cvCloneImage(tmp);//*/ IplImage *gray_image = cvCreateImage( cvGetSize( image ), 8, 1 ); // Capture Corner views loop until we've got n_boards // successful captures (all corners on the board are found) while( successes < n_boards ){ // Skp 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 ){ step = successes*board_n; for( int i=step, j=0; j < board_n; ++i, ++j ){ 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++; } }
void get_calib(CvCapture* capture) { int board_n = board_w * board_h; CvSize board_sz = cvSize(board_w, board_h); cvNamedWindow("Calibration"); 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); while(successes < n_boards) { if(frame++ % board_dt == 0) { int found = cvFindChessboardCorners( image, board_sz, corners, &corner_count, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS); cvCvtColor(image, gray_image, CV_RGB2GRAY); cvFindCornerSubPix(gray_image, corners, corner_count, cvSize(11,11), cvSize(-1, -1), cvTermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1)); cvDrawChessboardCorners(image, board_sz, corners, corner_count, found); cvShowImage("Calibration", image); //get a good board, add to data if(corner_count == board_n) { printf("here\n"); step = successes * board_n; for(int i = step, j = 0; j < board_n; ++i, ++j) { 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++; printf("Nice shot!\n"); int k = cvWaitKey(-1); while(k != 'n') k = cvWaitKey(-1); } }
/* search for chessboard */ void chessboard_finder(IplImage *frame) { CvPoint2D32f points[70]; int cornercount; if (cvFindChessboardCorners(frame, cvSize(10,7), points, &cornercount, 0)) { /* found chessboard */ cvDrawChessboardCorners(frame, cvSize(10,7), points, cornercount, 1); } }
int StereoVision::calibrationAddSample(IplImage* imageLeft,IplImage* imageRight) { if(!calibrationStarted) return RESULT_FAIL; IplImage* image[2] = {imageLeft,imageRight}; int succeses = 0; for(int lr=0; lr<2; lr++) { CvSize imageSize = cvGetSize(image[lr]); if(imageSize.width != this->imageSize.width || imageSize.height != this->imageSize.height) return RESULT_FAIL; int cornersDetected = 0; //FIND CHESSBOARDS AND CORNERS THEREIN: int result = cvFindChessboardCorners( image[lr], cvSize(cornersX, cornersY), &ponintsTemp[lr][0], &cornersDetected, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE ); if(result && cornersDetected == cornersN) { //Calibration will suffer without subpixel interpolation cvFindCornerSubPix( image[lr], &ponintsTemp[lr][0], cornersDetected, cvSize(11, 11), cvSize(-1,-1), cvTermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS,30, 0.01) ); succeses++; } } if(2==succeses) { for(int lr=0; lr<2; lr++) { points[lr].resize((sampleCount+1)*cornersN); copy( ponintsTemp[lr].begin(), ponintsTemp[lr].end(), points[lr].begin() + sampleCount*cornersN); } sampleCount++; return RESULT_OK; } else { return RESULT_FAIL; } }
const CheckerboardDetector::Checkerboard & CheckerboardDetector::detect(const Img8u &image){ const Img8u *useImage = ℑ if(image.getFormat() != formatGray && image.getChannels() != 1){ m_data->grayBuf.setFormat(formatGray); m_data->grayBuf.setSize(image.getSize()); cc(&image, &m_data->grayBuf); useImage = &m_data->grayBuf; } img_to_ipl(useImage, &m_data->ipl); std::vector<CvPoint2D32f> corners(m_data->cb.size.getDim()); CvSize s = {m_data->cb.size.width, m_data->cb.size.height }; int n = corners.size(); m_data->cb.found = cvFindChessboardCorners(m_data->ipl, s, corners.data(), &n, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS); bool optSubPix = getPropertyValue("subpixel opt.enabled"); int radius = getPropertyValue("subpixel opt.radius"); int innerR = getPropertyValue("subpixel opt.inner radius"); if(innerR >= radius) innerR = radius -1; if(innerR == 0) innerR = -1; int maxIter = getPropertyValue("subpixel opt.max iterations"); float minErr = getPropertyValue("subpixel opt.min error"); if(m_data->cb.found && optSubPix){ cvFindCornerSubPix(m_data->ipl, corners.data(), corners.size(), cvSize(radius,radius), cvSize(innerR, innerR), cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, maxIter, minErr)); } if(m_data->cb.found){ m_data->cb.corners.resize(corners.size()); for(size_t i=0;i<corners.size();++i){ m_data->cb.corners[i] = Point32f(corners[i].x, corners[i].y); } }else{ m_data->cb.corners.clear(); } return m_data->cb; }
static void mainLoop(void) { ARUint8 *dataPtr; int cornerCount; char buf[256]; int i; if ((dataPtr = arVideoGetImage()) == NULL) { arUtilSleep(2); return; } glClear(GL_COLOR_BUFFER_BIT); argDrawMode2D(vp); argDrawImage(dataPtr); // Convert to grayscale, results will go to arIPI->image, which also provides the backing for calibImage. arImageProcLuma(arIPI, dataPtr); cornerFlag = cvFindChessboardCorners(calibImage, cvSize(chessboardCornerNumY, chessboardCornerNumX), corners, &cornerCount, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS); if (cornerFlag) glColor4ub(255, 0, 0, 255); else glColor4ub(0, 255, 0, 255); glLineWidth(2.0f); // ARLOG("Detected corners = %d\n", cornerCount); for (i = 0; i < cornerCount; i++) { argDrawLineByObservedPos(corners[i].x - 5, corners[i].y - 5, corners[i].x + 5, corners[i].y + 5); argDrawLineByObservedPos(corners[i].x - 5, corners[i].y + 5, corners[i].x + 5, corners[i].y - 5); // ARLOG(" %f, %f\n", corners[i].x, corners[i].y); sprintf(buf, "%d\n", i); argDrawStringsByObservedPos(buf, corners[i].x, corners[i].y + 20); } sprintf(buf, "Captured Image: %2d/%2d\n", capturedImageNum, calibImageNum); argDrawStringsByObservedPos(buf, 10, 30); argSwapBuffers(); }
void compute_and_display_image_corners(char * imageName, CvSize * imageSize, CvSize chessboardSize, CvPoint2D32f * cornersArrayToFillIn) { IplImage * img = 0; int cornersCount = 0; int patternWasFound = 0; int i; img = cvLoadImage(imageName, 1); *imageSize = cvGetSize(img); // useful only for calibration function //initialisation of the given array for(i=0;i<chessboardSize.height*chessboardSize.width;i++){ cornersArrayToFillIn[i].x= 0; cornersArrayToFillIn[i].y= 0; } printf("OK1\n"); // core algorithm patternWasFound = cvFindChessboardCorners(img, chessboardSize, cornersArrayToFillIn, &cornersCount, 0); printf("OK2\n"); // display_array_values(cornersArrayToFillIn,chessboardSize.height*chessboardSize.width); improve_precision(img, cornersArrayToFillIn, cornersCount); // display_array_values(cornersArrayToFillIn,chessboardSize.height*chessboardSize.width); // visual only part cvDrawChessboardCorners(img, chessboardSize, cornersArrayToFillIn, cornersCount, patternWasFound); cvNamedWindow(imageName, CV_WINDOW_AUTOSIZE); cvMoveWindow(imageName, 100, 100); cvShowImage(imageName, img ); cvWaitKey(200); cvDestroyWindow(imageName); // end cvReleaseImage(&img ); }
bool CamCalib::FindChessboard(IplImage *src, IplImage *dst) { IplImage *gray = cvCreateImage (cvGetSize(src), IPL_DEPTH_8U, 1); cvCvtColor(src, gray, CV_BGR2GRAY); // 체스판 코너 찾기 CvPoint2D32f* corners = new CvPoint2D32f[_board_n]; int corner_count = 0; int found = cvFindChessboardCorners(src, cvSize(_board_w, _board_h), corners, &corner_count, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS ); // 검출된 코너로부터 서브픽셀 정확도로 코너 좌표를 구한다. cvFindCornerSubPix (gray, corners, corner_count, cvSize(11,11), cvSize(-1,-1), cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 )); // 코너를 dst 이미지에 그린다. cvDrawChessboardCorners (dst, cvSize(_board_w, _board_h), corners, corner_count, found); // 코너를 정상적으로 찾았다면, 코너 데이터를 저장한다. bool ret = false; if (found && corner_count == _board_n) { for( int i=_successes*_board_n, j=0; j<_board_n; ++i, ++j ) { 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) = (float)(j%_board_w)*_cell_w; CV_MAT_ELEM(*_object_points,float, i, 1) = (float)(_board_h - j/_board_w - 1)*_cell_h; CV_MAT_ELEM(*_object_points,float, i, 2) = 0.0f; } CV_MAT_ELEM(*_point_counts, int, _successes, 0) = _board_n; ret = true; } delete [] corners; cvReleaseImage(&gray); return ret; }
int main(int argc, char * argv[]) { int corner_count; int successes = 0; int step, frame = 0; const char* intrinsics_path = argv[1]; const char* distortions_path = argv[2]; int total = argc - 3 ; int start = 3; const char* loc = argv[start] ; board_w = 7; // Board width in squares board_h = 4; // Board height n_boards = total; // Number of boards int board_n = board_w * board_h; CvSize board_sz = cvSize( board_w, board_h ); // Allocate Sotrage 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( 5, 1, CV_32FC1 ); CvPoint2D32f* corners = new CvPoint2D32f[ board_n ]; IplImage *image = cvLoadImage( loc ); //IplImage *image = cvQueryFrame(capture); IplImage *gray_image = cvCreateImage( cvGetSize( image ), 8, 1 ); // Capture Corner views loop until we've got n_boards // succesful captures (all corners on the board are found) while( start < total ){ // Skp 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 ); if( found ) { cvSaveImage( "/tmp/grid_save.png", image); } // If we got a good board, add it to our data if( corner_count == board_n ){ step = successes*board_n; for( int i=step, j=0; j < board_n; ++i, ++j ){ 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++; } // } if( start < total ) { start++; } else if ( start == total ) { start = 1; // return -1; } loc = argv[start] ; image = cvLoadImage( loc ); } // End collection while loop
void get_bird_eye(CvCapture* capture) { printf("haha\n"); //get bird_eye picture cvNamedWindow("Get_birdeye"); CvMat *intrinsic = (CvMat*) cvLoad("Intrinsics.xml"); CvMat *distortion = (CvMat*) cvLoad("Distortion.xml"); IplImage *image = cvQueryFrame(capture); IplImage *gray_image = cvCreateImage(cvGetSize(image), 8, 1); int board_n = board_w * board_h; IplImage* mapx = cvCreateImage(cvGetSize(image), IPL_DEPTH_32F, 1); IplImage* mapy = cvCreateImage(cvGetSize(image), IPL_DEPTH_32F, 1); cvInitUndistortMap( intrinsic, distortion, mapx, mapy ); CvSize board_sz = cvSize(board_w, board_h); CvPoint2D32f* corners = new CvPoint2D32f[board_n]; int frame = 0; int corner_count; bool catch_bird = false; while(!catch_bird) { IplImage *t = cvCloneImage(image); if(frame++ % board_dt == 0) { IplImage* t = cvCloneImage(image); cvRemap(t, image, mapx, mapy); int found = cvFindChessboardCorners( image, board_sz, corners, &corner_count, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS ); cvCvtColor( image, gray_image, CV_RGB2GRAY ); cvFindCornerSubPix( gray_image, corners, corner_count, cvSize(11,11), cvSize(-1, -1), cvTermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1) ); cvDrawChessboardCorners( image, board_sz, corners, corner_count, found ); cvShowImage("Get_birdeye", image); //get a good board, add to data if(corner_count == board_n) { catch_bird = true; printf("That's it!\n"); } } int c; if(catch_bird) c = cvWaitKey(-1); else c = cvWaitKey(15); if(catch_bird && c == 's') { cvSaveImage("./Resource/bird-eye.jpg", t, 0); printf("save at ./Resource/bird-eye.jpg\n"); } else catch_bird = false; if(c == 'p') { c = 0; while(c!='p' && c!= 27){ c = cvWaitKey(250); } } image = cvQueryFrame(capture); } }
int CCalibration::CalibrateFromCAMorAVI( int nImages, int nChessRow, int nChessCol, int nSquareSize, int nSkip, CvMat* intrinsic, CvMat* distortion, int fromCamera, char* fName) { int nChessSize = nChessRow*nChessCol; int nAllPoints = nImages*nChessSize; int i, j, k; int corner_count, found; int* p_count = new int[nImages]; IplImage **src_img = new IplImage*[nImages]; CvSize pattern_size = cvSize (nChessCol, nChessRow); CvPoint3D32f* objects = new CvPoint3D32f[nAllPoints]; CvPoint2D32f *corners = (CvPoint2D32f *) cvAlloc (sizeof (CvPoint2D32f) * nAllPoints); CvMat object_points; CvMat image_points; CvMat point_counts; CvMat *rotation = cvCreateMat (1, 3, CV_32FC1); CvMat *translation = cvCreateMat (1, 3, CV_32FC1); // (1) for (i = 0; i < nImages; i++) { for (j = 0; j < nChessRow; j++) { for (k = 0; k < nChessCol; k++) { objects[i * nChessSize + j * nChessCol + k].x = (float)j * nSquareSize; objects[i * nChessSize + j * nChessCol + k].y = (float)k * nSquareSize; objects[i * nChessSize + j * nChessCol + k].z = 0.0; } } } cvInitMatHeader (&object_points, nAllPoints, 3, CV_32FC1, objects); // (2) CvCapture *capture = NULL; if (fromCamera) capture = cvCaptureFromCAM(0); else capture = cvCaptureFromAVI(fName); assert(capture); int found_num = 0; cvNamedWindow ("Calibration", CV_WINDOW_AUTOSIZE); cvNamedWindow ("Webcam", CV_WINDOW_AUTOSIZE); int c = 0; for (i = 0; i < nImages; i++) { IplImage * frame; while (true) { frame = cvQueryFrame(capture); cvShowImage("Webcam", frame); if (c++ % nSkip == 0) { found = cvFindChessboardCorners (frame, pattern_size, &corners[i * nChessSize], &corner_count); if (found) { char s[100]; sprintf(s, "%d.png", i); cvSaveImage(s, frame); src_img[i] = cvCloneImage(frame); fprintf (stderr, "ok\n"); found_num++; break; } } cvWaitKey(5); } fprintf (stderr, "%02d...", i); // (4) IplImage *src_gray = cvCreateImage (cvGetSize (src_img[i]), IPL_DEPTH_8U, 1); cvCvtColor (src_img[i], src_gray, CV_BGR2GRAY); cvFindCornerSubPix (src_gray, &corners[i * nChessSize], corner_count, cvSize (3, 3), cvSize (-1, -1), cvTermCriteria (CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03)); cvDrawChessboardCorners (src_img[i], pattern_size, &corners[i * nChessSize], corner_count, found); p_count[i] = corner_count; cvShowImage ("Calibration", src_img[i]); //cvWaitKey (0); } cvDestroyWindow ("Calibration"); if (found_num != nImages) return -1; cvInitMatHeader (&image_points, nAllPoints, 1, CV_32FC2, corners); cvInitMatHeader (&point_counts, nImages, 1, CV_32SC1, p_count); // (5) cvCalibrateCamera2 (&object_points, &image_points, &point_counts, cvSize (640, 480), intrinsic, distortion); // (6) /* CvMat sub_image_points, sub_object_points; int base = 0; cvGetRows (&image_points, &sub_image_points, base * nChessSize, (base + 1) * nChessSize); cvGetRows (&object_points, &sub_object_points, base * nChessSize, (base + 1) * nChessSize); cvFindExtrinsicCameraParams2 (&sub_object_points, &sub_image_points, intrinsic, distortion, rotation, translation); */ // (7)ToXML /* CvFileStorage *fs; fs = cvOpenFileStorage ("camera.xml", 0, CV_STORAGE_WRITE); cvWrite (fs, "intrinsic", intrinsic); cvWrite (fs, "rotation", rotation); cvWrite (fs, "translation", translation); cvWrite (fs, "distortion", distortion); cvReleaseFileStorage (&fs);*/ for (i = 0; i < nImages; i++) cvReleaseImage (&src_img[i]); delete p_count; delete src_img; delete objects; return 1; }
int CCalibration::CalibrateFromFiles(int nImages, int nChessRow, int nChessCol, float nSquareSize, CvMat* intrinsic, CvMat* distortion) { int nChessSize = nChessRow*nChessCol; int nAllPoints = nImages*nChessSize; int i, j, k; int corner_count, found; int* p_count = new int[nImages]; IplImage **src_img = new IplImage*[nImages]; CvSize pattern_size = cvSize (nChessCol, nChessRow); CvPoint3D32f* objects = new CvPoint3D32f[nAllPoints]; CvPoint2D32f *corners = (CvPoint2D32f *) cvAlloc (sizeof (CvPoint2D32f) * nAllPoints); CvMat object_points; CvMat image_points; CvMat point_counts; //CvMat *rotation = cvCreateMat (1, 3, CV_32FC1); //CvMat *translation = cvCreateMat (1, 3, CV_32FC1); CvMat *PMatrix = cvCreateMat (3, 4, CV_32FC1); CvMat *tmp33 = cvCreateMat(3,3,CV_32FC1); CvMat *tmp31 = cvCreateMat(3,1,CV_32FC1); CvMat *roTrans = cvCreateMat(3,3,CV_32FC1); CvMat *translationTrans = cvCreateMat(3,1,CV_32FC1); // (1) for (i = 0; i < nImages; i++) { char buf[32]; sprintf (buf, "%d.png", i); if ((src_img[i] = cvLoadImage (buf, CV_LOAD_IMAGE_COLOR)) == NULL) { fprintf (stderr, "cannot load image file : %s\n", buf); } } // (2) for (i = 0; i < nImages; i++) { for (j = 0; j < nChessRow; j++) { for (k = 0; k < nChessCol; k++) { objects[i * nChessSize + j * nChessCol + k].x = j * nSquareSize; objects[i * nChessSize + j * nChessCol + k].y = k * nSquareSize; objects[i * nChessSize + j * nChessCol + k].z = 0.0; } } } cvInitMatHeader (&object_points, nAllPoints, 3, CV_32FC1, objects); // (3) int found_num = 0; //cvNamedWindow ("Calibration", CV_WINDOW_AUTOSIZE); for (i = 0; i < nImages; i++) { found = cvFindChessboardCorners (src_img[i], pattern_size, &corners[i * nChessSize], &corner_count); fprintf (stderr, "%02d...", i); if (found) { fprintf (stderr, "ok\n"); found_num++; } else { fprintf (stderr, "fail\n"); } // (4) IplImage *src_gray = cvCreateImage (cvGetSize (src_img[i]), IPL_DEPTH_8U, 1); cvCvtColor (src_img[i], src_gray, CV_BGR2GRAY); cvFindCornerSubPix (src_gray, &corners[i * nChessSize], corner_count, cvSize (3, 3), cvSize (-1, -1), cvTermCriteria (CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03)); p_count[i] = corner_count; cvWaitKey (0); } if (found_num != nImages) return -1; cvInitMatHeader (&image_points, nAllPoints, 1, CV_32FC2, corners); cvInitMatHeader (&point_counts, nImages, 1, CV_32SC1, p_count); // (5) cvCalibrateCamera2 (&object_points, &image_points, &point_counts, cvSize (640, 480), intrinsic, distortion); /* (6) ***************** TEST EXTRINSIC PARAMETERS ****************** CvMat sub_image_points, sub_object_points; int base = 0; cvNamedWindow ("Calibration", CV_WINDOW_AUTOSIZE); CModel myBox("box.txt"); for (i=0; i<nImages; i++) { cvGetRows (&image_points, &sub_image_points, base * nChessSize, (base + 1) * nChessSize); cvGetRows (&object_points, &sub_object_points, base * nChessSize, (base + 1) * nChessSize); cvFindExtrinsicCameraParams2 (&sub_object_points, &sub_image_points, intrinsic, distortion, rotation, translation); CvMat *rotMat = cvCreateMat(3, 3, CV_32FC1); cvRodrigues2(rotation, rotMat); //cvTranspose(rotMat,roTrans); cvTranspose(translation,translationTrans); CvMat* tmp34 = cvCreateMat(3,4,CV_32FC1); for (int i = 0; i < 3; i++) for (int j=0; j<3; j++) cvmSet(tmp34,i,j,cvmGet(rotMat,i,j)); for (int i = 0; i < 3; i++) cvmSet(tmp34,i,3,cvmGet(translationTrans,i,0)); cvMatMul(intrinsic,tmp34,PMatrix); myBox.Draw(src_img[i], PMatrix, cvScalar(0, 255, 0), 2); cvShowImage ("Calibration", src_img[i]); cvWaitKey(0); base++; }*/ cvDestroyWindow("Calibration"); // Luu ket qua /* CvFileStorage *fs; fs = cvOpenFileStorage ("camera.xml", 0, CV_STORAGE_WRITE); cvWrite (fs, "intrinsic", intrinsic); cvWrite (fs, "rotation", rotation); cvWrite (fs, "translation", translation); cvWrite (fs, "distortion", distortion); cvReleaseFileStorage (&fs);*/ for (i = 0; i < nImages; i++) { cvReleaseImage (&src_img[i]); } delete p_count; delete src_img; delete objects; return 1; }
int main (int argc, char *argv[]) { int i, j, k; int corner_count, found; int p_count[IMAGE_NUM]; IplImage *src_img[IMAGE_NUM]; CvSize pattern_size = cvSize (PAT_COL, PAT_ROW); CvPoint3D32f objects[ALL_POINTS]; CvPoint2D32f *corners = (CvPoint2D32f *) cvAlloc (sizeof (CvPoint2D32f) * ALL_POINTS); CvMat object_points; CvMat image_points; CvMat point_counts; CvMat *intrinsic = cvCreateMat (3, 3, CV_32FC1); CvMat *rotation = cvCreateMat (1, 3, CV_32FC1); CvMat *translation = cvCreateMat (1, 3, CV_32FC1); CvMat *distortion = cvCreateMat (1, 4, CV_32FC1); // (1)キャリブレーション画像の読み込み for (i = 0; i < IMAGE_NUM; i++) { char buf[32]; sprintf (buf, "calib_img/%02d.png", i); if ((src_img[i] = cvLoadImage (buf, CV_LOAD_IMAGE_COLOR)) == NULL) { fprintf (stderr, "cannot load image file : %s\n", buf); } } // (2)3次元空間座標の設定 for (i = 0; i < IMAGE_NUM; i++) { for (j = 0; j < PAT_ROW; j++) { for (k = 0; k < PAT_COL; k++) { objects[i * PAT_SIZE + j * PAT_COL + k].x = j * CHESS_SIZE; objects[i * PAT_SIZE + j * PAT_COL + k].y = k * CHESS_SIZE; objects[i * PAT_SIZE + j * PAT_COL + k].z = 0.0; } } } cvInitMatHeader (&object_points, ALL_POINTS, 3, CV_32FC1, objects); // (3)チェスボード(キャリブレーションパターン)のコーナー検出 int found_num = 0; cvNamedWindow ("Calibration", CV_WINDOW_AUTOSIZE); for (i = 0; i < IMAGE_NUM; i++) { found = cvFindChessboardCorners (src_img[i], pattern_size, &corners[i * PAT_SIZE], &corner_count); fprintf (stderr, "%02d...", i); if (found) { fprintf (stderr, "ok\n"); found_num++; } else { fprintf (stderr, "fail\n"); } // (4)コーナー位置をサブピクセル精度に修正,描画 IplImage *src_gray = cvCreateImage (cvGetSize (src_img[i]), IPL_DEPTH_8U, 1); cvCvtColor (src_img[i], src_gray, CV_BGR2GRAY); cvFindCornerSubPix (src_gray, &corners[i * PAT_SIZE], corner_count, cvSize (3, 3), cvSize (-1, -1), cvTermCriteria (CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03)); cvDrawChessboardCorners (src_img[i], pattern_size, &corners[i * PAT_SIZE], corner_count, found); p_count[i] = corner_count; cvShowImage ("Calibration", src_img[i]); cvWaitKey (0); } cvDestroyWindow ("Calibration"); if (found_num != IMAGE_NUM) return -1; cvInitMatHeader (&image_points, ALL_POINTS, 1, CV_32FC2, corners); cvInitMatHeader (&point_counts, IMAGE_NUM, 1, CV_32SC1, p_count); // (5)内部パラメータ,歪み係数の推定 cvCalibrateCamera2 (&object_points, &image_points, &point_counts, cvSize (640, 480), intrinsic, distortion); // (6)外部パラメータの推定 CvMat sub_image_points, sub_object_points; int base = 0; cvGetRows (&image_points, &sub_image_points, base * PAT_SIZE, (base + 1) * PAT_SIZE); cvGetRows (&object_points, &sub_object_points, base * PAT_SIZE, (base + 1) * PAT_SIZE); cvFindExtrinsicCameraParams2 (&sub_object_points, &sub_image_points, intrinsic, distortion, rotation, translation); // (7)XMLファイルへの書き出し CvFileStorage *fs; fs = cvOpenFileStorage ("camera.xml", 0, CV_STORAGE_WRITE); cvWrite (fs, "intrinsic", intrinsic); cvWrite (fs, "rotation", rotation); cvWrite (fs, "translation", translation); cvWrite (fs, "distortion", distortion); cvReleaseFileStorage (&fs); for (i = 0; i < IMAGE_NUM; i++) { cvReleaseImage (&src_img[i]); } return 0; }
// -------------------------------------------------------------------------- // main(Number of arguments, Argument values) // Description : This is the entry point of the program. // Return value : SUCCESS:0 ERROR:-1 // -------------------------------------------------------------------------- int main(int argc, char **argv) { // AR.Drone class ARDrone ardrone; // Initialize if (!ardrone.open()) { printf("Failed to initialize.\n"); return -1; } // Images std::vector<IplImage*> images; printf("Press space key to take a sample picture !\n"); // Main loop while (1) { // Key input int key = cvWaitKey(1); if (key == 0x1b) break; // Update if (!ardrone.update()) break; // Get an image IplImage *image = ardrone.getImage(); // Convert the camera image to grayscale IplImage *gray = cvCreateImage(cvGetSize(image), IPL_DEPTH_8U, 1); cvCvtColor(image, gray, CV_BGR2GRAY); // Detect the chessboard int corner_count = 0; CvSize size = cvSize(PAT_COL, PAT_ROW); CvPoint2D32f corners[PAT_SIZE]; int found = cvFindChessboardCorners(gray, size, corners, &corner_count, CV_CALIB_CB_ADAPTIVE_THRESH+CV_CALIB_CB_NORMALIZE_IMAGE|CV_CALIB_CB_FAST_CHECK); // Chessboard detected if (found) { // Draw corners cvDrawChessboardCorners(image, size, corners, corner_count, found); // If you push Space key if (key == ' ') { // Add to buffer images.push_back(gray); } else { // Release the image cvReleaseImage(&gray); } } // Failed to detect else { // Release the image cvReleaseImage(&gray); } // Display the image cvDrawText(image, cvPoint(15, 20), "NUM = %d", (int)images.size()); cvShowImage("camera", image); } // Destroy the window cvDestroyWindow("camera"); // At least one image was taken if (!images.empty()) { // Total number of images const int num = (int)images.size(); //// For debug //for (int i = 0; i < num; i++) { // char name[256]; // sprintf(name, "images[%d/%d]", i+1, num); // cvShowImage(name, images[i]); // cvWaitKey(0); // cvDestroyWindow(name); //} // Ask save parameters or not if (cvAsk("Do you save the camera parameters ? (y/n)\n")) { // Detect coners int *p_count = (int*)malloc(sizeof(int) * num); CvPoint2D32f *corners = (CvPoint2D32f*)cvAlloc(sizeof(CvPoint2D32f) * num * PAT_SIZE); for (int i = 0; i < num; i++) { // Detect chessboard int corner_count = 0; CvSize size = cvSize(PAT_COL, PAT_ROW); int found = cvFindChessboardCorners(images[i], size, &corners[i * PAT_SIZE], &corner_count); // Convert the corners to sub-pixel cvFindCornerSubPix(images[i], &corners[i * PAT_SIZE], corner_count, cvSize(3, 3), cvSize(-1, -1), cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 20, 0.03)); p_count[i] = corner_count; } // Set the 3D position of patterns CvPoint3D32f *objects = (CvPoint3D32f*)cvAlloc(sizeof(CvPoint3D32f) * num * PAT_SIZE); for (int i = 0; i < num; i++) { for (int j = 0; j < PAT_ROW; j++) { for (int k = 0; k < PAT_COL; k++) { objects[i * PAT_SIZE + j * PAT_COL + k].x = j * CHESS_SIZE; objects[i * PAT_SIZE + j * PAT_COL + k].y = k * CHESS_SIZE; objects[i * PAT_SIZE + j * PAT_COL + k].z = 0.0; } } } // Create matrices CvMat object_points, image_points, point_counts; cvInitMatHeader(&object_points, num * PAT_SIZE, 3, CV_32FC1, objects); cvInitMatHeader(&image_points, num * PAT_SIZE, 1, CV_32FC2, corners); cvInitMatHeader(&point_counts, num, 1, CV_32SC1, p_count); // Estimate intrinsic parameters and distortion coefficients printf("Calicurating parameters..."); CvMat *intrinsic = cvCreateMat(3, 3, CV_32FC1); CvMat *distortion = cvCreateMat(1, 4, CV_32FC1); cvCalibrateCamera2(&object_points, &image_points, &point_counts, cvGetSize(images[0]), intrinsic, distortion); printf("Finished !\n"); // Output a file printf("Generating a XML file..."); CvFileStorage *fs = cvOpenFileStorage("camera.xml", 0, CV_STORAGE_WRITE); cvWrite(fs, "intrinsic", intrinsic); cvWrite(fs, "distortion", distortion); cvReleaseFileStorage(&fs); printf("Finished !\n"); // Release the matrices free(p_count); cvFree(&corners); cvFree(&objects); cvReleaseMat(&intrinsic); cvReleaseMat(&distortion); } // Release the images for (int i = 0; i < num; i++) cvReleaseImage(&images[i]); } // See you ardrone.close(); return 0; }
int main (int argc, char *argv[]) { // IMAGE_NUM, PAT_ROW, PAT_COL,PAT_SIZE, ALL_POINTS, CHESS_SIZE /* if (argc < 6) { std::cout<< "ERROR : augment is incorrect" << std::endl; return -1; } */ //int PAT_ROW = atoi(argv[3]); //int PAT_COL = atoi(argv[4]); //int CHESS_SIZE = atoi(argv[5]); //int PAT_SIZE = PAT_ROW*PAT_COL; char* NAME_IMG_IN = argv[1]; //char* NAME_XML_OUT = argv[2]; int i,j; int corner_count, found; IplImage *src_img; CvSize pattern_size = cvSize(PAT_COL, PAT_ROW); CvMat image_points; CvMat object_points; CvMat *intrinsic, *distortion; CvMat *rotation = cvCreateMat(1, 3, CV_32FC1); CvMat *rotationConv = cvCreateMat(3, 3, CV_32FC1); CvMat *translation = cvCreateMat(1, 3, CV_32FC1); CvPoint3D32f objects[PAT_SIZE]; CvFileStorage *fs; CvFileNode *param; CvPoint2D32f *corners = (CvPoint2D32f *) cvAlloc (sizeof (CvPoint2D32f) * PAT_SIZE); // (1)�����оݤȤʤ������ɤ߹��� if ( ( src_img = cvLoadImage(NAME_IMG_IN, CV_LOAD_IMAGE_COLOR) ) == 0) //if (argc < 2 || (src_img = cvLoadImage (argv[1], CV_LOAD_IMAGE_COLOR)) == 0) { std::cout<< "ERROR : input image is not exist or augment is incorrect" << std::endl; return -1; } // 3�������ֺ�ɸ������ for (i = 0; i < PAT_ROW; i++) { for (j = 0; j < PAT_COL; j++) { objects[i * PAT_COL + j].x = i * CHESS_SIZE; objects[i * PAT_COL + j].y = j * CHESS_SIZE; objects[i * PAT_COL + j].z = 0.0; } } cvInitMatHeader(&object_points, PAT_SIZE, 3, CV_32FC1, objects); // �������ܡ��ɡʥ����֥졼�����ѥ�����ˤΥ����ʡ����� int found_num = 0; // cvNamedWindow("Calibration", CV_WINDOW_AUTOSIZE); found = cvFindChessboardCorners(src_img, pattern_size, &corners[0], &corner_count); fprintf(stderr, "corner:%02d...\n", corner_count); if (found) { fprintf(stderr, "ok\n"); } else { fprintf(stderr, "fail\n"); } // (4)�����ʡ����֤֥ԥ��������٤˽��������� IplImage *src_gray = cvCreateImage (cvGetSize (src_img), IPL_DEPTH_8U, 1); cvCvtColor (src_img, src_gray, CV_BGR2GRAY); cvFindCornerSubPix (src_gray, &corners[0], corner_count, cvSize (3, 3), cvSize (-1, -1), cvTermCriteria (CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03)); cvDrawChessboardCorners (src_img, pattern_size, &corners[0], corner_count, found); // cvShowImage ("Calibration", src_img); // cvWaitKey (0); // cvDestroyWindow("Calibration"); cvShowImage ("Calibration", src_img); cvInitMatHeader(&image_points, PAT_SIZE, 1, CV_32FC2, corners); // (2)�ѥ����ե�������ɤ߹��� fs = cvOpenFileStorage ("xml/rgb.xml", 0, CV_STORAGE_READ); param = cvGetFileNodeByName (fs, NULL, "intrinsic"); intrinsic = (CvMat *) cvRead (fs, param); param = cvGetFileNodeByName (fs, NULL, "distortion"); distortion = (CvMat *) cvRead (fs, param); cvReleaseFileStorage (&fs); // (3) �����ѥ����ο��� CvMat sub_image_points, sub_object_points; int base = 0; cvGetRows(&image_points, &sub_image_points, base * PAT_SIZE, (base + 1) * PAT_SIZE); cvGetRows(&object_points, &sub_object_points, base * PAT_SIZE, (base + 1)* PAT_SIZE); cvFindExtrinsicCameraParams2(&sub_object_points, &sub_image_points, intrinsic, distortion, rotation, translation); int ret = cvRodrigues2(rotation, rotationConv); // int cols = sub_object_points.rows; // printf("cols = %d\n", cols); // printf("%f\n",sub_object_points.data.fl[0]); // mm -> m for (i = 0; i < translation->cols; i++) { translation->data.fl[i] = translation->data.fl[i] / 1000;} // (4)XML�ե�����ؤνФ� //fs = cvOpenFileStorage(argv[2], 0, CV_STORAGE_WRITE); fs = cvOpenFileStorage(NAME_XML_OUT, 0, CV_STORAGE_WRITE); cvWrite(fs, "rotation", rotationConv); cvWrite(fs, "translation", translation); cvReleaseFileStorage(&fs); ///////////////////////////////////////////////// // write out py if(1) { cv::Mat ttt(translation); cv::Mat rrr(rotationConv); char data2Write[1024]; char textFileName[256]; sprintf( textFileName , "cbCoord/cbOneShot.py"); std::ofstream outPy(textFileName); outPy << "import sys" <<std::endl; outPy << "sys.path.append('../')" <<std::endl; outPy << "from numpy import *" <<std::endl; outPy << "from numpy.linalg import svd" <<std::endl; outPy << "from numpy.linalg import inv" <<std::endl; outPy << "from chessboard_points import *"<<std::endl; outPy << "sys.path.append('../geo')" <<std::endl; outPy << "from geo import *" <<std::endl; /* /////////////////////////////////////////////////////////////////////////////////// // out translation and rotation as xyzabc list outPy << "xyzabc = []" <<std::endl; sprintf( data2Write, "xyzabc.append(%f)", ttt.at<float>(0) ); outPy << data2Write << std::endl; std::cout << data2Write << std::endl; sprintf( data2Write, "xyzabc.append(%f)", ttt.at<float>(1) ); outPy << data2Write << std::endl; std::cout << data2Write << std::endl; sprintf( data2Write, "xyzabc.append(%f)", ttt.at<float>(2) ); outPy << data2Write << std::endl; std::cout << data2Write << std::endl; sprintf( data2Write, "xyzabc.append(%f)", rrr.at<float>(0) ); outPy << data2Write << std::endl; std::cout << data2Write << std::endl; sprintf( data2Write, "xyzabc.append(%f)", rrr.at<float>(1) ); outPy << data2Write << std::endl; std::cout << data2Write << std::endl; sprintf( data2Write, "xyzabc.append(%f)", rrr.at<float>(2) ); outPy << data2Write << std::endl; std::cout << data2Write << std::endl; // out translation and rotation as xyzabc list /////////////////////////////////////////////////////////////////////////////////// */ /////////////////////////////////////////////////////////////////////////////////// // out translation outPy << "ttt = []" <<std::endl; sprintf( data2Write, "ttt.append(%f)", ttt.at<float>(0) ); outPy << data2Write << std::endl; std::cout << data2Write << std::endl; sprintf( data2Write, "ttt.append(%f)", ttt.at<float>(1) ); outPy << data2Write << std::endl; std::cout << data2Write << std::endl; sprintf( data2Write, "ttt.append(%f)", ttt.at<float>(2) ); outPy << data2Write << std::endl; std::cout << data2Write << std::endl; // out translation ////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////// // out rotation outPy << "rrr = []" <<std::endl; sprintf( data2Write, "rrr.append([%f,%f,%f])", rrr.at<float>(0), rrr.at<float>(1), rrr.at<float>(2) ); outPy << data2Write << std::endl; std::cout << data2Write << std::endl; sprintf( data2Write, "rrr.append([%f,%f,%f])", rrr.at<float>(3), rrr.at<float>(4), rrr.at<float>(5) ); outPy << data2Write << std::endl; std::cout << data2Write << std::endl; sprintf( data2Write, "rrr.append([%f,%f,%f])", rrr.at<float>(6), rrr.at<float>(7), rrr.at<float>(8) ); outPy << data2Write << std::endl; std::cout << data2Write << std::endl; // out rotation ////////////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////// outPy<< "_T = FRAME( vec=ttt, mat=rrr )" << std::endl; ///////////////////////////////////////////////////////////////// } // write out py ///////////////////////////////////////////////// std::cout<< "press any key..."<< std::endl; cvWaitKey (0); cvDestroyWindow("Calibration"); cvReleaseImage(&src_img); cvReleaseMat(&intrinsic); cvReleaseMat(&distortion); return 0; }
void* captureThread(void* args) { CvCapture* capture = NULL; IplImage* frame; IplImage* frame_copy; IplImage* gray; ChessBoard chess; int pointsPerScene; CvPoint2D32f* points; int pointsNum[1]; CvMat* cornerPoints; CvMat* objectPoints; CvMat pointsNumMat; CvMat* intrinsic = cvCreateMat( 3, 3, CV_64F ); CvMat* distortion = cvCreateMat( 4, 1, CV_64F ); CvMat* rotation = cvCreateMat( 3, 3, CV_64F ); CvMat* translation = cvCreateMat( 3, 1, CV_64F ); loadIntrinsicParams("intrinsic_param_ref.txt", intrinsic, distortion ); capture = cvCreateCameraCapture(0); if(capture == NULL){ fprintf(stderr, "ERROR: Could not open Camera Device\n"); exit(1); } frame = cvQueryFrame(capture); if(frame == NULL){ fprintf(stderr, "ERROR: Could not query frame\n"); exit(1); } frame_copy = cvCreateImage(cvGetSize(frame), frame->depth, 3); gray = cvCreateImage(cvGetSize(frame_copy), frame_copy->depth, 1); cvNamedWindow(captureWinName, CV_WINDOW_AUTOSIZE); cvMoveWindow(captureWinName, graphicsWinWidth + 10, 0); chess.dx = CHESS_ROW_DX; chess.dy = CHESS_COL_DY; chess.patternSize.width = CHESS_ROW_NUM; chess.patternSize.height = CHESS_COL_NUM; pointsPerScene = chess.patternSize.width * chess.patternSize.height; cornerPoints = cvCreateMat(pointsPerScene, 2, CV_32F); objectPoints = cvCreateMat(pointsPerScene, 3, CV_32F); pointsNum[0] = pointsPerScene; pointsNumMat = cvMat(1, 1, CV_32S, pointsNum); points = (CvPoint2D32f*)malloc( sizeof(CvPoint2D32f) * pointsPerScene ); while(1){ int allPointsFound = 0; int detectedPointsNum; frame = cvQueryFrame( capture ); if( !frame ) { fprintf(stderr, "could not query frame\n"); exit(1); } cvResize(frame, frame_copy, CV_INTER_NN); cvCvtColor(frame_copy, gray, CV_BGR2GRAY); if( cvFindChessboardCorners( gray, chess.patternSize, points, &detectedPointsNum, CV_CALIB_CB_ADAPTIVE_THRESH ) ){ cvFindCornerSubPix(gray, points, detectedPointsNum, cvSize(5, 5), cvSize(-1, -1), cvTermCriteria(CV_TERMCRIT_ITER, 100, 0.1)); allPointsFound = 1; } else { allPointsFound = 0; } cvDrawChessboardCorners( frame_copy, chess.patternSize, points, detectedPointsNum, allPointsFound ); if( allPointsFound ){ double cameraPosition[3]; double cameraOriVec[4]; store2DCoordinates( cornerPoints, points, chess, 0 ); store3DCoordinates( objectPoints, chess, 0 ); calibrate( cornerPoints, objectPoints, intrinsic, distortion, rotation, translation ); getCameraPosition(rotation, translation, cameraPosition); printf("cam pos relative to chess board: %.1f, %.1f, %.1f\n", cameraPosition[0], cameraPosition[1], cameraPosition[2]); convertCv2Gl(cameraPosition, transGL); getCameraOriVec(rotation, rotGL); } cvShowImage( captureWinName, frame_copy); if(cvWaitKey(10) == KEY_ESC){ exit(1); } } free(points); cvReleaseMat(&intrinsic); cvReleaseMat(&distortion); cvReleaseMat(&rotation); cvReleaseMat(&translation); cvReleaseMat(&cornerPoints); cvReleaseMat(&objectPoints); cvDestroyWindow(captureWinName); cvReleaseImage(&frame_copy); cvReleaseImage(&gray); cvReleaseCapture(&capture); }
int main(int argc, char* argv[]) { if(argc != 6){ printf("too few args\n"); return -1; } // INPUT PARAMETERS: // int board_w = atoi(argv[1]); int board_h = atoi(argv[2]); int board_n = board_w * board_h; CvSize board_sz = cvSize( board_w, board_h ); CvMat* intrinsic = (CvMat*)cvLoad(argv[3]); CvMat* distortion = (CvMat*)cvLoad(argv[4]); IplImage* image = 0; IplImage* gray_image = 0; if( (image = cvLoadImage(argv[5])) == 0 ) { printf("Error: Couldn't load %s\n",argv[5]); return -1; } CvMat* image_points = cvCreateMat(1*board_n,2,CV_32FC1); CvMat* object_points = cvCreateMat(1*board_n,3,CV_32FC1); CvMat* objdrawpoints = cvCreateMat(1,1,CV_32FC3); CvMat* imgdrawpoints = cvCreateMat(1,1,CV_32FC2); float x=0; float y=0; float z=0; double grid_width=2.85; gray_image = cvCreateImage( cvGetSize(image), 8, 1 ); cvCvtColor(image, gray_image, CV_BGR2GRAY ); CvPoint2D32f* corners = new CvPoint2D32f[ board_n ]; int corner_count = 0; int found = cvFindChessboardCorners( gray_image, board_sz, corners, &corner_count, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS ); if(!found){ printf("Couldn't aquire chessboard on %s, " "only found %d of %d corners\n", argv[5],corner_count,board_n ); return -1; } //Get Subpixel accuracy on those corners: cvFindCornerSubPix( gray_image, corners, corner_count, cvSize(11,11), cvSize(-1,-1), cvTermCriteria( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 30, 0.1 ) ); // If we got a good board, add it to our data for( int i=0, j=0; j<board_n; ++i,++j ) { 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) =grid_width*( j/board_w); // cout<<j/board_w<<" "<<j%board_w<<endl; CV_MAT_ELEM(*object_points,float,i,1) = grid_width*(j%board_w); CV_MAT_ELEM(*object_points,float,i,2) = 0.0f; } // DRAW THE FOUND CHESSBOARD // cvDrawChessboardCorners( image, board_sz, corners, corner_count, found ); // FIND THE HOMOGRAPHY // CvMat *trans = cvCreateMat( 1, 3, CV_32F); CvMat *rot = cvCreateMat( 1, 3, CV_32F); // LET THE USER ADJUST THE Z HEIGHT OF THE VIEW // cvFindExtrinsicCameraParams2(object_points,image_points,intrinsic,distortion,rot,trans); // cvSave("trans.xml",trans); // cvSave("rot.xml",rot); int key = 0; IplImage *drawn_image = cvCloneImage(image); cvNamedWindow("translation"); // LOOP TO ALLOW USER TO PLAY WITH HEIGHT: // // escape key stops // // cvSetZero(trans); // cvSetZero(rot); while(key != 27) { cvCopy(image,drawn_image); if(key==97)x--; else if(key==113)x++; else if(key==115)y--; else if(key==119)y++; else if(key==100)z--; else if(key==101)z++; ((float*)(objdrawpoints->data.ptr))[0]=x; ((float*)(objdrawpoints->data.ptr))[1]=y; ((float*)(objdrawpoints->data.ptr))[2]=z; printf("%f %f %f\n",x,y,z); cvProjectPoints2(objdrawpoints,rot,trans,intrinsic,distortion,imgdrawpoints); cvCircle(drawn_image,cvPoint(((float*)(imgdrawpoints->data.ptr))[0],((float*)(imgdrawpoints->data.ptr))[1]),5,cvScalar(255,0,0),-1); printf("%f %f\n",((float*)(imgdrawpoints->data.ptr))[0],((float*)(imgdrawpoints->data.ptr))[1]); cvShowImage( "translation", drawn_image ); key = cvWaitKey(3); } cvDestroyWindow( "translation" ); //must add a lot of memory releasing here return 0; }
static void mainLoop( void ) { AR2VideoBufferT *videoBuffL; AR2VideoBufferT *videoBuffR; ARUint8 *dataPtrL; ARUint8 *dataPtrR; int cornerFlagL; int cornerFlagR; int cornerCountL; int cornerCountR; char buf[256]; int i; if ((videoBuffL = ar2VideoGetImage(vidL))) { gVideoBuffL = videoBuffL; } if ((videoBuffR = ar2VideoGetImage(vidR))) { gVideoBuffR = videoBuffR; } if (gVideoBuffL && gVideoBuffR) { // Warn about significant time differences. i = ((int)gVideoBuffR->time_sec - (int)gVideoBuffL->time_sec) * 1000 + ((int)gVideoBuffR->time_usec - (int)gVideoBuffL->time_usec) / 1000; if( i > 20 ) { ARLOG("Time diff = %d[msec]\n", i); } else if( i < -20 ) { ARLOG("Time diff = %d[msec]\n", i); } dataPtrL = gVideoBuffL->buff; dataPtrR = gVideoBuffR->buff; glClear(GL_COLOR_BUFFER_BIT); argDrawMode2D( vpL ); argDrawImage( dataPtrL ); argDrawMode2D( vpR ); argDrawImage( dataPtrR ); copyImage( dataPtrL, (ARUint8 *)calibImageL->imageData, xsizeL*ysizeL, pixFormatL ); cornerFlagL = cvFindChessboardCorners(calibImageL, cvSize(chessboardCornerNumY,chessboardCornerNumX), cornersL, &cornerCountL, CV_CALIB_CB_ADAPTIVE_THRESH|CV_CALIB_CB_FILTER_QUADS ); copyImage( dataPtrR, (ARUint8 *)calibImageR->imageData, xsizeR*ysizeR, pixFormatR ); cornerFlagR = cvFindChessboardCorners(calibImageR, cvSize(chessboardCornerNumY,chessboardCornerNumX), cornersR, &cornerCountR, CV_CALIB_CB_ADAPTIVE_THRESH|CV_CALIB_CB_FILTER_QUADS ); argDrawMode2D( vpL ); if(cornerFlagL) glColor3f(1.0f, 0.0f, 0.0f); else glColor3f(0.0f, 1.0f, 0.0f); glLineWidth(2.0f); //ARLOG("Detected corners = %d\n", cornerCount); for( i = 0; i < cornerCountL; i++ ) { argDrawLineByObservedPos(cornersL[i].x-5, cornersL[i].y-5, cornersL[i].x+5, cornersL[i].y+5); argDrawLineByObservedPos(cornersL[i].x-5, cornersL[i].y+5, cornersL[i].x+5, cornersL[i].y-5); //ARLOG(" %f, %f\n", cornersL[i].x, cornersL[i].y); sprintf(buf, "%d\n", i); argDrawStringsByObservedPos(buf, cornersL[i].x, cornersL[i].y+20); } argDrawMode2D( vpR ); if(cornerFlagR) glColor3f(1.0f, 0.0f, 0.0f); else glColor3f(0.0f, 1.0f, 0.0f); glLineWidth(2.0f); //ARLOG("Detected corners = %d\n", cornerCount); for( i = 0; i < cornerCountR; i++ ) { argDrawLineByObservedPos(cornersR[i].x-5, cornersR[i].y-5, cornersR[i].x+5, cornersR[i].y+5); argDrawLineByObservedPos(cornersR[i].x-5, cornersR[i].y+5, cornersR[i].x+5, cornersR[i].y-5); //ARLOG(" %f, %f\n", cornersR[i].x, cornersR[i].y); sprintf(buf, "%d\n", i); argDrawStringsByObservedPos(buf, cornersR[i].x, cornersR[i].y+20); } if( cornerFlagL && cornerFlagR ) { cornerFlag = 1; glColor3f(1.0f, 0.0f, 0.0f); } else { cornerFlag = 0; glColor3f(0.0f, 1.0f, 0.0f); } argDrawMode2D( vpL ); sprintf(buf, "Captured Image: %2d/%2d\n", capturedImageNum, calibImageNum); argDrawStringsByIdealPos(buf, 10, 30); argSwapBuffers(); gVideoBuffL = gVideoBuffR = NULL; } else arUtilSleep(2); }
int main( int argc, char** argv ) { CvSize board_size = {0,0}; float square_size = 1.f, aspect_ratio = 1.f; const char* out_filename = "out_camera_data.yml"; const char* input_filename = 0; int i, image_count = 10; int write_extrinsics = 0, write_points = 0; int flags = 0; CvCapture* capture = 0; FILE* f = 0; char imagename[1024]; CvMemStorage* storage; CvSeq* image_points_seq = 0; int elem_size, flip_vertical = 0; int delay = 1000; clock_t prev_timestamp = 0; CvPoint2D32f* image_points_buf = 0; CvFont font = cvFont( 1, 1 ); double _camera[9], _dist_coeffs[4]; CvMat camera = cvMat( 3, 3, CV_64F, _camera ); CvMat dist_coeffs = cvMat( 1, 4, CV_64F, _dist_coeffs ); CvMat *extr_params = 0, *reproj_errs = 0; double avg_reproj_err = 0; int mode = DETECTION; int undistort_image = 0; CvSize img_size = {0,0}; const char* live_capture_help = "When the live video from camera is used as input, the following hot-keys may be used:\n" " <ESC>, 'q' - quit the program\n" " 'g' - start capturing images\n" " 'u' - switch undistortion on/off\n"; if( argc < 2 ) { // calibration -w 6 -h 8 -s 2 -n 10 -o camera.yml -op -oe [<list_of_views.txt>] printf( "This is a camera calibration sample.\n" "Usage: calibration\n" " -w <board_width> # the number of inner corners per one of board dimension\n" " -h <board_height> # the number of inner corners per another board dimension\n" " [-n <number_of_frames>] # the number of frames to use for calibration\n" " # (if not specified, it will be set to the number\n" " # of board views actually available)\n" " [-di <disk_images> # Number of disk images before triggering undistortion\n" " [-d <delay>] # a minimum delay in ms between subsequent attempts to capture a next view\n" " # (used only for video capturing)\n" " [-s <square_size>] # square size in some user-defined units (1 by default)\n" " [-o <out_camera_params>] # the output filename for intrinsic [and extrinsic] parameters\n" " [-op] # write detected feature points\n" " [-oe] # write extrinsic parameters\n" " [-zt] # assume zero tangential distortion\n" " [-a <aspect_ratio>] # fix aspect ratio (fx/fy)\n" " [-p] # fix the principal point at the center\n" " [-v] # flip the captured images around the horizontal axis\n" " [input_data] # input data, one of the following:\n" " # - text file with a list of the images of the board\n" " # - name of video file with a video of the board\n" " # if input_data not specified, a live view from the camera is used\n" "\n" ); printf( "%s", live_capture_help ); return 0; } for( i = 1; i < argc; i++ ) { const char* s = argv[i]; if( strcmp( s, "-w" ) == 0 ) { if( sscanf( argv[++i], "%u", &board_size.width ) != 1 || board_size.width <= 0 ) return fprintf( stderr, "Invalid board width\n" ), -1; } else if( strcmp( s, "-h" ) == 0 ) { if( sscanf( argv[++i], "%u", &board_size.height ) != 1 || board_size.height <= 0 ) return fprintf( stderr, "Invalid board height\n" ), -1; } else if( strcmp( s, "-s" ) == 0 ) { if( sscanf( argv[++i], "%f", &square_size ) != 1 || square_size <= 0 ) return fprintf( stderr, "Invalid board square width\n" ), -1; } else if( strcmp( s, "-n" ) == 0 ) { if( sscanf( argv[++i], "%u", &image_count ) != 1 || image_count <= 3 ) return printf("Invalid number of images\n" ), -1; } else if( strcmp( s, "-di") == 0) { if( sscanf( argv[++i], "%d", &images_from_file) != 1 || images_from_file < 3) return printf("Invalid di, must be >= 3\n"), -1; } else if( strcmp( s, "-a" ) == 0 ) { if( sscanf( argv[++i], "%f", &aspect_ratio ) != 1 || aspect_ratio <= 0 ) return printf("Invalid aspect ratio\n" ), -1; } else if( strcmp( s, "-d" ) == 0 ) { if( sscanf( argv[++i], "%u", &delay ) != 1 || delay <= 0 ) return printf("Invalid delay\n" ), -1; } else if( strcmp( s, "-op" ) == 0 ) { write_points = 1; } else if( strcmp( s, "-oe" ) == 0 ) { write_extrinsics = 1; } else if( strcmp( s, "-zt" ) == 0 ) { flags |= CV_CALIB_ZERO_TANGENT_DIST; } else if( strcmp( s, "-p" ) == 0 ) { flags |= CV_CALIB_FIX_PRINCIPAL_POINT; } else if( strcmp( s, "-v" ) == 0 ) { flip_vertical = 1; } else if( strcmp( s, "-o" ) == 0 ) { out_filename = argv[++i]; } else if( s[0] != '-' ) input_filename = s; else return fprintf( stderr, "Unknown option %s", s ), -1; } if( input_filename ) { capture = cvCreateFileCapture( input_filename ); if( !capture ) { f = fopen( input_filename, "rt" ); if( !f ) return fprintf( stderr, "The input file could not be opened\n" ), -1; image_count = -1; } mode = CAPTURING; } else capture = cvCreateCameraCapture(0); if( !capture && !f ) return fprintf( stderr, "Could not initialize video capture\n" ), -2; if( capture ) printf( "%s", live_capture_help ); elem_size = board_size.width*board_size.height*sizeof(image_points_buf[0]); storage = cvCreateMemStorage( MAX( elem_size*4, 1 << 16 )); image_points_buf = (CvPoint2D32f*)cvAlloc( elem_size ); image_points_seq = cvCreateSeq( 0, sizeof(CvSeq), elem_size, storage ); cvNamedWindow( "Image View", 1 ); cvNamedWindow( "Undistort",1); int disk_image_cnt = 0; for(;;) { IplImage *view = 0, *view_gray = 0; int count = 0, found, blink = 0; CvPoint text_origin; CvSize text_size = {0,0}; int base_line = 0; char s[100]; int key; if( f && fgets( imagename, sizeof(imagename)-2, f )) { int l = strlen(imagename); if( l > 0 && imagename[l-1] == '\n' ) imagename[--l] = '\0'; if( l > 0 ) { if( imagename[0] == '#' ) continue; view = cvLoadImage( imagename, 1 ); disk_image_cnt++; } } else if( capture ) { IplImage* view0 = cvQueryFrame( capture ); if( view0 ) { view = cvCreateImage( cvGetSize(view0), IPL_DEPTH_8U, view0->nChannels ); if( view0->origin == IPL_ORIGIN_BL ) cvFlip( view0, view, 0 ); else cvCopy( view0, view ); } } if( !view || (disk_image_cnt == images_from_file)) { if( image_points_seq->total > 0 ) { image_count = image_points_seq->total; goto calibrate; } break; } if( flip_vertical ) cvFlip( view, view, 0 ); img_size = cvGetSize(view); found = cvFindChessboardCorners( view, board_size, image_points_buf, &count, CV_CALIB_CB_ADAPTIVE_THRESH ); #if 1 // improve the found corners' coordinate accuracy view_gray = cvCreateImage( cvGetSize(view), 8, 1 ); cvCvtColor( view, view_gray, CV_BGR2GRAY ); cvFindCornerSubPix( view_gray, image_points_buf, count, cvSize(11,11), cvSize(-1,-1), cvTermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 )); cvReleaseImage( &view_gray ); #endif if( mode == CAPTURING && found && (f || clock() - prev_timestamp > delay*1e-3*CLOCKS_PER_SEC) ) { cvSeqPush( image_points_seq, image_points_buf ); prev_timestamp = clock(); blink = !f; #if 1 if( capture ) { sprintf( imagename, "view%03d.png", image_points_seq->total - 1 ); cvSaveImage( imagename, view ); } #endif } cvDrawChessboardCorners( view, board_size, image_points_buf, count, found ); cvGetTextSize( "100/100", &font, &text_size, &base_line ); text_origin.x = view->width - text_size.width - 10; text_origin.y = view->height - base_line - 10; if( mode == CAPTURING ) { if( image_count > 0 ) sprintf( s, "%d/%d", image_points_seq ? image_points_seq->total : 0, image_count ); else sprintf( s, "%d/?", image_points_seq ? image_points_seq->total : 0 ); } else if( mode == CALIBRATED ) sprintf( s, "Calibrated" ); else sprintf( s, "Press 'g' to start" ); cvPutText( view, s, text_origin, &font, mode != CALIBRATED ? CV_RGB(255,0,0) : CV_RGB(0,255,0)); if( blink ) cvNot( view, view ); //Rectify or Undistort the image if( mode == CALIBRATED && undistort_image ) { IplImage* t = cvCloneImage( view ); cvShowImage("Image View", view); cvUndistort2( t, view, &camera, &dist_coeffs ); cvReleaseImage( &t ); cvShowImage( "Undistort", view ); cvWaitKey(0); } else{ cvShowImage( "Image View", view ); key = cvWaitKey(capture ? 50 : 500); } if( key == 27 ) break; if( key == 'u' && mode == CALIBRATED ){ undistort_image = !undistort_image; } if( capture && key == 'g' ) { mode = CAPTURING; cvClearMemStorage( storage ); image_points_seq = cvCreateSeq( 0, sizeof(CvSeq), elem_size, storage ); } if( mode == CAPTURING && (unsigned)image_points_seq->total >= (unsigned)image_count ) { calibrate: if(disk_image_cnt == images_from_file) undistort_image = !undistort_image; cvReleaseMat( &extr_params ); cvReleaseMat( &reproj_errs ); int code = run_calibration( image_points_seq, img_size, board_size, square_size, aspect_ratio, flags, &camera, &dist_coeffs, &extr_params, &reproj_errs, &avg_reproj_err ); // save camera parameters in any case, to catch Inf's/NaN's save_camera_params( out_filename, image_count, img_size, board_size, square_size, aspect_ratio, flags, &camera, &dist_coeffs, write_extrinsics ? extr_params : 0, write_points ? image_points_seq : 0, reproj_errs, avg_reproj_err ); if( code ) mode = CALIBRATED; else mode = DETECTION; } if( !view ) break; cvReleaseImage( &view ); } if( capture ) cvReleaseCapture( &capture ); return 0; }
void bird_eye() { int board_n = board_w * board_h; CvSize board_sz = cvSize(board_w, board_h); CvMat *intrinsic = (CvMat*) cvLoad("Intrinsics.xml"); CvMat *distortion = (CvMat*) cvLoad("Distortion.xml"); IplImage* image = cvLoadImage("./Resource/bird-eye.jpg", 1); IplImage* gray_image = cvCreateImage(cvGetSize(image), 8, 1); cvCvtColor(image, gray_image, CV_BGR2GRAY); IplImage* mapx = cvCreateImage(cvGetSize(image), IPL_DEPTH_32F, 1); IplImage* mapy = cvCreateImage(cvGetSize(image), IPL_DEPTH_32F, 1); cvInitUndistortMap( intrinsic, distortion, mapx, mapy ); IplImage* t = cvCloneImage(image); cvRemap(t, image, mapx, mapy); cvNamedWindow("Chessboard"); cvShowImage("Chessboard", image); int c = cvWaitKey(-1); CvPoint2D32f* corners = new CvPoint2D32f[board_n]; int corner_count = 0; int found = cvFindChessboardCorners( image, board_sz, corners, &corner_count, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS ); if(!found){ printf("couldn't aquire chessboard!\n"); return; } cvFindCornerSubPix( gray_image, corners, corner_count, cvSize(11, 11), cvSize(-1, -1), cvTermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 30, 0.1) ); CvPoint2D32f objPts[4], imgPts[4]; objPts[0].x = 0; objPts[0].y = 0; objPts[1].x = board_w - 1; objPts[1].y = 0; objPts[2].x = 0; objPts[2].y = board_h - 1; objPts[3].x = board_w - 1; objPts[3].y = board_h - 1; imgPts[0] = corners[0]; imgPts[1] = corners[board_w - 1]; imgPts[2] = corners[(board_h - 1) * board_w]; imgPts[3] = corners[(board_h - 1) * board_w + board_w - 1]; cvCircle(image, cvPointFrom32f(imgPts[0]), 9, CV_RGB(0, 0, 255), 3); cvCircle(image, cvPointFrom32f(imgPts[1]), 9, CV_RGB(0, 255, 0), 3); cvCircle(image, cvPointFrom32f(imgPts[2]), 9, CV_RGB(255, 0, 0), 3); cvCircle(image, cvPointFrom32f(imgPts[3]), 9, CV_RGB(255, 255, 0), 3); cvDrawChessboardCorners( image, board_sz, corners, corner_count, found ); cvShowImage("Chessboard", image); CvMat *H = cvCreateMat(3, 3, CV_32F); cvGetPerspectiveTransform(objPts, imgPts, H); float z = 25; int key = 0; IplImage * birds_image = cvCloneImage(image); cvNamedWindow("Birds_Eye"); while(key != 27) { CV_MAT_ELEM(*H, float, 2, 2) = z; cvWarpPerspective( image, birds_image, H, CV_INTER_LINEAR| CV_WARP_INVERSE_MAP | CV_WARP_FILL_OUTLIERS ); cvShowImage("Birds_Eye", birds_image); key = cvWaitKey(); if(key == 'u') z += 0.5; if(key == 'd') z -= 0.5; } cvSave("H.xml", H); }
int main (int argc, char **argv) { if(argc < 8) { printf("usage: Compute <skip> <out_dir> <num_cameras> <corner_rows> <corner_cols> <int_dir1> <rgb_images1> <ir_images1> <int_dir2> ...\n"); return 1; } int skip = atoi(argv[1]); char * outDir = argv[2]; int numCameras = atoi(argv[3]); int corner_rows = atoi(argv[4]); int corner_cols = atoi(argv[5]); int argBase = 6; if(argc != argBase+3*numCameras) { printf("Expected %d intrinsics entries for %d cameras.\n", numCameras, numCameras); return -1; } char temp[1024]; int i, j, k, type; CvMat* cam_mat = cvCreateMat( 3, 3, CV_32FC1 ); CvMat* dist_coeff = cvCreateMat( 5, 1, CV_32FC1 ); CvPoint2D32f* corners = (CvPoint2D32f*) malloc(sizeof(CvPoint2D32f)*corner_rows*corner_cols); for(type = 0; type <= 1; type++) { for(i = 0; i < numCameras; i++) { int numSkipped = 0; int numImages = atoi(argv[argBase+i*3+(type==0?1:2)]); char * intDir = argv[argBase+i*3]; CvMat* image_points = cvCreateMat(numImages*corner_rows*corner_cols, 2, CV_32FC1 ); CvMat* object_points = cvCreateMat(numImages*corner_rows*corner_cols, 3, CV_32FC1 ); CvMat* point_counts = cvCreateMat(numImages, 1, CV_32SC1 ); for(j = 0; j < numImages; j++) { //load image sprintf(temp, "%s/img_%s_%d.png", intDir, type==0?"rgb":"ir", j*skip); printf("loading %s...\n",temp); IplImage *img = cvLoadImage(temp, CV_LOAD_IMAGE_GRAYSCALE); if(img != NULL) { int corner_count = 0; if(cvFindChessboardCorners(img, cvSize(corner_cols, corner_rows), corners, &corner_count, CV_CALIB_CB_ADAPTIVE_THRESH|CV_CALIB_CB_NORMALIZE_IMAGE)) { cvFindCornerSubPix(img, corners, corner_count, cvSize(11, 11), cvSize(-1, -1), cvTermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, REFINE_MAX_ITER, REFINE_EPSILON)); int offset = (j-numSkipped)*corner_rows*corner_cols; for(k = 0; k < corner_rows*corner_cols; k++) { CV_MAT_ELEM(*image_points, float, offset+k, 0 ) = corners[k].x; CV_MAT_ELEM(*image_points, float, offset+k, 1 ) = corners[k].y; CV_MAT_ELEM(*object_points, float, offset+k, 0 ) = SQUARE_SIZE*(k%corner_cols); CV_MAT_ELEM(*object_points, float, offset+k, 1 ) = SQUARE_SIZE*(k/corner_cols); CV_MAT_ELEM(*object_points, float, offset+k, 2 ) = 0.0f; } CV_MAT_ELEM(*point_counts, int, j-numSkipped, 0 ) = corner_rows*corner_cols; } else { printf("Cannot find corners in image %s, skipping\n", temp); numSkipped++; //return -1; } cvReleaseImage(&img); } else {
void ProjectionModel::calculateProjection() { if(intrinsic_matrix != 0 && distortion_coeffs != 0) { int corner_count = Chessboard::board_total; cvCvtColor(sourceImg, gray_image, CV_RGB2GRAY); int found = cvFindChessboardCorners(gray_image, board_sz, corners, &corner_count, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS); if(!found) { return; } cvFindCornerSubPix(gray_image, corners, corner_count, cvSize(11,11), cvSize(-1,-1), cvTermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1)); objPts[0].x = 0; objPts[0].y = 0; objPts[1].x = Chessboard::board_w - 1; objPts[1].y = 0; objPts[2].x = 0; objPts[2].y = Chessboard::board_h - 1; objPts[3].x = Chessboard::board_w - 1; objPts[3].y = Chessboard::board_h - 1; imgPts[3] = corners[0]; imgPts[2] = corners[Chessboard::board_w - 1]; imgPts[1] = corners[(Chessboard::board_h - 1) * Chessboard::board_w]; imgPts[0] = corners[(Chessboard::board_h - 1) * Chessboard::board_w + Chessboard::board_w - 1]; cvGetPerspectiveTransform(objPts, imgPts, H); for(int i = 0; i < 4; ++i) { CV_MAT_ELEM(*image_points, CvPoint2D32f, i, 0) = imgPts[i]; CV_MAT_ELEM(*object_points, CvPoint3D32f, i, 0) = cvPoint3D32f(objPts[i].x, objPts[i].y, 0); } cvFindExtrinsicCameraParams2(object_points, image_points, intrinsic_matrix, distortion_coeffs, rotation_vector, translation_vector); cvRodrigues2(rotation_vector, rotation_matrix); for(int f = 0; f < 3; f++) { for(int c = 0; c < 3; c++) { fullMatrix[c * 4 + f] = rotation_matrix->data.fl[f * 3 + c]; //transposed } } fullMatrix[3] = 0.0; fullMatrix[7] = 0.0; fullMatrix[11] = 0.0; fullMatrix[12] = translation_vector->data.fl[0]; fullMatrix[13] = translation_vector->data.fl[1]; fullMatrix[14] = translation_vector->data.fl[2]; fullMatrix[15] = 1.0; } }
void cameraCalibration() { board_w = 5; // Board width in squares board_h = 8; // Board height n_boards = 8; // Number of boards int board_n = board_w * board_h; CvSize board_sz = cvSize(board_w, board_h); CameraControl* cc = camera_control_new(0); //cvNamedWindow("Calibration", 0); // Allocate Sotrage 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(5, 1, CV_32FC1); IplImage *image; CvPoint2D32f corners[board_n]; int i = 0; int j = 0; for (i = 0; i < board_n; i++) corners[i] = cvPoint2D32f(0, 0); int corner_count; int successes = 0; int step = 0; while (1) { cvWaitKey(10); image = camera_control_query_frame(cc); if (image) break; } IplImage *gray_image = cvCreateImage(cvGetSize(image), 8, 1); // Capture Corner views loop until we've got n_boards // succesful captures (all corners on the board are found) while (successes < n_boards) { // Skp every board_dt frames to allow user to move chessboard // skip a second to allow user to move the chessboard image = camera_control_query_frame(cc); // Get next image //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); cvWaitKey(1); // 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); char text[222]; sprintf(text, "calibration image %d/%d", successes, n_boards); th_put_text(image, text, cvPoint(20, 20), th_white, 1.0); cvShowImage("Calibration", image); // If we got a good board, add it to our data if (corner_count == board_n ) { step = successes * board_n; for (i = step, j = 0; j < board_n; ++i, ++j) { 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++; } } }
/* this handles computation of the distortion matrices of the camera */ void calibration(int board_w, int board_h, int n_boards, float square_width, float square_height, CvSize resolution, int device_id) { int board_n = board_w * board_h; CvSize board_sz = cvSize( board_w, board_h ); VideoCapture capture(device_id,resolution.width,resolution.height); IplImage *image = capture.CreateCaptureImage(); 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(5,1,CV_32FC1); CvPoint2D32f* corners = new CvPoint2D32f[ board_n ]; int corner_count; int successes = 0; int step, frame = 0; IplImage *gray_image = cvCreateImage(cvGetSize(image),8,1);//subpixel // CAPTURE CORNER VIEWS LOOP UNTIL WE'VE GOT n_boards // SUCCESSFUL CAPTURES (ALL CORNERS ON THE BOARD ARE FOUND) // cvNamedWindow("Live View"); while(successes < n_boards) { char c=0; while(c!='c'){ capture.waitFrame(image); cvShowImage("Live View",image); c=cvWaitKey(1); } //============== //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 ) { step = successes*board_n; for( int i=step, j=0; j<board_n; ++i,++j ) { 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) = square_height*(j/board_w); CV_MAT_ELEM(*object_points,float,i,1) = square_width*(j%board_w); CV_MAT_ELEM(*object_points,float,i,2) = 0.0f; } CV_MAT_ELEM(*point_counts, int,successes,0) = board_n; successes++; } else {