Basic Image AlgorithmS Library 2.8.0

biascalibratecamera.cpp

00001 /*
00002    This file is part of the BIAS library (Basic ImageAlgorithmS).
00003 
00004    Copyright (C) 2003-2009    (see file CONTACT for details)
00005    Multimediale Systeme der Informationsverarbeitung
00006    Institut fuer Informatik
00007    Christian-Albrechts-Universitaet Kiel
00008 
00009 
00010    BIAS is free software; you can redistribute it and/or modify
00011    it under the terms of the GNU Lesser General Public License as published by
00012    the Free Software Foundation; either version 2.1 of the License, or
00013    (at your option) any later version.
00014 
00015    BIAS is distributed in the hope that it will be useful,
00016    but WITHOUT ANY WARRANTY; without even the implied warranty of
00017    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018    GNU Lesser General Public License for more details.
00019 
00020    You should have received a copy of the GNU Lesser General Public License
00021    along with BIAS; if not, write to the Free Software
00022    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00023 */
00024 
00025 
00026 #include <bias_config.h>
00027 #ifndef BIAS_HAVE_OPENCV
00028 #  error You need to enable OPENCV to compile this file. Please reconfigure MIP with USE_OPENCV (jw)
00029 #endif
00030 
00031 #include <Base/Common/W32Compat.hh>
00032 #include <math.h>
00033 
00034 #include <cv.h>
00035 #include <highgui.h>
00036 
00037 #ifndef CV_CALIB_ZERO_TANGENT_DIST
00038 #  error Your OpenCV version is probably too old because CV_CALIB_ZERO_TANGENT_DIST seems to be unsupported. Try again with OpenCV version >=beta5a
00039 #endif
00040 
00041 #define DRAW_IDEAL_OVERLAY
00042 #define DRAW_POINT_LABEL
00043 #define DRAW_AXES
00044 
00045 // write VRML model of P matrices using BIAS::ThreeDOut?
00046 #define WRITE_VRML
00047 
00048 // output formatting
00049 #define DIGW 12
00050 #define DIGPREC 5
00051 
00052 #include <string>
00053 #include <iostream>
00054 #include <fstream>
00055 #include <iomanip>
00056 #include <sstream>
00057 
00058 #ifdef WRITE_VRML
00059 #  include "Utils/ThreeDOut.hh"
00060 #endif // WRITE_VRML
00061 
00062 // BIAS
00063 #include <Base/Debug/Error.hh>
00064 #include <Base/Common/FileHandling.hh>
00065 #include <Base/Math/Vector3.hh>
00066 #include <Base/Image/ImageIO.hh>
00067 #include <Base/Image/ImageConvert.hh>
00068 #include <Geometry/RMatrix.hh>
00069 #include <Geometry/PMatrix.hh>
00070 #include <Geometry/Pose.hh>
00071 #include <Utils/Param.hh>
00072 
00073 using namespace BIAS;
00074 using namespace std;
00075 
00076 /**
00077    @file
00078    @ingroup g_tools
00079    @brief The program reads the images from disk,detects chessboard of given
00080    known dimension, and computes internal calibration parameters (and extrinsics
00081    for this purpose, too), see biascalibratecamera.cpp
00082    @author Jan Woetzel, University of Kiel, 03/2005, 09/2005
00083            additional features by grest
00084 */
00085 
00086 /*
00087   Usage:
00088    calibrateCamera  <chessboard_corners_per_row> <chessboard_corners_per_column>
00089    <chessboard_images> ...
00090    The program reads the images from disk,
00091    detects chessboard of given known dimension,
00092    and computes internal calibration parameters (and extrinsics for this
00093    purpose, too).
00094 
00095    Then undistorts and writes the results:
00096    - the distortion coefficient following the Bouguet model,
00097    - the camera calibration "K",
00098    - the camera image overlayed with the corners found,
00099    - the corners backprojected using their 3D scene corrdinates,
00100    - the extrinsic parameters R, C for each image
00101    - the backprojected corners in undistorted images.
00102 
00103    Based on OpenCV implementation of Bouguet calib toolbox and some sample code
00104    from Stanford University "cs223b" course notes.
00105 
00106    You may find "grabGui" and its "FindChessboard" mode useful for live
00107    capturing only images where the complete chessboard was clearly detected.
00108 
00109    Please note the different notation for extrinsics between Cv and BIAS:
00110    R_bias =      R_cv.transposed
00111    C_bias = -1 * R_cv.transpose * C_cv
00112 */
00113 
00114 BIAS::Param params;
00115 
00116 /* Backprojection from world to image coordinates
00117    @todo change to BIAS::PMatrix multiplication JW */
00118 CvPoint2D32f * ConvertWorldToPixel(CvPoint3D32f *pts3d,
00119                                    int numImages, int *numPts,
00120                                    CvMatr32f cam, CvVect32f t, CvMatr32f r,
00121                                    int xCorners, int yCorners)
00122 {
00123   int i=0, j=0, k=0;
00124   CvPoint2D32f *pts2d = new CvPoint2D32f[numImages * xCorners * yCorners];
00125 
00126   CvMat *C        = cvCreateMat(3, 3, CV_32FC1);
00127   CvMat *point3D  = cvCreateMat(3, 1, CV_32FC1);
00128   CvMat *R        = cvCreateMat(3, 3, CV_32FC1);
00129   CvMat *T        = cvCreateMat(3, 1, CV_32FC1);
00130 
00131   CvPoint3D32f *pts3dCur = pts3d;
00132   CvPoint2D32f *pts2dCur = pts2d;
00133 
00134   for (i = 0; i < 3; i++) {
00135     for (j = 0; j < 3; j++) {
00136       CV_MAT_ELEM(*C, float, i, j) = cam[3 * i + j];
00137     }
00138   }
00139 
00140   for (k = 0; k < numImages; k++) {
00141     for (j = 0; j < 9; j++) {
00142       if (j < 3) CV_MAT_ELEM(*T, float, j, 0) = t[k*3 + j];
00143       CV_MAT_ELEM(*R, float, j / 3, j%3) = r[k*9 + j];
00144     }
00145     for (i = 0; i < numPts[k]; i++) {
00146       CV_MAT_ELEM(*point3D, float, 0, 0) = pts3dCur[i].x;
00147       CV_MAT_ELEM(*point3D, float, 1, 0) = pts3dCur[i].y;
00148       CV_MAT_ELEM(*point3D, float, 2, 0) = pts3dCur[i].z;
00149 
00150       cvMatMulAdd(R, point3D, T, point3D); //rotate and translate
00151       cvMatMul(C, point3D, point3D);       //camera
00152 
00153       pts2dCur[i].x = CV_MAT_ELEM(*point3D, float, 0, 0) /
00154                       CV_MAT_ELEM(*point3D, float, 2, 0);
00155       pts2dCur[i].y = CV_MAT_ELEM(*point3D, float, 1, 0) /
00156                       CV_MAT_ELEM(*point3D, float, 2, 0);
00157     }
00158     pts3dCur += numPts[k];
00159     pts2dCur += numPts[k];
00160   }
00161 
00162   cvReleaseMat(&point3D);
00163   cvReleaseMat(&T);
00164   cvReleaseMat(&R);
00165 
00166   return pts2d;
00167 }
00168 
00169 
00170 /* DrawPoints takes number of corners on x axis and y axis,
00171    and number of images to builds an array of corners.
00172    Assumes all corners on z = 0 plane.
00173    @todo Move to BIAS/FeatureDetector/CheckerboardDetector JW
00174 */
00175 void DrawPoints(CvArr *img, CvPoint2D32f *ptArray, int numPts,
00176                 const int xCorners, const int yCorners)
00177 {
00178   CvPoint pt;
00179   CvPoint ptOld;
00180 #ifdef DRAW_POINT_LABEL
00181   CvFont font;
00182   cvInitFont(&font, CV_FONT_HERSHEY_PLAIN, 1.0f, 1.0f);
00183 #endif //DRAW_POINT_LABEL
00184 
00185   // init old as first
00186   if (numPts > 0) {
00187     ptOld.x = (int)ptArray[0].x;
00188     ptOld.y = (int)ptArray[0].y;
00189   }
00190   for (int i = 0; i < numPts; i++) {
00191     int clr = (int)((float)i / numPts * 255.0);
00192     pt.x = (int)ptArray[i].x;
00193     pt.y = (int)ptArray[i].y;
00194     cvCircle(img, pt, 3, CV_RGB(255-clr,0,clr), CV_FILLED);
00195 
00196 #ifdef DRAW_POINT_LABEL
00197     // label the point
00198     stringstream ssID;
00199     ssID << i;
00200     cvPutText(img, ssID.str().c_str(), pt, &font, CV_RGB(0,0,255));
00201 #endif //DRAW_POINT_LABEL
00202     // next
00203     ptOld=pt;
00204   }
00205 
00206 #ifdef DRAW_IDEAL_OVERLAY
00207   // mark ideal pattern
00208   // horizontal lines
00209   for (int xx = 0; xx < xCorners*yCorners; xx+=xCorners) {
00210     cvLine(img, cvPointFrom32f(ptArray[xx]),
00211            cvPointFrom32f(ptArray[xx+xCorners-1]),
00212            CV_RGB(0,0,255), 1, CV_AA);
00213   }
00214   // vertical lines
00215   for (int yy = 0; yy < xCorners; yy+=1) {
00216     cvLine(img, cvPointFrom32f(ptArray[yy]),
00217            cvPointFrom32f(ptArray[yy+xCorners*(yCorners-1)]),
00218            CV_RGB(0,0,255), 1, CV_AA);
00219   }
00220 #endif
00221 
00222 #ifdef DRAW_AXES
00223   CvFont fontBig;
00224   cvInitFont(&fontBig, CV_FONT_HERSHEY_PLAIN, 5.0f, 5.0f, 0, 2, CV_AA);
00225   cvPutText(img, "0", cvPointFrom32f(ptArray[0]), &fontBig, CV_RGB(0,0,255));
00226   cvLine(img, cvPointFrom32f(ptArray[0]), cvPointFrom32f(ptArray[xCorners-1]),
00227          CV_RGB(255,0,0), 1, CV_AA);
00228   cvPutText(img, "X", cvPointFrom32f(ptArray[xCorners-1]),
00229             &fontBig, CV_RGB(255,0,0));
00230   cvLine(img, cvPointFrom32f(ptArray[0]),
00231          cvPointFrom32f(ptArray[xCorners*(yCorners-1)]),
00232          CV_RGB(0,255,0), 1, CV_AA);
00233   cvPutText(img, "Y", cvPointFrom32f(ptArray[xCorners*(yCorners-1)]),
00234             &fontBig, CV_RGB(0,255,0));
00235 #endif //DRAW_AXES
00236 }
00237 
00238 /* InitCorners3d
00239    Takes number of corners on x axis and y axis + number of images
00240    Builds a regular checkerboard array of corners.
00241    Assumes all corners on z = 0 plane.
00242 */
00243 void InitCorners3d(CvPoint3D32f *corners3d, int xCorners,
00244                    int yCorners, int numImages)
00245 {
00246   int n=0, x=0, y=0;
00247   int nStride = xCorners * yCorners;
00248   for (n = 0; n < numImages; n++) {
00249     for (x = 0; x < xCorners; x++) {
00250       for (y = 0; y < yCorners; y++) {
00251         corners3d[n*nStride + x +xCorners*y].x = (float)x;
00252         corners3d[n*nStride + x +xCorners*y].y = (float)y;
00253         corners3d[n*nStride + x +xCorners*y].z = 0.0f;
00254       }
00255     }
00256   }
00257 }
00258 
00259 /* InitCorners3dMatrix
00260    Takes number of corners on x axis and y axis + number of images
00261    Builds a regular checkerboard matrix of corners.
00262    Assumes all corners on z = 0 plane.
00263 */
00264 void InitCorners3dMatrix(CvMat *corners3dMat,
00265                          int xCorners, int yCorners, int numImages)
00266 {
00267   int n=0, x=0, y=0;
00268   int offset=xCorners * yCorners;
00269   for (n = 0; n < numImages; n++) {
00270     for (x = 0; x < xCorners; x++) {
00271       for (y = 0; y < yCorners; y++) {
00272         CV_MAT_ELEM(*corners3dMat,float,n*offset+x+ y*xCorners, 0) = (float)x;
00273         CV_MAT_ELEM(*corners3dMat,float,n*offset+x+ y*xCorners, 1) = (float)y;
00274         CV_MAT_ELEM(*corners3dMat,float,n*offset+x+ y*xCorners, 2) = 0.0f;
00275       }
00276     }
00277   }
00278 }
00279 
00280 
00281 void InitCorners2dPointMatrix(CvMat *points2dMat,CvPoint2D32f *cornersArray,
00282                               CvMat *pointCounts, int* cornersFound,
00283                               int numImages)
00284 {
00285   int n=0, x=0, row=0;
00286   for (n = 0; n < numImages; n++) {
00287     int corners=cornersFound[n];
00288     CV_MAT_ELEM(*pointCounts,int,n,0)=corners;
00289     for (x = 0; x < corners; x++) {
00290       CV_MAT_ELEM(*points2dMat,float,row+x,0) = cornersArray[row+x].x;
00291       CV_MAT_ELEM(*points2dMat,float,row+x,1) = cornersArray[row+x].y;
00292     }
00293     row+=corners;
00294   }
00295 }
00296 
00297 /* return BIAS::Vector3 structure from float array */
00298 BIAS::Vector3<double> GetCfromArr(const unsigned int camIndex,
00299                                   const float * translation_vectors,
00300                                   const unsigned int numImages)
00301 {
00302   BIASASSERT(translation_vectors!=NULL);
00303   BIASASSERT(camIndex<numImages);
00304   BIAS::Vector3<double> C;
00305   for (unsigned int k=0; k<3; k++)
00306     C.GetData()[k] = translation_vectors[camIndex*3 +k];
00307   return C;
00308 }
00309 
00310 /* return BIAS::RMatrix structure from float array */
00311 BIAS::RMatrix GetRfromArr(const unsigned int camIndex,
00312                           const float * rotation_matrices,
00313                           const unsigned int numImages)
00314 {
00315   BIASASSERT(rotation_matrices!=NULL);
00316   BIASASSERT(camIndex<numImages);
00317   BIAS::RMatrix R;
00318   for (unsigned int k=0; k<9; k++)
00319     R.GetData()[k] = rotation_matrices[camIndex*9 +k];
00320   return R;
00321 }
00322 
00323 /* return BIAS::KMatrix structure from float array */
00324 BIAS::KMatrix GetKfromArr(const float * camera_matrix)
00325 {
00326   BIASASSERT(camera_matrix!=NULL);
00327   BIAS::KMatrix K;
00328   for (unsigned int i=0; i<9; i++)
00329     K.GetData()[i] = camera_matrix[i];
00330   return K;
00331 }
00332 
00333 /* return BIAS::PMatrix structure from float array */
00334 BIAS::PMatrix GetPfromArr(const unsigned int camIndex,
00335                           const float * rotation_matrices,
00336                           const float * translation_vectors,
00337                           const unsigned int numImages)
00338 {
00339   BIAS::PMatrix P;
00340   P.SetIdentity();
00341   BIASASSERT(rotation_matrices!=NULL);
00342   BIASASSERT(translation_vectors!=NULL);
00343   BIASASSERT(camIndex<numImages);
00344   return P;
00345 }
00346 
00347 /* formatted output of intrinsics */
00348 void PrintIntrinsics(CvVect32f distortion,
00349                      CvMatr32f camera_matrix,
00350                      std::ostream & os)
00351 {
00352   BIASASSERT(distortion!=NULL);
00353   os << "Distortion coefficients:" << endl
00354      << "[0] radial     "
00355      << setw(DIGW)<<setprecision(DIGPREC)<<distortion[0]<<endl
00356      << "[1] radial     "
00357      << setw(DIGW)<<setprecision(DIGPREC)<<distortion[1]<<endl
00358      << "[2] tangential "
00359      << setw(DIGW)<<setprecision(DIGPREC)<<distortion[2]<<endl
00360      << "[3] tangential "
00361      << setw(DIGW)<<setprecision(DIGPREC)<<distortion[3]<<endl;
00362 
00363   BIASASSERT(camera_matrix != NULL);
00364   os << "Camera matrix K:" << endl;
00365   //PrintMatrix(camera_matrix, 3, 3, os);
00366   os << "  " << GetKfromArr(camera_matrix) << endl;
00367 }
00368 
00369 /* BIAS::Vector4 compatible output for undistortion call */
00370 void PrintDistortionVec4(CvVect32f distortion,
00371                          std::ostream & os)
00372 {
00373   BIASASSERT(distortion!=NULL);
00374   os << "4" << endl
00375      << setw(DIGW)<<setprecision(DIGPREC)<<distortion[0]<<endl
00376      << setw(DIGW)<<setprecision(DIGPREC)<<distortion[1]<<endl
00377      << setw(DIGW)<<setprecision(DIGPREC)<<distortion[2]<<endl
00378      << setw(DIGW)<<setprecision(DIGPREC)<<distortion[3]<<endl;
00379 }
00380 
00381 /* convert rotation matrix from OpenCV notation to BIAS notation JW */
00382 BIAS::RMatrix R_cv2bias(const BIAS::RMatrix & Rcv)
00383 {
00384   BIAS::RMatrix Rbias = Rcv.Transpose();
00385   return Rbias;
00386 }
00387 
00388 /* convert translation vector from OpenCV notation to BIAS notation JW */
00389 BIAS::Vector3<double> C_cv2bias(const BIAS::Vector3<double> & Ccv,
00390                                 const BIAS::RMatrix & Rcv )
00391 {
00392   BIAS::Vector3<double> Cbias = -1.0 * (Rcv.Transpose() * Ccv);
00393   return Cbias;
00394 }
00395 
00396 /* print formatted extrinsic parameters JW */
00397 void PrintExtrinsics(const float * translation_vectors,
00398                      const float * rotation_matrices,
00399                      const unsigned int numImages,
00400                      std::ostream & os)
00401 {
00402   BIASASSERT(translation_vectors != NULL);
00403   BIASASSERT(rotation_matrices != NULL);
00404 
00405   BIAS::RMatrix tmpRcv, Rbias;
00406   BIAS::Vector3<double> tmpCcv, Cbias;
00407   for (unsigned int i = 0; i < numImages; i++) {
00408     tmpRcv = GetRfromArr(i, rotation_matrices,   numImages);
00409     tmpCcv = GetCfromArr(i, translation_vectors, numImages);
00410     Rbias = R_cv2bias(tmpRcv);
00411     Cbias = C_cv2bias(tmpCcv, tmpRcv);
00412     os << "[" << i << "] R = " << Rbias << "\tC = " << Cbias << endl;
00413   }
00414 }
00415 
00416 /* print usage information for application */
00417 void usage(int argc, char** argv)
00418 {
00419   cout<<"Usage: calibrateCamera <inner_chessboard_corners_X> "
00420       <<"<inner_chessboard_cornersY> [all-image-filenames]"<<endl
00421       <<"  with at least 2 inner chessboard corners and "
00422       <<"*non-symmetric* chessboard pattern. "<<endl
00423       <<"  In particular 8x8 is *NOT* OK while 8x7 is good."<<endl
00424       <<endl
00425       <<"Sample usage:"<<endl
00426       <<"  calibrateCamera 7 4  a.jpg b.jpg c.jpg d.jpg e.jpg "<<endl
00427       <<"or special dart test modus:"<<endl
00428       <<"  calibrateCamera --darttest [0|1] a.jpg b.jpg c.jpg d.jpg e.jpg ..."<<endl
00429       <<endl
00430       <<"(c) MIP University of Kiel"<<endl
00431       <<"Jan Woetzel (www.mip.informatik.uni-kiel.de/~jw)"<<endl
00432       <<"feel free to ask me fo details."<<endl;
00433   params.Usage();
00434 }
00435 
00436 
00437 /* MAIN APPLICATION */
00438 int main(int argc, char** argv)
00439 {
00440   if (argc > 0) {
00441     cout<<"Started "<<argv[0]<<"\n(c) MIP University of Kiel, Jan Woetzel"<<endl;
00442   }
00443   IplImage *image = NULL;
00444   IplImage *undistort = NULL;
00445   IplImage *gray = NULL;
00446   CvPoint2D32f *cornersArray = NULL;
00447   int numImages=0;
00448   CvPoint3D32f *corners3d = NULL;
00449   CvVect32f translation_vectors = NULL;
00450   CvMatr32f rotation_matrices = NULL;
00451 
00452   bool usegui=true;
00453 
00454 
00455   params.DisableDestructorWarning();
00456   params.AddParamBool("fixedAspect","focal length is euqal in both directions",
00457                       false,'a');
00458   params.AddParamBool("fixedPrincipal",
00459                       "principal point is fixed in image center",
00460                       false,'p');
00461   params.AddParamBool("fixedAll","only calibrate extrinsic parameters, "
00462                       "KMatrix must be given, distortion is optional",
00463                       false,'f');
00464   params.AddParamString("kmatrix","file name of K-matrix with inital values,"
00465                         " which are unchanged if fixedAll","",'K');
00466   params.AddParamString("distortion","file name of vector with distortion "
00467                         " values (radial k1, k2, tangential p1,p2)",
00468                         "",'D');
00469   params.AddParamString("darttest","perform dart test");
00470   params.SetEnhancedFlag("darttest",true);
00471   //params.SetHiddenFlag("darttest",true);
00472   int nextParam=params.ParseCommandLine(argc,argv);
00473   BIASASSERT(nextParam >= 0);
00474 
00475   BIASASSERT(params.GetParamString("darttest") != NULL);
00476   if ((argc-nextParam<3) && (params.GetParamString("darttest")->size()==0)) {
00477     usage(argc, argv);
00478     return -1;
00479   }
00480 
00481   BIAS::Vector4<float>   distortion;
00482   distortion.SetZero();
00483   BIAS::Matrix3x3<float> camera_matrix;
00484   camera_matrix.SetIdentity();
00485   int flags = 0;
00486   flags |= CV_CALIB_ZERO_TANGENT_DIST; // no tangential distortion
00487 
00488   if (params.GetParamString("kmatrix")->length() > 0) {
00489     ifstream in(params.GetParamString("kmatrix")->c_str());
00490     if (!in) {
00491       cerr << "Error opening KMatrix file "
00492            << *params.GetParamString("kmatrix") << endl;
00493       return -1;
00494     }
00495     in >> camera_matrix;
00496     flags |= CV_CALIB_USE_INTRINSIC_GUESS;
00497   }
00498   if (*params.GetParamBool("fixedAspect"))
00499     flags |= CV_CALIB_FIX_ASPECT_RATIO;
00500   if (*params.GetParamBool("fixedPrincipal"))
00501     flags |= CV_CALIB_FIX_PRINCIPAL_POINT;
00502 
00503   BIAS::Vector4<float> biasDistortion(0, 0, 0, 0);
00504   if (params.GetParamString("distortion")->length() > 0) {
00505     ifstream in(params.GetParamString("distortion")->c_str());
00506     if (!in) {
00507       cerr << "Error opening distortion file "
00508            << *params.GetParamString("distortion") << endl;
00509       return -1;
00510     }
00511     in >> biasDistortion;
00512     cout << "User given distortion values:" << biasDistortion << endl;
00513   }
00514 
00515   int xCorners = 0;
00516   int yCorners = 0;
00517   int iImg = 0;
00518   int width = 0;
00519   int height = 0;
00520 
00521   // HACK Dart Testing (JW)
00522   bool darttest = false;
00523   if (argc >= 2) {
00524     if (strcmp(argv[1],"--darttest") == 0) {
00525       std::cout << "Started dart test for " << argv[0] << std::endl;
00526       xCorners = 7; // HACK fixed size for darttest
00527       yCorners = 4;
00528       darttest = true;
00529       if (argv[2][0] == '1') {
00530         usegui = true;
00531       } else {
00532         usegui = false;
00533       }
00534       //nextParam=0;
00535     } else {
00536       // parse cmd line
00537       xCorners = atoi(argv[nextParam]);
00538       nextParam++;
00539       if (xCorners < 1)
00540         cout<<"[ERROR] xCorners = "<<xCorners<<" is invalid!"<<endl;
00541       yCorners = atoi(argv[nextParam]);
00542       nextParam++;
00543       if (yCorners<1)
00544         cout<<"[ERROR] yCorners = "<<yCorners<<" is invalid!"<<endl;
00545     }
00546   }
00547 
00548   // atoi return 0 on error, non numeric.
00549   if ((xCorners<=2) || (yCorners<=2)) {
00550     usage(argc, argv);
00551     return -2;
00552   }
00553 
00554   // the rest are the input images
00555   numImages = argc - nextParam;
00556 
00557   // allocate:
00558   cornersArray = new CvPoint2D32f[xCorners*yCorners*numImages];
00559   corners3d = new CvPoint3D32f[xCorners*yCorners*numImages];
00560   int *cornersFound   = new int[numImages];
00561   translation_vectors = new float[3*numImages];
00562   rotation_matrices   = new float[9*numImages];
00563   InitCorners3d(corners3d, xCorners, yCorners, numImages);
00564 
00565   // grest stuff
00566   CvMat *corners3dMat = cvCreateMat(xCorners*yCorners*numImages,3, CV_32FC1);
00567   InitCorners3dMatrix(corners3dMat, xCorners, yCorners, numImages);
00568 
00569   if (usegui) {
00570     cvNamedWindow("Image", CV_WINDOW_AUTOSIZE);
00571     cvMoveWindow("Image", 0,0);
00572   }
00573   CvPoint2D32f *nextCornersArray = cornersArray;
00574   string imgFilename;
00575   string outFilename;
00576   string outFilenameVRML("out_allP.wrl");
00577 
00578 #ifdef WRITE_VRML
00579   ThreeDOutParameters threeDOutParams;
00580   ThreeDOut CameraVRML(threeDOutParams);
00581 #endif // WRITE_VRML
00582 
00583   BIAS::Image<unsigned char> biasImg;
00584   for (iImg = 0; iImg < numImages; iImg++) {
00585     BIASASSERT((iImg+nextParam)<argc);
00586     imgFilename = argv[iImg+nextParam];
00587     // Load all the new images into their data structures
00588     cout << "Loading " << imgFilename << endl;
00589     if (ImageIO::Load( imgFilename.c_str(), biasImg)!=0) {
00590       BIASERR("BIAS::ImageIO failed to load image " << imgFilename
00591               << ". Now trying OpenCV routines...");
00592       image = cvLoadImage(imgFilename.c_str());
00593       if (image == NULL){
00594         BIASERR("OpenCV failed to load " << imgFilename << " either. Aborting!");
00595         BIASBREAK;
00596         BIASABORT;
00597       }
00598     } else {
00599 #ifdef BIAS_HAVE_OPENCV
00600       // conversion required fromBIAS to cv image
00601       ImageConvert::BIAS2ipl(biasImg, image);
00602 #else
00603       BIASERR("case not implemented.");
00604 #endif
00605     }
00606     BIASASSERT(image!=NULL); // loaded?
00607     width = image->width;
00608     height = image->height;
00609 
00610     if (image->depth != IPL_DEPTH_8U) {
00611       cerr << "[ERROR] Depth is not IPL_DEPTH_8U for image "
00612            << imgFilename <<"!" << endl;
00613       return -2;
00614     }
00615 
00616     // Convert image to grayscale if it is not already
00617     if (image->nChannels == 1 && image->depth == IPL_DEPTH_8U) {
00618       gray = cvCloneImage(image);
00619     } else {
00620       gray = cvCreateImage(cvSize(image->width, image->height),
00621                            IPL_DEPTH_8U, 1);
00622       BIASASSERT(gray!=NULL);
00623       cvConvertImage(image, gray);
00624     }
00625 
00626     // Feedback of images loaded for sanity
00627     if (usegui)
00628       cvShowImage("Image", image);
00629 
00630     // Start calibration of points
00631     cornersFound[iImg] = xCorners * yCorners;
00632 
00633     // nextCornersArray is a pointer into cornersArray to the last slot open
00634     // this will overflow if cvchessboard ever gives us more corners then expected
00635 
00636 /*
00637     // deprecated call
00638     IplImage *threshold = cvCreateImage(cvSize(image->width, image->height),
00639                                         IPL_DEPTH_8U, 1);
00640     BIASASSERT(threshold != NULL);
00641     int res = cvFindChessBoardCornerGuesses(gray, threshold, NULL,
00642                                             cvSize(xCorners, yCorners),
00643                                             nextCornersArray,
00644                                             &cornersFound[iImg]);
00645     cvReleaseImage(&threshold);
00646 */
00647 
00648     int res = cvFindChessboardCorners(gray, cvSize(xCorners, yCorners),
00649                                       nextCornersArray,
00650                                       &cornersFound[iImg],
00651                                       CV_CALIB_CB_ADAPTIVE_THRESH);
00652                                       //CV_CALIB_CB_NORMALIZE_IMAGE);
00653                                       //CV_CALIB_CB_FILTER_QUADS);
00654 
00655     cvFindCornerSubPix(gray, nextCornersArray,
00656                        cornersFound[iImg], cvSize(5,5), cvSize(-1,-1),
00657                        cvTermCriteria(CV_TERMCRIT_ITER, 100, 0.1));
00658 
00659     // more visual feedback to confirm correctness
00660     DrawPoints(image, nextCornersArray, cornersFound[iImg], xCorners, yCorners);
00661     if (usegui) {
00662       cvShowImage("Image", image);
00663       cvWaitKey(1);
00664     }
00665 
00666     // save debug image
00667     outFilename = FileHandling::Basename(imgFilename) +"_findChessBoardCornerGuesses.jpg";
00668     cvSaveImage(outFilename.c_str(), image);
00669 
00670     if (!res) {
00671       cerr<<"[ERROR] Could not find all "<<xCorners<<"x"<<yCorners
00672           <<" inner corners of checkerboard in image "<<imgFilename << endl
00673           <<"        but actually checkerboard has to be detectable"
00674           <<" in *all* images!"<<endl<<"        Please remove image "
00675           <<imgFilename<<" and try again!"<<endl;
00676       cerr<<"Press any key in GUI window."<<endl;
00677       cvWaitKey(0);
00678       return -3;
00679     }
00680     // update so that nextCornersArray points to the next open slot in cornersArray
00681     nextCornersArray += cornersFound[iImg];
00682 
00683     // required to update display:
00684     cvWaitKey(1);
00685 
00686     cvReleaseImage(&image);
00687     cvReleaseImage(&gray);
00688   }
00689   if (usegui) cvDestroyWindow("Image");
00690 
00691   cout<<"Solving calibration... "<<flush;
00692 
00693 /*
00694   // run calibration routine on pre-detecteed coordinates
00695   cvCalibrateCamera(numImages,
00696                     cornersFound, //point count
00697                     cvSize(width, height), // image size
00698                     cornersArray, // image points
00699                     corners3d, // object points
00700                     distortion.GetData(), // coefficients
00701                     camera_matrix.GetData(),
00702                     translation_vectors,
00703                     rotation_matrices,
00704                     FLAGS);
00705 */
00706 
00707   int totalpoints = 0;
00708   for (int i = 0;i < numImages; i++) {
00709     totalpoints += cornersFound[i];
00710   }
00711   CvMat *points2dMat = cvCreateMat(totalpoints, 2, CV_32FC1);
00712   CvMat *pointCountMat = cvCreateMat(numImages, 1, CV_32SC1);
00713   InitCorners2dPointMatrix(points2dMat, cornersArray, pointCountMat,
00714                            cornersFound, numImages);
00715   CvMat *camMatrix = cvCreateMat(3,3,CV_32FC1);
00716   if (params.GetParamString("kmatrix")->length() > 0) {
00717     cvInitMatHeader(camMatrix,3,3,CV_32FC1,  camera_matrix.GetData());
00718   } else {
00719     cvmSetZero(camMatrix);
00720     CV_MAT_ELEM(*camMatrix,float,0,0) = 600.;
00721     CV_MAT_ELEM(*camMatrix,float,1,1) = 600.;
00722     CV_MAT_ELEM(*camMatrix,float,0,2) = float(width/2.);
00723     CV_MAT_ELEM(*camMatrix,float,1,2) = float(height/2.);
00724     CV_MAT_ELEM(*camMatrix,float,2,2) = 1.;
00725   }
00726 
00727   CvMat *distortMatrix = cvCreateMat(4,1,CV_32FC1);
00728   if (params.GetParamString("distortion")->length() > 0) {
00729     cvInitMatHeader(distortMatrix,4,1,CV_32FC1,  biasDistortion.GetData());
00730   } else {
00731     cvmSetZero(distortMatrix);
00732   }
00733 
00734   CvMat *rotVectors = cvCreateMat(numImages,3,CV_32FC1);
00735   CvMat *transVectors = cvCreateMat(numImages,3,CV_32FC1);
00736 
00737   if (*params.GetParamBool("fixedAll")) {
00738     CvMat *rotVector = cvCreateMat(1,3,CV_32FC1);
00739     CvMat *transVector = cvCreateMat(1,3,CV_32FC1);
00740     int nextPoint=0;
00741     for (int n=0; n<numImages; n++) {
00742       // build up temporary 3d and 2d points
00743       int nrPoints =  CV_MAT_ELEM(*pointCountMat,int,n,0);
00744       CvMat *points3DSingleView = cvCreateMat(nrPoints,3,CV_32FC1);
00745       CvMat *points2DSingleView = cvCreateMat(nrPoints,2,CV_32FC1);
00746       for (int p=0; p < nrPoints; p++) {
00747         int r;
00748         for (r=0; r < 2; r++) {
00749           CV_MAT_ELEM(*points2DSingleView,float,p,r) =
00750             CV_MAT_ELEM(*points2dMat,float,nextPoint+p,r);
00751         }
00752         for (r=0; r < 3; r++) {
00753           CV_MAT_ELEM(*points3DSingleView,float,p,r) =
00754             CV_MAT_ELEM(*corners3dMat,float,nextPoint+p,r);
00755         }
00756       }
00757       nextPoint+=nrPoints;
00758       cvFindExtrinsicCameraParams2(//corners3dMat, points2dMat,
00759                                    points3DSingleView, points2DSingleView,
00760                                    camMatrix, distortMatrix,
00761                                    rotVector, transVector);
00762       for (int r=0; r < 3; r++) {
00763         CV_MAT_ELEM(*rotVectors,float,n,r) =
00764           CV_MAT_ELEM(*rotVector,float,0,r);
00765         CV_MAT_ELEM(*transVectors,float,n,r) =
00766           CV_MAT_ELEM(*transVector,float,0,r);
00767       }
00768     }
00769   } else {
00770     cvCalibrateCamera2(corners3dMat, points2dMat,
00771                        pointCountMat, cvSize(width, height),
00772                        camMatrix, distortMatrix,
00773                        rotVectors, transVectors, flags);
00774   }
00775 
00776   for (unsigned int i=0;i<3;i++) {
00777     for (unsigned int j=0;j<3;j++) {
00778       camera_matrix[i][j]= CV_MAT_ELEM(*camMatrix,float,i,j);
00779     }
00780   }
00781   memcpy(distortion.GetData(),distortMatrix->data.fl,sizeof(float)*4);
00782   CvMat *dummyRot= cvCreateMat(1,3,CV_32FC1);
00783   CvMat *dummyRes= cvCreateMat(3,3,CV_32FC1);
00784   for (int i=0; i<numImages; i++) {
00785     dummyRot->data.fl= &(rotVectors->data.fl[i*3]);
00786     cvRodrigues2( dummyRot, dummyRes);
00787     memcpy(&rotation_matrices[i*9],dummyRes->data.fl,sizeof(float)*9);
00788     translation_vectors= transVectors->data.fl;
00789   }
00790   cout<<" done."<<endl;
00791 
00792   // output result
00793   PrintExtrinsics(translation_vectors, rotation_matrices, numImages,std::cout);
00794   {
00795     ofstream ofs("out_extrinsics.txt");
00796     PrintExtrinsics(translation_vectors, rotation_matrices, numImages,  ofs);
00797     cout<<"Wrote result to file out_extrinsics.txt"<<endl;
00798     ofs.close();
00799   }
00800   PrintIntrinsics(distortion.GetData(),
00801                   camera_matrix.GetData(),
00802                   std::cout);
00803   {
00804     ofstream ofs("out_intrinsics.txt");
00805     PrintIntrinsics(distortion.GetData(),
00806                     camera_matrix.GetData(),
00807                     ofs);
00808     cout<<"Wrote result to file out_intrinsics.txt"<<endl;
00809     ofs.close();
00810   }
00811   {
00812     // save in BIAS Vector4 format, too.
00813     ofstream ofs("out_distortion.txt");
00814     PrintDistortionVec4(distortion.GetData(), ofs);
00815     cout<<"Wrote result to file out_intrinsics.txt"<<endl;
00816     ofs.close();
00817   }
00818 
00819 
00820   // display indivisual resulta to disk (same notation for intrinsics betwene CV,BIAS
00821   BIAS::KMatrix Kbi;
00822   Kbi = GetKfromArr(camera_matrix.GetData());
00823   Kbi.Save("out_Kmat.K");
00824 
00825   BIAS::RMatrix Rbi;
00826   BIAS::PMatrix Pbi;
00827   BIAS::Vector3<double> Cbi;
00828   BIAS::Vector3<double> lastC;
00829   lastC.SetZero();
00830   ofstream ofs;
00831   for (iImg = 0; iImg < numImages; iImg++) {
00832     Rbi = R_cv2bias(GetRfromArr(iImg,rotation_matrices, numImages));
00833     Cbi = C_cv2bias(GetCfromArr(iImg, translation_vectors, numImages),
00834                     GetRfromArr(iImg,rotation_matrices, numImages));
00835 
00836     // write R for each image
00837     imgFilename = string("out_")+FileHandling::Basename(string(argv[iImg+nextParam]))+
00838                   string(".Rmat");
00839     ofs.open(imgFilename.c_str());
00840     ofs << Rbi;
00841     ofs.close();
00842 
00843     // write C for each image
00844     imgFilename = string("out_")+FileHandling::Basename(string(argv[iImg+nextParam]))+
00845                   string(".Cmat");
00846     ofs.open(imgFilename.c_str());
00847     ofs<<Cbi;
00848     ofs.close();
00849 
00850     // compose P from K,R,C and write it for each image.
00851     imgFilename = string("out_")+FileHandling::Basename(string(argv[iImg+nextParam]))+
00852                   string(".P");
00853     ofs.open(imgFilename.c_str());
00854     Pbi=BIAS::PMatrix( Kbi, Rbi, Cbi );
00855     ofs<<Pbi;
00856     ofs.close();
00857 
00858 #ifdef WRITE_VRML
00859     if (iImg==0) {
00860       // first camera
00861       CameraVRML.AddPMatrix(Pbi, width, height, RGBAuc(0,0,255,0));
00862       // mark coordinate system:
00863       CameraVRML.AddLine(lastC, Cbi, RGBAuc(255,255,0,0));
00864     } else {
00865       // other cams
00866       CameraVRML.AddPMatrix(Pbi, width, height, RGBAuc(0,255,0,0));
00867       CameraVRML.AddLine(lastC, Cbi, RGBAuc(255,0,0,0));
00868     }
00869 #endif // WRITE_VRML
00870 
00871     // next:
00872     lastC=Cbi;
00873   }
00874   cout<<"Wrote results to out_*.Rmat, out_*.Cmat and out_*.P"<<endl;
00875 
00876 #ifdef WRITE_VRML
00877   const double scale=1.0;
00878   // mark coordinate system:
00879   CameraVRML.AddLine(Vector3<double>(0,0,0),
00880                      scale*Vector3<double>(1,0,0), RGBAuc(255,0,0,0));
00881   CameraVRML.AddLine(Vector3<double>(0,0,0),
00882                      scale*Vector3<double>(0,1,0), RGBAuc(0,255,0,0));
00883   CameraVRML.AddLine(Vector3<double>(0,0,0),
00884                      scale*Vector3<double>(0,0,1), RGBAuc(0,0,255,0));
00885 
00886   // save to disk:
00887   CameraVRML.VRMLOut(outFilenameVRML);
00888   cout<<"wrote vrml \""<<outFilenameVRML<<"\""<<endl;
00889 #endif // WRITE_VRML
00890 
00891   CvPoint2D32f *backprojPts2d =
00892     ConvertWorldToPixel(corners3d, numImages, cornersFound,
00893                         camera_matrix.GetData(), translation_vectors,
00894                         rotation_matrices, xCorners, yCorners);
00895   // display undistortion result:
00896   if (usegui) {
00897     cvNamedWindow("Undistorted", CV_WINDOW_AUTOSIZE);
00898     cvMoveWindow("Undistorted", 0,0);
00899     cout<<"Opened GUI"<<endl;
00900   }
00901 
00902   for (iImg = 0; iImg < numImages; iImg++) {
00903     // Load all the input images again for undistortion
00904     imgFilename = argv[iImg+nextParam];
00905     if (ImageIO::Load(imgFilename.c_str(), biasImg) == 0) {
00906 #ifdef BIAS_HAVE_OPENCV
00907       ImageConvert::BIAS2ipl(biasImg, image);
00908 #else
00909       BIASERR("No support to convert image loaded by BIAS into OpenCV format!");
00910 #endif
00911     } else {
00912       BIASERR("Failed to load image with BIAS! Trying OpenCV routines, now...");
00913       image = cvLoadImage(imgFilename.c_str());
00914       if (image == NULL){
00915         BIASERR("Failed to load image " << imgFilename
00916                 << "with OpenCV either! Aborting.");
00917         BIASBREAK;
00918         BIASABORT;
00919       }
00920     }
00921     cout << "Loaded and converted image " << imgFilename.c_str() << "." << endl;
00922 
00923     // Create undistorted image (grayscale for grayscale input images)
00924     undistort = cvCreateImage(cvSize(width, height), IPL_DEPTH_8U,
00925                               image->nChannels);
00926     cvUndistort2(image, undistort, camMatrix, distortMatrix);
00927 
00928     DrawPoints(image, &backprojPts2d[iImg*cornersFound[iImg]],
00929                 cornersFound[iImg], xCorners,yCorners);
00930     if (usegui) {
00931       cvShowImage("Image", image);
00932       cvWaitKey(1);
00933     }
00934     DrawPoints(undistort, &backprojPts2d[iImg*cornersFound[iImg]],
00935                cornersFound[iImg], xCorners,yCorners);
00936     if (usegui) {
00937       cvShowImage("Undistorted", undistort);
00938       cvWaitKey(1);
00939     }
00940     cout << "Drawed checkerboard corners into image" << endl;
00941     outFilename = FileHandling::Basename(imgFilename) +"_backproject.jpg";
00942     cvSaveImage(outFilename.c_str(), image);
00943 
00944     outFilename = FileHandling::Basename(imgFilename) +"_backproject_undistorted.jpg";
00945     cvSaveImage(outFilename.c_str(), undistort);
00946 
00947     cout << "Wrote images to files" << endl;
00948     cvReleaseImage(&image);
00949     cvReleaseImage(&undistort);
00950   }
00951   cout << "Wrote backprojected images to files" << endl;
00952 
00953   if (darttest) {
00954     // attach Dart client XML data for submission to dart server (JW):
00955     cout<<endl
00956         <<"<DartMeasurementFile name=\"chess7x4_003_"
00957         <<"findChessBoardCornerGuesses.jpg\" type=\"image/jpeg\">"
00958         <<"chess7x4_003_findChessBoardCornerGuesses.jpg"
00959         <<"</DartMeasurementFile>"<<endl
00960         <<"<DartMeasurementFile name=\"chess7x4_003_backproject_"
00961         <<"undistorted.jpg\" type=\"image/jpeg\">chess7x4_003_backproject_"
00962         <<"undistorted.jpg</DartMeasurementFile>"<<endl;
00963   }
00964 
00965   //// setup xmls camera file (grest)
00966   ofstream outXML("out_cameraParam.xml");
00967   outXML<<"<?xml version=\"1.0\" encoding=\"ISO-8859-1\"?>"<<endl;
00968   outXML<<"<!-- File created from MIP/calibrateIntrinsics with K-Matrix"<<endl;
00969   for (unsigned int i=0;i<3; i++) {
00970     for (unsigned int j=0;j<3; j++) {
00971       outXML<<setw(DIGW)<<setprecision(DIGPREC)<<Kbi[i][j]<<" ";
00972     }
00973     outXML<<endl;
00974   }
00975   outXML<<"-->"<<endl;
00976   outXML<<"  <camera version=\"2.0\">"<<endl;
00977   outXML<<"<!--\"Model\" is an arbitrary camera name. "<<endl;
00978   outXML<<"  \"SerialID\" is i.e. the camera firewire-bus ID."<<endl;
00979   outXML<<"  \"ImageWidth\" and \"ImageHeight\" are in pixel."<<endl;
00980   outXML<<" \"CellSizeX\" is the size of one CCD-cell in meter."<<endl;
00981   outXML<<" \"AspectRatio\" is CellsizeX/CellSizeY "
00982         <<"(same as FocalLengthY/FocalLengthX).-->"<<endl;
00983   outXML<<" <Sensor AspectRatio=\""
00984         <<setprecision(10)<<Kbi[1][1]/Kbi[0][0]<<"\""<<endl;
00985   outXML<<" CellSizeX=\"1\" Model=\""<<FileHandling::Basename(imgFilename)
00986         <<"\" "<<endl;
00987   outXML<<" ImageHeight=\""<<height<<"\"  ImageWidth=\""<<width<<"\""<<endl;
00988   outXML<<"  SerialID=\"666\" />"<<endl;
00989   outXML<<" <!--\"Model\" is an arbitrary name. \"Type\" can be either "
00990         <<"\"Spherical\" or \"Perspective\"-->  "<<endl;
00991   outXML<<" <Lens Model=\"not specified\" Type=\"Perspective\" > "<<endl;
00992   outXML<<"  <PerspectiveCalib>"<<endl;
00993   outXML<<" <!--\"FocalLength\" is FocalLengthX in meters, "
00994         <<"if you want to give it in pixel set \"CellSizeX\"=1."<<endl;
00995   outXML<<" k1 and k2 are radial distortion; p1, p2 are "
00996         <<" tangential distortion. -->"<<endl;
00997   outXML<<" <Default p1=\"0\" p2=\"0\" k1=\""
00998         <<setprecision(10)<<distortion[0]<<"\" k2=\""<< setprecision(10)
00999         <<distortion[1]<<"\" FocalLength=\""<<Kbi[0][0]<<"\" />"<<endl;
01000   outXML<<"   </PerspectiveCalib>"<<endl;
01001   outXML<<"   <VignetteCalib>"<<endl;
01002   outXML<<"    <!--The measurements for vignette calibration relate the "
01003         <<"distance of a pixel from the image principal point to an absorbtion "
01004         <<"factor for the corresponding image pixels.-->   "<<endl;
01005   outXML<<"   </VignetteCalib>"<<endl;
01006   outXML<<"  </Lens>"<<endl;
01007   outXML<<" <!--The \"PrincipalPoint\" is given as a fraction of "
01008         <<"\"ImageWidth\"/\"ImageHeight\" with (0.5,0.5) is the image center.-->"
01009         <<endl;
01010   outXML<<"   <PrincipalPoint X=\""
01011         <<setprecision(10)<<Kbi[0][2]/(double)(width-1)<<"\" "<<endl;
01012   outXML<<"        Y=\""
01013         <<setprecision(10)<<Kbi[1][2]/(double)(height-1)<<"\" />"<<endl;
01014   outXML<<"  </camera>"<<endl;
01015   outXML.close();
01016 
01017   cout<<"MIP camera parameter file written to out_cameraParam.xml" << endl;
01018 
01019   //create and write projection xml-file
01020   BIAS::Projection proj;
01021   BIAS::Pose pose;
01022   Quaternion<double> quat;
01023   Rbi.GetQuaternion(quat);
01024   pose.Set(quat,Cbi);
01025   std::vector<double> distortionDouble(4,0.0);
01026   distortionDouble[0] = distortion[0];
01027   distortionDouble[1] = distortion[1];
01028   distortionDouble[2] = distortion[2];
01029   distortionDouble[3] = distortion[3];
01030   BIAS_ProjParaPersp_DISTORTION_TYPE radDistType=DISTYPE_DEF;
01031   proj.CreatePerspective(pose,Kbi,width,height,radDistType,distortionDouble);
01032 
01033   //  Create tree and base node for XML-file...
01034 #ifdef BIAS_HAVE_XML2
01035   XMLIO tree;
01036   xmlNodePtr root = tree.create("Projection");
01037   tree.addAttribute(root, "Version", 0.1);
01038 
01039   //  ...write data to XML-tree...
01040   proj.XMLOut(root,tree);
01041 
01042   //  ...if XML-file has been written...
01043   if (tree.write("out_projParam.xml") == 0){
01044     cout << "MIP projection parameter file written to out_projParam.xml" << endl;
01045     // ...clear tree
01046     tree.clear();
01047   } else {
01048     cout << "Failed to write MIP projection parameter file to "
01049          << "out_projParam.xml!" << endl;
01050   }
01051 #endif
01052 
01053   if (usegui) cvDestroyAllWindows();
01054   delete cornersArray;
01055   delete corners3d;
01056   delete cornersFound;
01057   return 0;
01058 }
 All Classes Functions Variables Typedefs Enumerations Enumerator Friends