PARP Research Group University of Murcia, Spain


examples/OpenCV/cameraCalibrator/cameraCalibrator.cpp

Go to the documentation of this file.
00001 /*
00002  *      Copyright (C) 2007, 2008, 2009. PARP Research Group.
00003  *      <http://perception.inf.um.es>
00004  *      University of Murcia, Spain.
00005  *
00006  *      This file is part of the QVision library.
00007  *
00008  *      QVision is free software: you can redistribute it and/or modify
00009  *      it under the terms of the GNU Lesser General Public License as
00010  *      published by the Free Software Foundation, version 3 of the License.
00011  *
00012  *      QVision is distributed in the hope that it will be useful,
00013  *      but WITHOUT ANY WARRANTY; without even the implied warranty of
00014  *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015  *      GNU Lesser General Public License for more details.
00016  *
00017  *      You should have received a copy of the GNU Lesser General Public
00018  *      License along with QVision. If not, see <http://www.gnu.org/licenses/>.
00019  */
00020 
00046 #include <iostream>
00047 #include <QVMatrix>
00048 #include <QVApplication>
00049 #include <QVImageCanvas>
00050 #include <QVMPlayerCameraWorker>
00051 #include <QVDefaultGUI>
00052 
00053 #ifndef DOXYGEN_IGNORE_THIS
00054 #ifdef OPENCV
00055 #include <cv.h>
00056 #include <highgui.h>
00057 
00058 /* *************** License:**************************
00059    Oct. 3, 2008
00060    Right to use this code in any way you want without warrenty, support or any guarentee of it working.
00061 
00062    BOOK: It would be nice if you cited it:
00063    Learning OpenCV: Computer Vision with the OpenCV Library
00064      by Gary Bradski and Adrian Kaehler
00065      Published by O'Reilly Media, October 3, 2008
00066  
00067    AVAILABLE AT: 
00068      http://www.amazon.com/Learning-OpenCV-Computer-Vision-Library/dp/0596516134
00069      Or: http://oreilly.com/catalog/9780596516130/
00070      ISBN-10: 0596516134 or: ISBN-13: 978-0596516130    
00071 
00072    OTHER OPENCV SITES:
00073    * The source code is on sourceforge at:
00074      http://sourceforge.net/projects/opencvlibrary/
00075    * The OpenCV wiki page (As of Oct 1, 2008 this is down for changing over servers, but should come back):
00076      http://opencvlibrary.sourceforge.net/
00077    * An active user group is at:
00078      http://tech.groups.yahoo.com/group/OpenCV/
00079    * The minutes of weekly OpenCV development meetings are at:
00080      http://pr.willowgarage.com/wiki/OpenCV
00081    ************************************************** */
00082 
00083 /*void help()
00084         {
00085         std::cout << "\n\n"
00086         " Calling convention:\n"
00087         " ./cameraCalibratorOpenCV  <board_w>  <board_h>  <number_of_boards>  <skip_frames>\n"
00088         "\n"
00089         "   WHERE:\n"
00090         "     board_w, board_h   -- are the number of corners along the row and columns respectively\n"
00091         "     number_of_boards   -- are the number of chessboard views to collect before calibration\n"
00092         "     skip_frames        -- are the number of frames to skip before trying to collect another\n"
00093         "                           good chessboard.  This allows you time to move the chessboard.  \n"
00094         "                           Move it to many different locations and angles so that calibration \n"
00095         "                           space will be well covered. \n"
00096         "\n"
00097         " Hit ‘p’ to pause/unpause, ESC to quit\n"
00098         "\n";
00099         }*/
00100 
00101 class CameraCalibratorWorker: public QVWorker
00102         {
00103         private:
00104                 QVImage<uChar, 3> actualImage;
00105 
00106                 const int board_w, board_h, n_boards, board_dt, board_n;
00107                 const CvSize board_sz;
00108                 CvSize image_sz;
00109 
00110                 CvMat* image_points;
00111                 CvMat* object_points;
00112                 CvMat* point_counts;
00113                 
00114                 CvPoint2D32f* corners;
00115                 int successes;//, corner_count, step;//, frame;
00116 
00117         public:
00118                 CameraCalibratorWorker(QString name): QVWorker(name),
00119                         board_w(6), board_h(9), n_boards(10), board_dt(25), board_n(board_w * board_h),
00120                         board_sz(cvSize( board_w, board_h )), image_points(cvCreateMat(n_boards*board_n,2,CV_32FC1)),
00121                         object_points(cvCreateMat(n_boards*board_n,3,CV_32FC1)), point_counts(cvCreateMat(n_boards,1,CV_32SC1)),
00122                         corners(new CvPoint2D32f[ board_n ]), successes(0)//, frame(0)
00123                         {
00124                         addProperty< QVImage<uChar,3> >("Input image", inputFlag|outputFlag);
00125                         addTrigger("Grab chessboard");
00126                         addTrigger("Calibrate");
00127                         }
00128 
00129                 void processTrigger(const QString triggerName)
00130                         {
00131                         if (triggerName == "Grab chessboard")
00132                                 {
00133                                 if (successes >= n_boards) 
00134                                         {
00135                                         std::cout << "Already got "<< n_boards << " boards" << std::endl;
00136                                         return;
00137                                         }
00138 
00139                                 std::cout << "Capturing image" << std::endl;
00140                                 IplImage *image = actualImage;
00141                                 IplImage *gray_image = cvCreateImage(cvGetSize(image), 8, 1); //subpixel
00142         
00143                                 image_sz = cvGetSize(image);
00144 
00145                                 int corner_count, found = cvFindChessboardCorners(
00146                                                                         image, board_sz, corners, &corner_count,
00147                                                                         CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS);
00148                                 
00149                                 //Get Subpixel accuracy on those corners
00150                                 cvCvtColor(image, gray_image, CV_BGR2GRAY);
00151                                 cvFindCornerSubPix(     gray_image, corners, corner_count, cvSize(11,11),cvSize(-1,-1),
00152                                                         cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1));
00153         
00154                                 if(corner_count == board_n)
00155                                         {
00156                                         //cvShowImage( "Calibration", image ); //show in color if we did collect the image
00157                                         int step = successes*board_n;
00158                                         for( int i=step, j=0; j<board_n; ++i,++j )
00159                                                 {
00160                                                 CV_MAT_ELEM(*image_points, float,i,0) = corners[j].x;
00161                                                 CV_MAT_ELEM(*image_points, float,i,1) = corners[j].y;
00162                                                 CV_MAT_ELEM(*object_points,float,i,0) = j/board_w;
00163                                                 CV_MAT_ELEM(*object_points,float,i,1) = j%board_w;
00164                                                 CV_MAT_ELEM(*object_points,float,i,2) = 0.0f;
00165                                                 }
00166                                         CV_MAT_ELEM(*point_counts, int,successes,0) = board_n;    
00167                                         successes++;
00168                                         std::cout << "Got board # " << successes << " of " << n_boards << " required" << std::endl;
00169                                         }
00170                                 cvReleaseImage(&image);
00171                                 cvReleaseImage(&gray_image);
00172                                 }
00173                         else if (triggerName == "Calibrate")
00174                                 {
00175                                 if (successes < n_boards) 
00176                                         {
00177                                         std::cout << "Not enough boards" << std::endl;
00178                                         return;
00179                                         }
00180                                 std::cout << "Proceeding to camera calibration." << std::endl;
00181                                 CvMat* object_points2  = cvCreateMat(successes*board_n,3,CV_32FC1);
00182                                 CvMat* image_points2   = cvCreateMat(successes*board_n,2,CV_32FC1);
00183                                 CvMat* point_counts2   = cvCreateMat(successes,1,CV_32SC1);
00184                         
00185                                 //TRANSFER THE POINTS INTO THE CORRECT SIZE MATRICES
00186                                 for(int i = 0; i<successes*board_n; ++i)
00187                                         {
00188                                         CV_MAT_ELEM( *image_points2, float, i, 0) = CV_MAT_ELEM( *image_points, float, i, 0);
00189                                         CV_MAT_ELEM( *image_points2, float,i,1) = CV_MAT_ELEM( *image_points, float, i, 1);
00190                                         CV_MAT_ELEM(*object_points2, float, i, 0) = CV_MAT_ELEM( *object_points, float, i, 0) ;
00191                                         CV_MAT_ELEM( *object_points2, float, i, 1) = CV_MAT_ELEM( *object_points, float, i, 1) ;
00192                                         CV_MAT_ELEM( *object_points2, float, i, 2) = CV_MAT_ELEM( *object_points, float, i, 2) ;
00193                                         }
00194                         
00195                                 for(int i=0; i<successes; ++i)
00196                                         //These are all the same number
00197                                         CV_MAT_ELEM( *point_counts2, int, i, 0) = CV_MAT_ELEM( *point_counts, int, i, 0);
00198         
00199                                 CvMat* intrinsic_matrix  = cvCreateMat(3,3,CV_32FC1);
00200                                 CvMat* distortion_coeffs = cvCreateMat(4,1,CV_32FC1);
00201                         
00202                                 CV_MAT_ELEM( *intrinsic_matrix, float, 0, 0 ) = 1.0f;
00203                                 CV_MAT_ELEM( *intrinsic_matrix, float, 1, 1 ) = 1.0f;
00204                                 
00205                                 cvCalibrateCamera2(object_points2, image_points2, point_counts2,  image_sz, intrinsic_matrix, distortion_coeffs, NULL, NULL,0);
00206                                 
00207                                 std::cout << "Intrinsic Matrix K = " << std::endl << QVMatrix(intrinsic_matrix) << std::endl;
00208                                 std::cout << "Distortion coefficents = " << std::endl << QVMatrix(distortion_coeffs) << std::endl;
00209 
00210                                 cvReleaseMat(&object_points2);
00211                                 cvReleaseMat(&image_points2);
00212                                 cvReleaseMat(&point_counts2);
00213                                 cvReleaseMat(&intrinsic_matrix);
00214                                 cvReleaseMat(&distortion_coeffs);
00215                                 }
00216                         }
00217 
00218                 void iterate()
00219                         {
00220                         actualImage = getPropertyValue<QVImage<uChar, 3> >("Input image");
00221                         }
00222         };
00223 
00224 #include <QVVector>
00225 #include <QVMatrix>
00226 int main(int argc, char *argv[])
00227         {
00228         QVApplication app(argc, argv, "Example program for QVision library. Displays the contents of a video source.");
00229 
00230         QVMPlayerCameraWorker camera("Video");
00231 
00232         CameraCalibratorWorker player("Video player");
00233         camera.linkProperty(&player,"Input image");
00234 
00235         QVDefaultGUI interface;
00236 
00237         QVImageCanvas imageCanvas("Output image");
00238         player.linkProperty("Input image", imageCanvas);
00239 
00240         return app.exec();
00241         }
00242 
00243 /*int main(int argc, char* argv[])
00244         {
00245         exit(0);
00246         CvCapture* capture;
00247         
00248         if(argc != 5)
00249                 {
00250                 std::cout << std::endl << "ERROR: Wrong number of input parameters" << std::endl;
00251                 help();
00252                 return -1;
00253                 }
00254 
00255         const int       board_w  = 6,
00256                         board_h  = 9,
00257                         n_boards = 10,
00258                         board_dt = 25,
00259                         board_n  = board_w * board_h;
00260 
00261         CvSize board_sz = cvSize( board_w, board_h );
00262 
00264         capture = cvCreateCameraCapture( 0 );
00265         if(!capture)
00266                 {
00267                 std::cout << std::endl << "Couldn't open the camera" << std::endl;
00268                 help();
00269                 return -1;
00270                 }
00271         
00272         cvNamedWindow("Calibration");
00273         cvNamedWindow("Raw Video");
00275         
00276         //ALLOCATE STORAGE
00277         CvMat* image_points      = cvCreateMat(n_boards*board_n,2,CV_32FC1);
00278         CvMat* object_points     = cvCreateMat(n_boards*board_n,3,CV_32FC1);
00279         CvMat* point_counts      = cvCreateMat(n_boards,1,CV_32SC1);
00280         
00281         CvPoint2D32f* corners = new CvPoint2D32f[ board_n ];
00282         int corner_count, successes = 0, step, frame = 0;
00283         IplImage *image = cvQueryFrame( capture );
00284         IplImage *gray_image = cvCreateImage(cvGetSize(image), 8, 1); //subpixel
00285         
00286         // CAPTURE CORNER VIEWS LOOP UNTIL WE’VE GOT n_boards 
00287         // SUCCESSFUL CAPTURES (ALL CORNERS ON THE BOARD ARE FOUND)
00288         //
00289         help();
00290 
00291         while(successes < n_boards)
00292                 {
00293                 //Skip every board_dt frames to allow user to move chessboard
00294                 if((frame++ % board_dt) == 0)
00295                         {
00296                         //Find chessboard corners:
00297                         int found = cvFindChessboardCorners(    image, board_sz, corners, &corner_count,
00298                                                                 CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS);
00299                         
00300                         //Get Subpixel accuracy on those corners
00301                         cvCvtColor(image, gray_image, CV_BGR2GRAY);
00302                         cvFindCornerSubPix(     gray_image, corners, corner_count, cvSize(11,11),cvSize(-1,-1),
00303                                                 cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1));
00304                         
00305                         //Draw it
00306                         cvDrawChessboardCorners(image, board_sz, corners, corner_count, found);
00307                         
00308                         // If we got a good board, add it to our data
00309                         if(corner_count == board_n)
00310                                 {
00311                                 cvShowImage( "Calibration", image ); //show in color if we did collect the image
00312                                 step = successes*board_n;
00313                                 for( int i=step, j=0; j<board_n; ++i,++j )
00314                                         {
00315                                         CV_MAT_ELEM(*image_points, float,i,0) = corners[j].x;
00316                                         CV_MAT_ELEM(*image_points, float,i,1) = corners[j].y;
00317                                         CV_MAT_ELEM(*object_points,float,i,0) = j/board_w;
00318                                         CV_MAT_ELEM(*object_points,float,i,1) = j%board_w;
00319                                         CV_MAT_ELEM(*object_points,float,i,2) = 0.0f;
00320                                         }
00321                                 CV_MAT_ELEM(*point_counts, int,successes,0) = board_n;    
00322                                 successes++;
00323                                 std::cout << "Collected our " << successes << " of " << n_boards << " needed chessboard images" << std::endl;
00324                                 }
00325                         else
00326                                 cvShowImage("Calibration", gray_image); //Show Gray if we didn't collect the image
00327                         } //end skip board_dt between chessboard capture
00328                 
00329                 //Handle pause/unpause and ESC
00330                 int c = cvWaitKey(15);
00331                 if(c == 'p')
00332                         {  
00333                         c = 0;
00334                         while(c != 'p' && c != 27)
00335                                 c = cvWaitKey(250);
00336                         }
00337         
00338                 if(c == 27)
00339                         return 0;
00340         
00341                 image = cvQueryFrame( capture ); //Get next image
00342                 cvShowImage("Raw Video", image);
00343                 } //END COLLECTION WHILE LOOP.
00344 
00345         cvDestroyWindow("Calibration");
00346         std::cout << "\n\n*** CALLIBRATING THE CAMERA..." << std::endl;
00347 
00348         //ALLOCATE MATRICES ACCORDING TO HOW MANY CHESSBOARDS FOUND
00349         CvMat* object_points2  = cvCreateMat(successes*board_n,3,CV_32FC1);
00350         CvMat* image_points2   = cvCreateMat(successes*board_n,2,CV_32FC1);
00351         CvMat* point_counts2   = cvCreateMat(successes,1,CV_32SC1);
00352 
00353         //TRANSFER THE POINTS INTO THE CORRECT SIZE MATRICES
00354         for(int i = 0; i<successes*board_n; ++i)
00355                 {
00356                 CV_MAT_ELEM( *image_points2, float, i, 0) = CV_MAT_ELEM( *image_points, float, i, 0);
00357                 CV_MAT_ELEM( *image_points2, float,i,1) = CV_MAT_ELEM( *image_points, float, i, 1);
00358                 CV_MAT_ELEM(*object_points2, float, i, 0) = CV_MAT_ELEM( *object_points, float, i, 0) ;
00359                 CV_MAT_ELEM( *object_points2, float, i, 1) = CV_MAT_ELEM( *object_points, float, i, 1) ;
00360                 CV_MAT_ELEM( *object_points2, float, i, 2) = CV_MAT_ELEM( *object_points, float, i, 2) ;
00361                 }
00362 
00363         for(int i=0; i<successes; ++i)
00364                 //These are all the same number
00365                 CV_MAT_ELEM( *point_counts2, int, i, 0) = CV_MAT_ELEM( *point_counts, int, i, 0);
00366 
00367         cvReleaseMat(&object_points);
00368         cvReleaseMat(&image_points);
00369         cvReleaseMat(&point_counts);
00370         
00371         // At this point we have all of the chessboard corners we need.
00372         // Initialize the intrinsic matrix such that the two focal
00373         // lengths have a ratio of 1.0
00374         CvMat* intrinsic_matrix  = cvCreateMat(3,3,CV_32FC1);
00375         CvMat* distortion_coeffs = cvCreateMat(4,1,CV_32FC1);
00376 
00377         CV_MAT_ELEM( *intrinsic_matrix, float, 0, 0 ) = 1.0f;
00378         CV_MAT_ELEM( *intrinsic_matrix, float, 1, 1 ) = 1.0f;
00379         
00380         //CALIBRATE THE CAMERA!
00381         cvCalibrateCamera2(object_points2, image_points2, point_counts2,  cvGetSize( image ), intrinsic_matrix, distortion_coeffs, NULL, NULL,0);
00382         
00383         // SAVE THE INTRINSICS AND DISTORTIONS
00384         std::cout << " *** DONE!" << std::endl << std::endl << "Storing Intrinsics.xml and Distortions.xml files" << std::endl << std::endl;
00385 
00386         //std::cout << "Intrinsic Matrix K = " << std::endl << CvMatToQVMatrix(intrinsic_matrix) << std::endl;
00387         std::cout << "Intrinsic Matrix K = " << std::endl << QVMatrix(intrinsic_matrix) << std::endl;
00388         //std::cout << "Distortion coefficents = " << std::endl << CvMatToQVMatrix(distortion_coeffs) << std::endl;
00389         std::cout << "Distortion coefficents = " << std::endl << QVMatrix(distortion_coeffs) << std::endl;
00390         
00391         return 0;
00392         }*/
00393 #else   // not OPENCV
00394 int main(int argc, char* argv[])
00395         {
00396         std::cout << "ERROR: OpenCV compatibility was not activated in QVision compilation." << std::endl;
00397         return -1;
00398         }
00399 #endif  // OPENCV
00400 #endif  // DOXYGEN_IGNORE_THIS
00401 



QVision framework. PARP research group, copyright 2007, 2008.