Code:/* This is sample from the OpenCV book. The copyright notice is below */ /* *************** License:************************** Oct. 3, 2008 Right to use this code in any way you want without warrenty, support or any guarentee of it working. BOOK: It would be nice if you cited it: Learning OpenCV: Computer Vision with the OpenCV Library by Gary Bradski and Adrian Kaehler Published by O'Reilly Media, October 3, 2008 AVAILABLE AT: http://www.amazon.com/Learning-OpenC.../dp/0596516134 Or: http://oreilly.com/catalog/9780596516130/ ISBN-10: 0596516134 or: ISBN-13: 978-0596516130 OTHER OPENCV SITES: * The source code is on sourceforge at: http://sourceforge.net/projects/opencvlibrary/ * The OpenCV Wiki page (As of Oct 1, 2008 this is down for changing over servers, but should come back): http://opencvlibrary.sourceforge.net/ * An active user group is at: http://tech.groups.yahoo.com/group/OpenCV/ * The minutes of weekly OpenCV development meetings are at: http://pr.willowgarage.com/wiki/OpenCV ************************************************** */ #include "cv.h" #include "cxmisc.h" #include "highgui.h" #include <vector> #include <string> #include <algorithm> #include <stdio.h> #include <ctype.h> using namespace std; // // Given a list of chessboard images, the number of corners (nx, ny) // on the chessboards, and a flag: useCalibrated for calibrated (0) or // uncalibrated (1: use cvStereoCalibrate(), 2: compute fundamental // matrix separately) stereo. Calibrate the cameras and display the // rectified results along with the computed disparity images. // static void StereoCalib(const char* imageList, int useUncalibrated) { int nx = 0, ny = 0; int displayCorners = 0; int showUndistorted = 1; bool isVerticalStereo = false;//OpenCV can handle left-right //or up-down camera arrangements const int maxScale = 1; const float squareSize = 1.f; //Set this to your actual square size FILE* f = fopen(imageList, "rt"); int i, j, lr, nframes, n, N = 0; vector<string> imageNames[2]; vector<CvPoint3D32f> objectPoints; vector<CvPoint2D32f> points[2]; vector<int> npoints; vector<uchar> active[2]; vector<CvPoint2D32f> temp; CvSize imageSize = {0,0}; // ARRAY AND VECTOR STORAGE: double M1[3][3], M2[3][3], D1[5], D2[5]; double R[3][3], T[3], E[3][3], F[3][3]; CvMat _M1 = cvMat(3, 3, CV_64F, M1 ); CvMat _M2 = cvMat(3, 3, CV_64F, M2 ); CvMat _D1 = cvMat(1, 5, CV_64F, D1 ); CvMat _D2 = cvMat(1, 5, CV_64F, D2 ); CvMat _R = cvMat(3, 3, CV_64F, R ); CvMat _T = cvMat(3, 1, CV_64F, T ); CvMat _E = cvMat(3, 3, CV_64F, E ); CvMat _F = cvMat(3, 3, CV_64F, F ); char buf[1024]; if( displayCorners ) cvNamedWindow( "corners", 1 ); // READ IN THE LIST OF CHESSBOARDS: if( !f ) { fprintf(stderr, "can not open file %s\n", imageList ); return; } if( !fgets(buf, sizeof(buf)-3, f) || sscanf(buf, "%d%d", &nx, &ny) != 2 ) return; n = nx*ny; temp.resize(n); for(i=0;;i++) { int count = 0, result=0; lr = i % 2; vector<CvPoint2D32f>& pts = points[lr]; if( !fgets( buf, sizeof(buf)-3, f )) break; size_t len = strlen(buf); while( len > 0 && isspace(buf[len-1])) buf[--len] = '\0'; if( buf[0] == '#') continue; IplImage* img = cvLoadImage( buf, 0 ); if( !img ) break; imageSize = cvGetSize(img); imageNames[lr].push_back(buf); //FIND CHESSBOARDS AND CORNERS THEREIN: for( int s = 1; s <= maxScale; s++ ) { IplImage* timg = img; if( s > 1 ) { timg = cvCreateImage(cvSize(img->width*s,img->height*s), img->depth, img->nChannels ); cvResize( img, timg, CV_INTER_CUBIC ); } result = cvFindChessboardCorners( timg, cvSize(nx, ny), &temp[0], &count, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE); if( timg != img ) cvReleaseImage( &timg ); if( result || s == maxScale ) for( j = 0; j < count; j++ ) { temp[j].x /= s; temp[j].y /= s; } if( result ) break; } if( displayCorners ) { printf("%s\n", buf); IplImage* cimg = cvCreateImage( imageSize, 8, 3 ); cvCvtColor( img, cimg, CV_GRAY2BGR ); cvDrawChessboardCorners( cimg, cvSize(nx, ny), &temp[0], count, result ); cvShowImage( "corners", cimg ); cvReleaseImage( &cimg ); int c = cvWaitKey(1000); if( c == 27 || c == 'q' || c == 'Q' ) //Allow ESC to quit exit(-1); } else putchar('.'); N = pts.size(); pts.resize(N + n, cvPoint2D32f(0,0)); active[lr].push_back((uchar)result); //assert( result != 0 ); if( result ) { //Calibration will suffer without subpixel interpolation cvFindCornerSubPix( img, &temp[0], count, cvSize(11, 11), cvSize(-1,-1), cvTermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, 30, 0.01) ); copy( temp.begin(), temp.end(), pts.begin() + N ); } cvReleaseImage( &img ); } fclose(f); printf("\n"); // HARVEST CHESSBOARD 3D OBJECT POINT LIST: nframes = active[0].size();//Number of good chessboads found objectPoints.resize(nframes*n); for( i = 0; i < ny; i++ ) for( j = 0; j < nx; j++ ) objectPoints[i*nx + j] = cvPoint3D32f(i*squareSize, j*squareSize, 0); for( i = 1; i < nframes; i++ ) copy( objectPoints.begin(), objectPoints.begin() + n, objectPoints.begin() + i*n ); npoints.resize(nframes,n); N = nframes*n; CvMat _objectPoints = cvMat(1, N, CV_32FC3, &objectPoints[0] ); CvMat _imagePoints1 = cvMat(1, N, CV_32FC2, &points[0][0] ); CvMat _imagePoints2 = cvMat(1, N, CV_32FC2, &points[1][0] ); CvMat _npoints = cvMat(1, npoints.size(), CV_32S, &npoints[0] ); cvSetIdentity(&_M1); cvSetIdentity(&_M2); cvZero(&_D1); cvZero(&_D2); // CALIBRATE THE STEREO CAMERAS printf("Running stereo calibration ..."); fflush(stdout); cvStereoCalibrate( &_objectPoints, &_imagePoints1, &_imagePoints2, &_npoints, &_M1, &_D1, &_M2, &_D2, imageSize, &_R, &_T, &_E, &_F, cvTermCriteria(CV_TERMCRIT_ITER+ CV_TERMCRIT_EPS, 100, 1e-5), CV_CALIB_FIX_ASPECT_RATIO + CV_CALIB_ZERO_TANGENT_DIST + CV_CALIB_SAME_FOCAL_LENGTH ); printf(" done\n"); // CALIBRATION QUALITY CHECK // because the output fundamental matrix implicitly // includes all the output information, // we can check the quality of calibration using the // epipolar geometry constraint: m2^t*F*m1=0 vector<CvPoint3D32f> lines[2]; points[0].resize(N); points[1].resize(N); _imagePoints1 = cvMat(1, N, CV_32FC2, &points[0][0] ); _imagePoints2 = cvMat(1, N, CV_32FC2, &points[1][0] ); lines[0].resize(N); lines[1].resize(N); CvMat _L1 = cvMat(1, N, CV_32FC3, &lines[0][0]); CvMat _L2 = cvMat(1, N, CV_32FC3, &lines[1][0]); //Always work in undistorted space cvUndistortPoints( &_imagePoints1, &_imagePoints1, &_M1, &_D1, 0, &_M1 ); cvUndistortPoints( &_imagePoints2, &_imagePoints2, &_M2, &_D2, 0, &_M2 ); cvComputeCorrespondEpilines( &_imagePoints1, 1, &_F, &_L1 ); cvComputeCorrespondEpilines( &_imagePoints2, 2, &_F, &_L2 ); double avgErr = 0; for( i = 0; i < N; i++ ) { double err = fabs(points[0][i].x*lines[1][i].x + points[0][i].y*lines[1][i].y + lines[1][i].z) + fabs(points[1][i].x*lines[0][i].x + points[1][i].y*lines[0][i].y + lines[0][i].z); avgErr += err; } printf( "avg err = %g\n", avgErr/(nframes*n) ); //COMPUTE AND DISPLAY RECTIFICATION if( showUndistorted ) { CvMat* mx1 = cvCreateMat( imageSize.height, imageSize.width, CV_32F ); CvMat* my1 = cvCreateMat( imageSize.height, imageSize.width, CV_32F ); CvMat* mx2 = cvCreateMat( imageSize.height, imageSize.width, CV_32F ); CvMat* my2 = cvCreateMat( imageSize.height, imageSize.width, CV_32F ); CvMat* img1r = cvCreateMat( imageSize.height, imageSize.width, CV_8U ); CvMat* img2r = cvCreateMat( imageSize.height, imageSize.width, CV_8U ); CvMat* disp = cvCreateMat( imageSize.height, imageSize.width, CV_16S ); CvMat* vdisp = cvCreateMat( imageSize.height, imageSize.width, CV_8U ); CvMat* pair; double R1[3][3], R2[3][3], P1[3][4], P2[3][4]; CvMat _R1 = cvMat(3, 3, CV_64F, R1); CvMat _R2 = cvMat(3, 3, CV_64F, R2); // IF BY CALIBRATED (BOUGUET'S METHOD) if( useUncalibrated == 0 ) { CvMat _P1 = cvMat(3, 4, CV_64F, P1); CvMat _P2 = cvMat(3, 4, CV_64F, P2); cvStereoRectify( &_M1, &_M2, &_D1, &_D2, imageSize, &_R, &_T, &_R1, &_R2, &_P1, &_P2, 0, 0/*CV_CALIB_ZERO_DISPARITY*/ ); isVerticalStereo = fabs(P2[1][3]) > fabs(P2[0][3]); //Precompute maps for cvRemap() cvInitUndistortRectifyMap(&_M1,&_D1,&_R1,&_P1,mx1,my1); cvInitUndistortRectifyMap(&_M2,&_D2,&_R2,&_P2,mx2,my2); } //OR ELSE HARTLEY'S METHOD else if( useUncalibrated == 1 || useUncalibrated == 2 ) // use intrinsic parameters of each camera, but // compute the rectification transformation directly // from the fundamental matrix { double H1[3][3], H2[3][3], iM[3][3]; CvMat _H1 = cvMat(3, 3, CV_64F, H1); CvMat _H2 = cvMat(3, 3, CV_64F, H2); CvMat _iM = cvMat(3, 3, CV_64F, iM); //Just to show you could have independently used F if( useUncalibrated == 2 ) cvFindFundamentalMat( &_imagePoints1, &_imagePoints2, &_F); cvStereoRectifyUncalibrated( &_imagePoints1, &_imagePoints2, &_F, imageSize, &_H1, &_H2, 3); cvInvert(&_M1, &_iM); cvMatMul(&_H1, &_M1, &_R1); cvMatMul(&_iM, &_R1, &_R1); cvInvert(&_M2, &_iM); cvMatMul(&_H2, &_M2, &_R2); cvMatMul(&_iM, &_R2, &_R2); //Precompute map for cvRemap() cvInitUndistortRectifyMap(&_M1,&_D1,&_R1,&_M1,mx1,my1); cvInitUndistortRectifyMap(&_M2,&_D1,&_R2,&_M2,mx2,my2); } else assert(0); cvNamedWindow( "rectified", 1 ); // RECTIFY THE IMAGES AND FIND DISPARITY MAPS if( !isVerticalStereo ) pair = cvCreateMat( imageSize.height, imageSize.width*2, CV_8UC3 ); else pair = cvCreateMat( imageSize.height*2, imageSize.width, CV_8UC3 ); //Setup for finding stereo corrrespondences CvStereoBMState *BMState = cvCreateStereoBMState(); assert(BMState != 0); BMState->preFilterSize=33; BMState->preFilterCap=33; BMState->SADWindowSize=33; if( useUncalibrated ) { BMState->minDisparity=-64; BMState->numberOfDisparities=128; } else { BMState->minDisparity=-32; BMState->numberOfDisparities=192; } BMState->textureThreshold=10; BMState->uniquenessRatio=15; for( i = 0; i < nframes; i++ ) { IplImage* img1=cvLoadImage(imageNames[0][i].c_str(),0); IplImage* img2=cvLoadImage(imageNames[1][i].c_str(),0); if( img1 && img2 ) { CvMat part; cvRemap( img1, img1r, mx1, my1 ); cvRemap( img2, img2r, mx2, my2 ); if( !isVerticalStereo || useUncalibrated != 0 ) { // When the stereo camera is oriented vertically, // useUncalibrated==0 does not transpose the // image, so the epipolar lines in the rectified // images are vertical. Stereo correspondence // function does not support such a case. cvFindStereoCorrespondenceBM( img1r, img2r, disp, BMState); cvNormalize( disp, vdisp, 0, 256, CV_MINMAX ); cvNamedWindow( "disparity" ); cvShowImage( "disparity", vdisp ); } if( !isVerticalStereo ) { cvGetCols( pair, &part, 0, imageSize.width ); cvCvtColor( img1r, &part, CV_GRAY2BGR ); cvGetCols( pair, &part, imageSize.width, imageSize.width*2 ); cvCvtColor( img2r, &part, CV_GRAY2BGR ); for( j = 0; j < imageSize.height; j += 16 ) cvLine( pair, cvPoint(0,j), cvPoint(imageSize.width*2,j), CV_RGB(0,255,0)); } else { cvGetRows( pair, &part, 0, imageSize.height ); cvCvtColor( img1r, &part, CV_GRAY2BGR ); cvGetRows( pair, &part, imageSize.height, imageSize.height*2 ); cvCvtColor( img2r, &part, CV_GRAY2BGR ); for( j = 0; j < imageSize.width; j += 16 ) cvLine( pair, cvPoint(j,0), cvPoint(j,imageSize.height*2), CV_RGB(0,255,0)); } cvShowImage( "rectified", pair ); if( cvWaitKey() == 27 ) break; } cvReleaseImage( &img1 ); cvReleaseImage( &img2 ); } cvReleaseStereoBMState(&BMState); cvReleaseMat( &mx1 ); cvReleaseMat( &my1 ); cvReleaseMat( &mx2 ); cvReleaseMat( &my2 ); cvReleaseMat( &img1r ); cvReleaseMat( &img2r ); cvReleaseMat( &disp ); } } int main(int argc, char** argv) { StereoCalib(argc > 1 ? argv[1] : "stereo_calib.txt", 1); return 0; }
Zitieren
Lesezeichen