biascalibratecamera.cpp

/*
   This file is part of the BIAS library (Basic ImageAlgorithmS).

   Copyright (C) 2003-2009    (see file CONTACT for details)
   Multimediale Systeme der Informationsverarbeitung
   Institut fuer Informatik
   Christian-Albrechts-Universitaet Kiel


   BIAS is free software; you can redistribute it and/or modify
   it under the terms of the GNU Lesser General Public License as published by
   the Free Software Foundation; either version 2.1 of the License, or
   (at your option) any later version.

   BIAS is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU Lesser General Public License for more details.

   You should have received a copy of the GNU Lesser General Public License
   along with BIAS; if not, write to the Free Software
   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/


#include <bias_config.h>
#ifndef BIAS_HAVE_OPENCV
#  error You need to enable OPENCV to compile this file. Please reconfigure MIP with USE_OPENCV (jw)
#endif

#include <Base/Common/W32Compat.hh>
#include <math.h>

#include <cv.h>
#include <highgui.h>

#ifndef CV_CALIB_ZERO_TANGENT_DIST
#  error Your OpenCV version is probably too old because CV_CALIB_ZERO_TANGENT_DIST seems to be unsupported. Try again with OpenCV version >=beta5a
#endif

#define DRAW_IDEAL_OVERLAY
#define DRAW_POINT_LABEL
#define DRAW_AXES

// write VRML model of P matrices using BIAS::ThreeDOut?
#define WRITE_VRML

// output formatting
#define DIGW 12
#define DIGPREC 5

#include <string>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <sstream>

#ifdef WRITE_VRML
#  include "Utils/ThreeDOut.hh"
#endif // WRITE_VRML

// BIAS
#include <Base/Debug/Error.hh>
#include <Base/Common/FileHandling.hh>
#include <Base/Math/Vector3.hh>
#include <Base/Image/ImageIO.hh>
#include <Base/Image/ImageConvert.hh>
#include <Geometry/RMatrix.hh>
#include <Geometry/PMatrix.hh>
#include <Geometry/Pose.hh>
#include <Utils/Param.hh>

using namespace BIAS;
using namespace std;

/**
   @file
   @ingroup g_tools
   @brief The program reads the images from disk,detects chessboard of given
   known dimension, and computes internal calibration parameters (and extrinsics
   for this purpose, too), see biascalibratecamera.cpp
   @author Jan Woetzel, University of Kiel, 03/2005, 09/2005
           additional features by grest
*/

/*
  Usage:
   calibrateCamera  <chessboard_corners_per_row> <chessboard_corners_per_column>
   <chessboard_images> ...
   The program reads the images from disk,
   detects chessboard of given known dimension,
   and computes internal calibration parameters (and extrinsics for this
   purpose, too).

   Then undistorts and writes the results:
   - the distortion coefficient following the Bouguet model,
   - the camera calibration "K",
   - the camera image overlayed with the corners found,
   - the corners backprojected using their 3D scene corrdinates,
   - the extrinsic parameters R, C for each image
   - the backprojected corners in undistorted images.

   Based on OpenCV implementation of Bouguet calib toolbox and some sample code
   from Stanford University "cs223b" course notes.

   You may find "grabGui" and its "FindChessboard" mode useful for live
   capturing only images where the complete chessboard was clearly detected.

   Please note the different notation for extrinsics between Cv and BIAS:
   R_bias =      R_cv.transposed
   C_bias = -1 * R_cv.transpose * C_cv
*/

BIAS::Param params;

/* Backprojection from world to image coordinates
   @todo change to BIAS::PMatrix multiplication JW */
CvPoint2D32f * ConvertWorldToPixel(CvPoint3D32f *pts3d,
                                   int numImages, int *numPts,
                                   CvMatr32f cam, CvVect32f t, CvMatr32f r,
                                   int xCorners, int yCorners)
{
  int i=0, j=0, k=0;
  CvPoint2D32f *pts2d = new CvPoint2D32f[numImages * xCorners * yCorners];

  CvMat *C        = cvCreateMat(3, 3, CV_32FC1);
  CvMat *point3D  = cvCreateMat(3, 1, CV_32FC1);
  CvMat *R        = cvCreateMat(3, 3, CV_32FC1);
  CvMat *T        = cvCreateMat(3, 1, CV_32FC1);

  CvPoint3D32f *pts3dCur = pts3d;
  CvPoint2D32f *pts2dCur = pts2d;

  for (i = 0; i < 3; i++) {
    for (j = 0; j < 3; j++) {
      CV_MAT_ELEM(*C, float, i, j) = cam[3 * i + j];
    }
  }

  for (k = 0; k < numImages; k++) {
    for (j = 0; j < 9; j++) {
      if (j < 3) CV_MAT_ELEM(*T, float, j, 0) = t[k*3 + j];
      CV_MAT_ELEM(*R, float, j / 3, j%3) = r[k*9 + j];
    }
    for (i = 0; i < numPts[k]; i++) {
      CV_MAT_ELEM(*point3D, float, 0, 0) = pts3dCur[i].x;
      CV_MAT_ELEM(*point3D, float, 1, 0) = pts3dCur[i].y;
      CV_MAT_ELEM(*point3D, float, 2, 0) = pts3dCur[i].z;

      cvMatMulAdd(R, point3D, T, point3D); //rotate and translate
      cvMatMul(C, point3D, point3D);       //camera

      pts2dCur[i].x = CV_MAT_ELEM(*point3D, float, 0, 0) /
                      CV_MAT_ELEM(*point3D, float, 2, 0);
      pts2dCur[i].y = CV_MAT_ELEM(*point3D, float, 1, 0) /
                      CV_MAT_ELEM(*point3D, float, 2, 0);
    }
    pts3dCur += numPts[k];
    pts2dCur += numPts[k];
  }

  cvReleaseMat(&point3D);
  cvReleaseMat(&T);
  cvReleaseMat(&R);

  return pts2d;
}


/* DrawPoints takes number of corners on x axis and y axis,
   and number of images to builds an array of corners.
   Assumes all corners on z = 0 plane.
   @todo Move to BIAS/FeatureDetector/CheckerboardDetector JW
*/
void DrawPoints(CvArr *img, CvPoint2D32f *ptArray, int numPts,
                const int xCorners, const int yCorners)
{
  CvPoint pt;
  CvPoint ptOld;
#ifdef DRAW_POINT_LABEL
  CvFont font;
  cvInitFont(&font, CV_FONT_HERSHEY_PLAIN, 1.0f, 1.0f);
#endif //DRAW_POINT_LABEL

  // init old as first
  if (numPts > 0) {
    ptOld.x = (int)ptArray[0].x;
    ptOld.y = (int)ptArray[0].y;
  }
  for (int i = 0; i < numPts; i++) {
    int clr = (int)((float)i / numPts * 255.0);
    pt.x = (int)ptArray[i].x;
    pt.y = (int)ptArray[i].y;
    cvCircle(img, pt, 3, CV_RGB(255-clr,0,clr), CV_FILLED);

#ifdef DRAW_POINT_LABEL
    // label the point
    stringstream ssID;
    ssID << i;
    cvPutText(img, ssID.str().c_str(), pt, &font, CV_RGB(0,0,255));
#endif //DRAW_POINT_LABEL
    // next
    ptOld=pt;
  }

#ifdef DRAW_IDEAL_OVERLAY
  // mark ideal pattern
  // horizontal lines
  for (int xx = 0; xx < xCorners*yCorners; xx+=xCorners) {
    cvLine(img, cvPointFrom32f(ptArray[xx]),
           cvPointFrom32f(ptArray[xx+xCorners-1]),
           CV_RGB(0,0,255), 1, CV_AA);
  }
  // vertical lines
  for (int yy = 0; yy < xCorners; yy+=1) {
    cvLine(img, cvPointFrom32f(ptArray[yy]),
           cvPointFrom32f(ptArray[yy+xCorners*(yCorners-1)]),
           CV_RGB(0,0,255), 1, CV_AA);
  }
#endif

#ifdef DRAW_AXES
  CvFont fontBig;
  cvInitFont(&fontBig, CV_FONT_HERSHEY_PLAIN, 5.0f, 5.0f, 0, 2, CV_AA);
  cvPutText(img, "0", cvPointFrom32f(ptArray[0]), &fontBig, CV_RGB(0,0,255));
  cvLine(img, cvPointFrom32f(ptArray[0]), cvPointFrom32f(ptArray[xCorners-1]),
         CV_RGB(255,0,0), 1, CV_AA);
  cvPutText(img, "X", cvPointFrom32f(ptArray[xCorners-1]),
            &fontBig, CV_RGB(255,0,0));
  cvLine(img, cvPointFrom32f(ptArray[0]),
         cvPointFrom32f(ptArray[xCorners*(yCorners-1)]),
         CV_RGB(0,255,0), 1, CV_AA);
  cvPutText(img, "Y", cvPointFrom32f(ptArray[xCorners*(yCorners-1)]),
            &fontBig, CV_RGB(0,255,0));
#endif //DRAW_AXES
}

/* InitCorners3d
   Takes number of corners on x axis and y axis + number of images
   Builds a regular checkerboard array of corners.
   Assumes all corners on z = 0 plane.
*/
void InitCorners3d(CvPoint3D32f *corners3d, int xCorners,
                   int yCorners, int numImages)
{
  int n=0, x=0, y=0;
  int nStride = xCorners * yCorners;
  for (n = 0; n < numImages; n++) {
    for (x = 0; x < xCorners; x++) {
      for (y = 0; y < yCorners; y++) {
        corners3d[n*nStride + x +xCorners*y].x = (float)x;
        corners3d[n*nStride + x +xCorners*y].y = (float)y;
        corners3d[n*nStride + x +xCorners*y].z = 0.0f;
      }
    }
  }
}

/* InitCorners3dMatrix
   Takes number of corners on x axis and y axis + number of images
   Builds a regular checkerboard matrix of corners.
   Assumes all corners on z = 0 plane.
*/
void InitCorners3dMatrix(CvMat *corners3dMat,
                         int xCorners, int yCorners, int numImages)
{
  int n=0, x=0, y=0;
  int offset=xCorners * yCorners;
  for (n = 0; n < numImages; n++) {
    for (x = 0; x < xCorners; x++) {
      for (y = 0; y < yCorners; y++) {
        CV_MAT_ELEM(*corners3dMat,float,n*offset+x+ y*xCorners, 0) = (float)x;
        CV_MAT_ELEM(*corners3dMat,float,n*offset+x+ y*xCorners, 1) = (float)y;
        CV_MAT_ELEM(*corners3dMat,float,n*offset+x+ y*xCorners, 2) = 0.0f;
      }
    }
  }
}


void InitCorners2dPointMatrix(CvMat *points2dMat,CvPoint2D32f *cornersArray,
                              CvMat *pointCounts, int* cornersFound,
                              int numImages)
{
  int n=0, x=0, row=0;
  for (n = 0; n < numImages; n++) {
    int corners=cornersFound[n];
    CV_MAT_ELEM(*pointCounts,int,n,0)=corners;
    for (x = 0; x < corners; x++) {
      CV_MAT_ELEM(*points2dMat,float,row+x,0) = cornersArray[row+x].x;
      CV_MAT_ELEM(*points2dMat,float,row+x,1) = cornersArray[row+x].y;
    }
    row+=corners;
  }
}

/* return BIAS::Vector3 structure from float array */
BIAS::Vector3<double> GetCfromArr(const unsigned int camIndex,
                                  const float * translation_vectors,
                                  const unsigned int numImages)
{
  BIASASSERT(translation_vectors!=NULL);
  BIASASSERT(camIndex<numImages);
  BIAS::Vector3<double> C;
  for (unsigned int k=0; k<3; k++)
    C.GetData()[k] = translation_vectors[camIndex*3 +k];
  return C;
}

/* return BIAS::RMatrix structure from float array */
BIAS::RMatrix GetRfromArr(const unsigned int camIndex,
                          const float * rotation_matrices,
                          const unsigned int numImages)
{
  BIASASSERT(rotation_matrices!=NULL);
  BIASASSERT(camIndex<numImages);
  BIAS::RMatrix R;
  for (unsigned int k=0; k<9; k++)
    R.GetData()[k] = rotation_matrices[camIndex*9 +k];
  return R;
}

/* return BIAS::KMatrix structure from float array */
BIAS::KMatrix GetKfromArr(const float * camera_matrix)
{
  BIASASSERT(camera_matrix!=NULL);
  BIAS::KMatrix K;
  for (unsigned int i=0; i<9; i++)
    K.GetData()[i] = camera_matrix[i];
  return K;
}

/* return BIAS::PMatrix structure from float array */
BIAS::PMatrix GetPfromArr(const unsigned int camIndex,
                          const float * rotation_matrices,
                          const float * translation_vectors,
                          const unsigned int numImages)
{
  BIAS::PMatrix P;
  P.SetIdentity();
  BIASASSERT(rotation_matrices!=NULL);
  BIASASSERT(translation_vectors!=NULL);
  BIASASSERT(camIndex<numImages);
  return P;
}

/* formatted output of intrinsics */
void PrintIntrinsics(CvVect32f distortion,
                     CvMatr32f camera_matrix,
                     std::ostream & os)
{
  BIASASSERT(distortion!=NULL);
  os << "Distortion coefficients:" << endl
     << "[0] radial     "
     << setw(DIGW)<<setprecision(DIGPREC)<<distortion[0]<<endl
     << "[1] radial     "
     << setw(DIGW)<<setprecision(DIGPREC)<<distortion[1]<<endl
     << "[2] tangential "
     << setw(DIGW)<<setprecision(DIGPREC)<<distortion[2]<<endl
     << "[3] tangential "
     << setw(DIGW)<<setprecision(DIGPREC)<<distortion[3]<<endl;

  BIASASSERT(camera_matrix != NULL);
  os << "Camera matrix K:" << endl;
  //PrintMatrix(camera_matrix, 3, 3, os);
  os << "  " << GetKfromArr(camera_matrix) << endl;
}

/* BIAS::Vector4 compatible output for undistortion call */
void PrintDistortionVec4(CvVect32f distortion,
                         std::ostream & os)
{
  BIASASSERT(distortion!=NULL);
  os << "4" << endl
     << setw(DIGW)<<setprecision(DIGPREC)<<distortion[0]<<endl
     << setw(DIGW)<<setprecision(DIGPREC)<<distortion[1]<<endl
     << setw(DIGW)<<setprecision(DIGPREC)<<distortion[2]<<endl
     << setw(DIGW)<<setprecision(DIGPREC)<<distortion[3]<<endl;
}

/* convert rotation matrix from OpenCV notation to BIAS notation JW */
BIAS::RMatrix R_cv2bias(const BIAS::RMatrix & Rcv)
{
  BIAS::RMatrix Rbias = Rcv.Transpose();
  return Rbias;
}

/* convert translation vector from OpenCV notation to BIAS notation JW */
BIAS::Vector3<double> C_cv2bias(const BIAS::Vector3<double> & Ccv,
                                const BIAS::RMatrix & Rcv )
{
  BIAS::Vector3<double> Cbias = -1.0 * (Rcv.Transpose() * Ccv);
  return Cbias;
}

/* print formatted extrinsic parameters JW */
void PrintExtrinsics(const float * translation_vectors,
                     const float * rotation_matrices,
                     const unsigned int numImages,
                     std::ostream & os)
{
  BIASASSERT(translation_vectors != NULL);
  BIASASSERT(rotation_matrices != NULL);

  BIAS::RMatrix tmpRcv, Rbias;
  BIAS::Vector3<double> tmpCcv, Cbias;
  for (unsigned int i = 0; i < numImages; i++) {
    tmpRcv = GetRfromArr(i, rotation_matrices,   numImages);
    tmpCcv = GetCfromArr(i, translation_vectors, numImages);
    Rbias = R_cv2bias(tmpRcv);
    Cbias = C_cv2bias(tmpCcv, tmpRcv);
    os << "[" << i << "] R = " << Rbias << "\tC = " << Cbias << endl;
  }
}

/* print usage information for application */
void usage(int argc, char** argv)
{
  cout<<"Usage: calibrateCamera <inner_chessboard_corners_X> "
      <<"<inner_chessboard_cornersY> [all-image-filenames]"<<endl
      <<"  with at least 2 inner chessboard corners and "
      <<"*non-symmetric* chessboard pattern. "<<endl
      <<"  In particular 8x8 is *NOT* OK while 8x7 is good."<<endl
      <<endl
      <<"Sample usage:"<<endl
      <<"  calibrateCamera 7 4  a.jpg b.jpg c.jpg d.jpg e.jpg "<<endl
      <<"or special dart test modus:"<<endl
      <<"  calibrateCamera --darttest [0|1] a.jpg b.jpg c.jpg d.jpg e.jpg ..."<<endl
      <<endl
      <<"(c) MIP University of Kiel"<<endl
      <<"Jan Woetzel (www.mip.informatik.uni-kiel.de/~jw)"<<endl
      <<"feel free to ask me fo details."<<endl;
  params.Usage();
}


/* MAIN APPLICATION */
int main(int argc, char** argv)
{
  if (argc > 0) {
    cout<<"Started "<<argv[0]<<"\n(c) MIP University of Kiel, Jan Woetzel"<<endl;
  }
  IplImage *image = NULL;
  IplImage *undistort = NULL;
  IplImage *gray = NULL;
  CvPoint2D32f *cornersArray = NULL;
  int numImages=0;
  CvPoint3D32f *corners3d = NULL;
  CvVect32f translation_vectors = NULL;
  CvMatr32f rotation_matrices = NULL;

  bool usegui=true;


  params.DisableDestructorWarning();
  params.AddParamBool("fixedAspect","focal length is euqal in both directions",
                      false,'a');
  params.AddParamBool("fixedPrincipal",
                      "principal point is fixed in image center",
                      false,'p');
  params.AddParamBool("fixedAll","only calibrate extrinsic parameters, "
                      "KMatrix must be given, distortion is optional",
                      false,'f');
  params.AddParamString("kmatrix","file name of K-matrix with inital values,"
                        " which are unchanged if fixedAll","",'K');
  params.AddParamString("distortion","file name of vector with distortion "
                        " values (radial k1, k2, tangential p1,p2)",
                        "",'D');
  params.AddParamString("darttest","perform dart test");
  params.SetEnhancedFlag("darttest",true);
  //params.SetHiddenFlag("darttest",true);
  int nextParam=params.ParseCommandLine(argc,argv);
  BIASASSERT(nextParam >= 0);

  BIASASSERT(params.GetParamString("darttest") != NULL);
  if ((argc-nextParam<3) && (params.GetParamString("darttest")->size()==0)) {
    usage(argc, argv);
    return -1;
  }

  BIAS::Vector4<float>   distortion;
  distortion.SetZero();
  BIAS::Matrix3x3<float> camera_matrix;
  camera_matrix.SetIdentity();
  int flags = 0;
  flags |= CV_CALIB_ZERO_TANGENT_DIST; // no tangential distortion

  if (params.GetParamString("kmatrix")->length() > 0) {
    ifstream in(params.GetParamString("kmatrix")->c_str());
    if (!in) {
      cerr << "Error opening KMatrix file "
           << *params.GetParamString("kmatrix") << endl;
      return -1;
    }
    in >> camera_matrix;
    flags |= CV_CALIB_USE_INTRINSIC_GUESS;
  }
  if (*params.GetParamBool("fixedAspect"))
    flags |= CV_CALIB_FIX_ASPECT_RATIO;
  if (*params.GetParamBool("fixedPrincipal"))
    flags |= CV_CALIB_FIX_PRINCIPAL_POINT;

  BIAS::Vector4<float> biasDistortion(0, 0, 0, 0);
  if (params.GetParamString("distortion")->length() > 0) {
    ifstream in(params.GetParamString("distortion")->c_str());
    if (!in) {
      cerr << "Error opening distortion file "
           << *params.GetParamString("distortion") << endl;
      return -1;
    }
    in >> biasDistortion;
    cout << "User given distortion values:" << biasDistortion << endl;
  }

  int xCorners = 0;
  int yCorners = 0;
  int iImg = 0;
  int width = 0;
  int height = 0;

  // HACK Dart Testing (JW)
  bool darttest = false;
  if (argc >= 2) {
    if (strcmp(argv[1],"--darttest") == 0) {
      std::cout << "Started dart test for " << argv[0] << std::endl;
      xCorners = 7; // HACK fixed size for darttest
      yCorners = 4;
      darttest = true;
      if (argv[2][0] == '1') {
        usegui = true;
      } else {
        usegui = false;
      }
      //nextParam=0;
    } else {
      // parse cmd line
      xCorners = atoi(argv[nextParam]);
      nextParam++;
      if (xCorners < 1)
        cout<<"[ERROR] xCorners = "<<xCorners<<" is invalid!"<<endl;
      yCorners = atoi(argv[nextParam]);
      nextParam++;
      if (yCorners<1)
        cout<<"[ERROR] yCorners = "<<yCorners<<" is invalid!"<<endl;
    }
  }

  // atoi return 0 on error, non numeric.
  if ((xCorners<=2) || (yCorners<=2)) {
    usage(argc, argv);
    return -2;
  }

  // the rest are the input images
  numImages = argc - nextParam;

  // allocate:
  cornersArray = new CvPoint2D32f[xCorners*yCorners*numImages];
  corners3d = new CvPoint3D32f[xCorners*yCorners*numImages];
  int *cornersFound   = new int[numImages];
  translation_vectors = new float[3*numImages];
  rotation_matrices   = new float[9*numImages];
  InitCorners3d(corners3d, xCorners, yCorners, numImages);

  // grest stuff
  CvMat *corners3dMat = cvCreateMat(xCorners*yCorners*numImages,3, CV_32FC1);
  InitCorners3dMatrix(corners3dMat, xCorners, yCorners, numImages);

  if (usegui) {
    cvNamedWindow("Image", CV_WINDOW_AUTOSIZE);
    cvMoveWindow("Image", 0,0);
  }
  CvPoint2D32f *nextCornersArray = cornersArray;
  string imgFilename;
  string outFilename;
  string outFilenameVRML("out_allP.wrl");

#ifdef WRITE_VRML
  ThreeDOutParameters threeDOutParams;
  ThreeDOut CameraVRML(threeDOutParams);
#endif // WRITE_VRML

  BIAS::Image<unsigned char> biasImg;
  for (iImg = 0; iImg < numImages; iImg++) {
    BIASASSERT((iImg+nextParam)<argc);
    imgFilename = argv[iImg+nextParam];
    // Load all the new images into their data structures
    cout << "Loading " << imgFilename << endl;
    if (ImageIO::Load( imgFilename.c_str(), biasImg)!=0) {
      BIASERR("BIAS::ImageIO failed to load image " << imgFilename
              << ". Now trying OpenCV routines...");
      image = cvLoadImage(imgFilename.c_str());
      if (image == NULL){
        BIASERR("OpenCV failed to load " << imgFilename << " either. Aborting!");
        BIASBREAK;
        BIASABORT;
      }
    } else {
#ifdef BIAS_HAVE_OPENCV
      // conversion required fromBIAS to cv image
      ImageConvert::BIAS2ipl(biasImg, image);
#else
      BIASERR("case not implemented.");
#endif
    }
    BIASASSERT(image!=NULL); // loaded?
    width = image->width;
    height = image->height;

    if (image->depth != IPL_DEPTH_8U) {
      cerr << "[ERROR] Depth is not IPL_DEPTH_8U for image "
           << imgFilename <<"!" << endl;
      return -2;
    }

    // Convert image to grayscale if it is not already
    if (image->nChannels == 1 && image->depth == IPL_DEPTH_8U) {
      gray = cvCloneImage(image);
    } else {
      gray = cvCreateImage(cvSize(image->width, image->height),
                           IPL_DEPTH_8U, 1);
      BIASASSERT(gray!=NULL);
      cvConvertImage(image, gray);
    }

    // Feedback of images loaded for sanity
    if (usegui)
      cvShowImage("Image", image);

    // Start calibration of points
    cornersFound[iImg] = xCorners * yCorners;

    // nextCornersArray is a pointer into cornersArray to the last slot open
    // this will overflow if cvchessboard ever gives us more corners then expected

/*
    // deprecated call
    IplImage *threshold = cvCreateImage(cvSize(image->width, image->height),
                                        IPL_DEPTH_8U, 1);
    BIASASSERT(threshold != NULL);
    int res = cvFindChessBoardCornerGuesses(gray, threshold, NULL,
                                            cvSize(xCorners, yCorners),
                                            nextCornersArray,
                                            &cornersFound[iImg]);
    cvReleaseImage(&threshold);
*/

    int res = cvFindChessboardCorners(gray, cvSize(xCorners, yCorners),
                                      nextCornersArray,
                                      &cornersFound[iImg],
                                      CV_CALIB_CB_ADAPTIVE_THRESH);
                                      //CV_CALIB_CB_NORMALIZE_IMAGE);
                                      //CV_CALIB_CB_FILTER_QUADS);

    cvFindCornerSubPix(gray, nextCornersArray,
                       cornersFound[iImg], cvSize(5,5), cvSize(-1,-1),
                       cvTermCriteria(CV_TERMCRIT_ITER, 100, 0.1));

    // more visual feedback to confirm correctness
    DrawPoints(image, nextCornersArray, cornersFound[iImg], xCorners, yCorners);
    if (usegui) {
      cvShowImage("Image", image);
      cvWaitKey(1);
    }

    // save debug image
    outFilename = FileHandling::Basename(imgFilename) +"_findChessBoardCornerGuesses.jpg";
    cvSaveImage(outFilename.c_str(), image);

    if (!res) {
      cerr<<"[ERROR] Could not find all "<<xCorners<<"x"<<yCorners
          <<" inner corners of checkerboard in image "<<imgFilename << endl
          <<"        but actually checkerboard has to be detectable"
          <<" in *all* images!"<<endl<<"        Please remove image "
          <<imgFilename<<" and try again!"<<endl;
      cerr<<"Press any key in GUI window."<<endl;
      cvWaitKey(0);
      return -3;
    }
    // update so that nextCornersArray points to the next open slot in cornersArray
    nextCornersArray += cornersFound[iImg];

    // required to update display:
    cvWaitKey(1);

    cvReleaseImage(&image);
    cvReleaseImage(&gray);
  }
  if (usegui) cvDestroyWindow("Image");

  cout<<"Solving calibration... "<<flush;

/*
  // run calibration routine on pre-detecteed coordinates
  cvCalibrateCamera(numImages,
                    cornersFound, //point count
                    cvSize(width, height), // image size
                    cornersArray, // image points
                    corners3d, // object points
                    distortion.GetData(), // coefficients
                    camera_matrix.GetData(),
                    translation_vectors,
                    rotation_matrices,
                    FLAGS);
*/

  int totalpoints = 0;
  for (int i = 0;i < numImages; i++) {
    totalpoints += cornersFound[i];
  }
  CvMat *points2dMat = cvCreateMat(totalpoints, 2, CV_32FC1);
  CvMat *pointCountMat = cvCreateMat(numImages, 1, CV_32SC1);
  InitCorners2dPointMatrix(points2dMat, cornersArray, pointCountMat,
                           cornersFound, numImages);
  CvMat *camMatrix = cvCreateMat(3,3,CV_32FC1);
  if (params.GetParamString("kmatrix")->length() > 0) {
    cvInitMatHeader(camMatrix,3,3,CV_32FC1,  camera_matrix.GetData());
  } else {
    cvmSetZero(camMatrix);
    CV_MAT_ELEM(*camMatrix,float,0,0) = 600.;
    CV_MAT_ELEM(*camMatrix,float,1,1) = 600.;
    CV_MAT_ELEM(*camMatrix,float,0,2) = float(width/2.);
    CV_MAT_ELEM(*camMatrix,float,1,2) = float(height/2.);
    CV_MAT_ELEM(*camMatrix,float,2,2) = 1.;
  }

  CvMat *distortMatrix = cvCreateMat(4,1,CV_32FC1);
  if (params.GetParamString("distortion")->length() > 0) {
    cvInitMatHeader(distortMatrix,4,1,CV_32FC1,  biasDistortion.GetData());
  } else {
    cvmSetZero(distortMatrix);
  }

  CvMat *rotVectors = cvCreateMat(numImages,3,CV_32FC1);
  CvMat *transVectors = cvCreateMat(numImages,3,CV_32FC1);

  if (*params.GetParamBool("fixedAll")) {
    CvMat *rotVector = cvCreateMat(1,3,CV_32FC1);
    CvMat *transVector = cvCreateMat(1,3,CV_32FC1);
    int nextPoint=0;
    for (int n=0; n<numImages; n++) {
      // build up temporary 3d and 2d points
      int nrPoints =  CV_MAT_ELEM(*pointCountMat,int,n,0);
      CvMat *points3DSingleView = cvCreateMat(nrPoints,3,CV_32FC1);
      CvMat *points2DSingleView = cvCreateMat(nrPoints,2,CV_32FC1);
      for (int p=0; p < nrPoints; p++) {
        int r;
        for (r=0; r < 2; r++) {
          CV_MAT_ELEM(*points2DSingleView,float,p,r) =
            CV_MAT_ELEM(*points2dMat,float,nextPoint+p,r);
        }
        for (r=0; r < 3; r++) {
          CV_MAT_ELEM(*points3DSingleView,float,p,r) =
            CV_MAT_ELEM(*corners3dMat,float,nextPoint+p,r);
        }
      }
      nextPoint+=nrPoints;
      cvFindExtrinsicCameraParams2(//corners3dMat, points2dMat,
                                   points3DSingleView, points2DSingleView,
                                   camMatrix, distortMatrix,
                                   rotVector, transVector);
      for (int r=0; r < 3; r++) {
        CV_MAT_ELEM(*rotVectors,float,n,r) =
          CV_MAT_ELEM(*rotVector,float,0,r);
        CV_MAT_ELEM(*transVectors,float,n,r) =
          CV_MAT_ELEM(*transVector,float,0,r);
      }
    }
  } else {
    cvCalibrateCamera2(corners3dMat, points2dMat,
                       pointCountMat, cvSize(width, height),
                       camMatrix, distortMatrix,
                       rotVectors, transVectors, flags);
  }

  for (unsigned int i=0;i<3;i++) {
    for (unsigned int j=0;j<3;j++) {
      camera_matrix[i][j]= CV_MAT_ELEM(*camMatrix,float,i,j);
    }
  }
  memcpy(distortion.GetData(),distortMatrix->data.fl,sizeof(float)*4);
  CvMat *dummyRot= cvCreateMat(1,3,CV_32FC1);
  CvMat *dummyRes= cvCreateMat(3,3,CV_32FC1);
  for (int i=0; i<numImages; i++) {
    dummyRot->data.fl= &(rotVectors->data.fl[i*3]);
    cvRodrigues2( dummyRot, dummyRes);
    memcpy(&rotation_matrices[i*9],dummyRes->data.fl,sizeof(float)*9);
    translation_vectors= transVectors->data.fl;
  }
  cout<<" done."<<endl;

  // output result
  PrintExtrinsics(translation_vectors, rotation_matrices, numImages,std::cout);
  {
    ofstream ofs("out_extrinsics.txt");
    PrintExtrinsics(translation_vectors, rotation_matrices, numImages,  ofs);
    cout<<"Wrote result to file out_extrinsics.txt"<<endl;
    ofs.close();
  }
  PrintIntrinsics(distortion.GetData(),
                  camera_matrix.GetData(),
                  std::cout);
  {
    ofstream ofs("out_intrinsics.txt");
    PrintIntrinsics(distortion.GetData(),
                    camera_matrix.GetData(),
                    ofs);
    cout<<"Wrote result to file out_intrinsics.txt"<<endl;
    ofs.close();
  }
  {
    // save in BIAS Vector4 format, too.
    ofstream ofs("out_distortion.txt");
    PrintDistortionVec4(distortion.GetData(), ofs);
    cout<<"Wrote result to file out_intrinsics.txt"<<endl;
    ofs.close();
  }


  // display indivisual resulta to disk (same notation for intrinsics betwene CV,BIAS
  BIAS::KMatrix Kbi;
  Kbi = GetKfromArr(camera_matrix.GetData());
  Kbi.Save("out_Kmat.K");

  BIAS::RMatrix Rbi;
  BIAS::PMatrix Pbi;
  BIAS::Vector3<double> Cbi;
  BIAS::Vector3<double> lastC;
  lastC.SetZero();
  ofstream ofs;
  for (iImg = 0; iImg < numImages; iImg++) {
    Rbi = R_cv2bias(GetRfromArr(iImg,rotation_matrices, numImages));
    Cbi = C_cv2bias(GetCfromArr(iImg, translation_vectors, numImages),
                    GetRfromArr(iImg,rotation_matrices, numImages));

    // write R for each image
    imgFilename = string("out_")+FileHandling::Basename(string(argv[iImg+nextParam]))+
                  string(".Rmat");
    ofs.open(imgFilename.c_str());
    ofs << Rbi;
    ofs.close();

    // write C for each image
    imgFilename = string("out_")+FileHandling::Basename(string(argv[iImg+nextParam]))+
                  string(".Cmat");
    ofs.open(imgFilename.c_str());
    ofs<<Cbi;
    ofs.close();

    // compose P from K,R,C and write it for each image.
    imgFilename = string("out_")+FileHandling::Basename(string(argv[iImg+nextParam]))+
                  string(".P");
    ofs.open(imgFilename.c_str());
    Pbi=BIAS::PMatrix( Kbi, Rbi, Cbi );
    ofs<<Pbi;
    ofs.close();

#ifdef WRITE_VRML
    if (iImg==0) {
      // first camera
      CameraVRML.AddPMatrix(Pbi, width, height, RGBAuc(0,0,255,0));
      // mark coordinate system:
      CameraVRML.AddLine(lastC, Cbi, RGBAuc(255,255,0,0));
    } else {
      // other cams
      CameraVRML.AddPMatrix(Pbi, width, height, RGBAuc(0,255,0,0));
      CameraVRML.AddLine(lastC, Cbi, RGBAuc(255,0,0,0));
    }
#endif // WRITE_VRML

    // next:
    lastC=Cbi;
  }
  cout<<"Wrote results to out_*.Rmat, out_*.Cmat and out_*.P"<<endl;

#ifdef WRITE_VRML
  const double scale=1.0;
  // mark coordinate system:
  CameraVRML.AddLine(Vector3<double>(0,0,0),
                     scale*Vector3<double>(1,0,0), RGBAuc(255,0,0,0));
  CameraVRML.AddLine(Vector3<double>(0,0,0),
                     scale*Vector3<double>(0,1,0), RGBAuc(0,255,0,0));
  CameraVRML.AddLine(Vector3<double>(0,0,0),
                     scale*Vector3<double>(0,0,1), RGBAuc(0,0,255,0));

  // save to disk:
  CameraVRML.VRMLOut(outFilenameVRML);
  cout<<"wrote vrml \""<<outFilenameVRML<<"\""<<endl;
#endif // WRITE_VRML

  CvPoint2D32f *backprojPts2d =
    ConvertWorldToPixel(corners3d, numImages, cornersFound,
                        camera_matrix.GetData(), translation_vectors,
                        rotation_matrices, xCorners, yCorners);
  // display undistortion result:
  if (usegui) {
    cvNamedWindow("Undistorted", CV_WINDOW_AUTOSIZE);
    cvMoveWindow("Undistorted", 0,0);
    cout<<"Opened GUI"<<endl;
  }

  for (iImg = 0; iImg < numImages; iImg++) {
    // Load all the input images again for undistortion
    imgFilename = argv[iImg+nextParam];
    if (ImageIO::Load(imgFilename.c_str(), biasImg) == 0) {
#ifdef BIAS_HAVE_OPENCV
      ImageConvert::BIAS2ipl(biasImg, image);
#else
      BIASERR("No support to convert image loaded by BIAS into OpenCV format!");
#endif
    } else {
      BIASERR("Failed to load image with BIAS! Trying OpenCV routines, now...");
      image = cvLoadImage(imgFilename.c_str());
      if (image == NULL){
        BIASERR("Failed to load image " << imgFilename
                << "with OpenCV either! Aborting.");
        BIASBREAK;
        BIASABORT;
      }
    }
    cout << "Loaded and converted image " << imgFilename.c_str() << "." << endl;

    // Create undistorted image (grayscale for grayscale input images)
    undistort = cvCreateImage(cvSize(width, height), IPL_DEPTH_8U,
                              image->nChannels);
    cvUndistort2(image, undistort, camMatrix, distortMatrix);

    DrawPoints(image, &backprojPts2d[iImg*cornersFound[iImg]],
                cornersFound[iImg], xCorners,yCorners);
    if (usegui) {
      cvShowImage("Image", image);
      cvWaitKey(1);
    }
    DrawPoints(undistort, &backprojPts2d[iImg*cornersFound[iImg]],
               cornersFound[iImg], xCorners,yCorners);
    if (usegui) {
      cvShowImage("Undistorted", undistort);
      cvWaitKey(1);
    }
    cout << "Drawed checkerboard corners into image" << endl;
    outFilename = FileHandling::Basename(imgFilename) +"_backproject.jpg";
    cvSaveImage(outFilename.c_str(), image);

    outFilename = FileHandling::Basename(imgFilename) +"_backproject_undistorted.jpg";
    cvSaveImage(outFilename.c_str(), undistort);

    cout << "Wrote images to files" << endl;
    cvReleaseImage(&image);
    cvReleaseImage(&undistort);
  }
  cout << "Wrote backprojected images to files" << endl;

  if (darttest) {
    // attach Dart client XML data for submission to dart server (JW):
    cout<<endl
        <<"<DartMeasurementFile name=\"chess7x4_003_"
        <<"findChessBoardCornerGuesses.jpg\" type=\"image/jpeg\">"
        <<"chess7x4_003_findChessBoardCornerGuesses.jpg"
        <<"</DartMeasurementFile>"<<endl
        <<"<DartMeasurementFile name=\"chess7x4_003_backproject_"
        <<"undistorted.jpg\" type=\"image/jpeg\">chess7x4_003_backproject_"
        <<"undistorted.jpg</DartMeasurementFile>"<<endl;
  }

  //// setup xmls camera file (grest)
  ofstream outXML("out_cameraParam.xml");
  outXML<<"<?xml version=\"1.0\" encoding=\"ISO-8859-1\"?>"<<endl;
  outXML<<"<!-- File created from MIP/calibrateIntrinsics with K-Matrix"<<endl;
  for (unsigned int i=0;i<3; i++) {
    for (unsigned int j=0;j<3; j++) {
      outXML<<setw(DIGW)<<setprecision(DIGPREC)<<Kbi[i][j]<<" ";
    }
    outXML<<endl;
  }
  outXML<<"-->"<<endl;
  outXML<<"  <camera version=\"2.0\">"<<endl;
  outXML<<"<!--\"Model\" is an arbitrary camera name. "<<endl;
  outXML<<"  \"SerialID\" is i.e. the camera firewire-bus ID."<<endl;
  outXML<<"  \"ImageWidth\" and \"ImageHeight\" are in pixel."<<endl;
  outXML<<" \"CellSizeX\" is the size of one CCD-cell in meter."<<endl;
  outXML<<" \"AspectRatio\" is CellsizeX/CellSizeY "
        <<"(same as FocalLengthY/FocalLengthX).-->"<<endl;
  outXML<<" <Sensor AspectRatio=\""
        <<setprecision(10)<<Kbi[1][1]/Kbi[0][0]<<"\""<<endl;
  outXML<<" CellSizeX=\"1\" Model=\""<<FileHandling::Basename(imgFilename)
        <<"\" "<<endl;
  outXML<<" ImageHeight=\""<<height<<"\"  ImageWidth=\""<<width<<"\""<<endl;
  outXML<<"  SerialID=\"666\" />"<<endl;
  outXML<<" <!--\"Model\" is an arbitrary name. \"Type\" can be either "
        <<"\"Spherical\" or \"Perspective\"-->  "<<endl;
  outXML<<" <Lens Model=\"not specified\" Type=\"Perspective\" > "<<endl;
  outXML<<"  <PerspectiveCalib>"<<endl;
  outXML<<" <!--\"FocalLength\" is FocalLengthX in meters, "
        <<"if you want to give it in pixel set \"CellSizeX\"=1."<<endl;
  outXML<<" k1 and k2 are radial distortion; p1, p2 are "
        <<" tangential distortion. -->"<<endl;
  outXML<<" <Default p1=\"0\" p2=\"0\" k1=\""
        <<setprecision(10)<<distortion[0]<<"\" k2=\""<< setprecision(10)
        <<distortion[1]<<"\" FocalLength=\""<<Kbi[0][0]<<"\" />"<<endl;
  outXML<<"   </PerspectiveCalib>"<<endl;
  outXML<<"   <VignetteCalib>"<<endl;
  outXML<<"    <!--The measurements for vignette calibration relate the "
        <<"distance of a pixel from the image principal point to an absorbtion "
        <<"factor for the corresponding image pixels.-->   "<<endl;
  outXML<<"   </VignetteCalib>"<<endl;
  outXML<<"  </Lens>"<<endl;
  outXML<<" <!--The \"PrincipalPoint\" is given as a fraction of "
        <<"\"ImageWidth\"/\"ImageHeight\" with (0.5,0.5) is the image center.-->"
        <<endl;
  outXML<<"   <PrincipalPoint X=\""
        <<setprecision(10)<<Kbi[0][2]/(double)(width-1)<<"\" "<<endl;
  outXML<<"        Y=\""
        <<setprecision(10)<<Kbi[1][2]/(double)(height-1)<<"\" />"<<endl;
  outXML<<"  </camera>"<<endl;
  outXML.close();

  cout<<"MIP camera parameter file written to out_cameraParam.xml" << endl;

  //create and write projection xml-file
  BIAS::Projection proj;
  BIAS::Pose pose;
  Quaternion<double> quat;
  Rbi.GetQuaternion(quat);
  pose.Set(quat,Cbi);
  std::vector<double> distortionDouble(4,0.0);
  distortionDouble[0] = distortion[0];
  distortionDouble[1] = distortion[1];
  distortionDouble[2] = distortion[2];
  distortionDouble[3] = distortion[3];
  BIAS_ProjParaPersp_DISTORTION_TYPE radDistType=DISTYPE_DEF;
  proj.CreatePerspective(pose,Kbi,width,height,radDistType,distortionDouble);

  //  Create tree and base node for XML-file...
#ifdef BIAS_HAVE_XML2
  XMLIO tree;
  xmlNodePtr root = tree.create("Projection");
  tree.addAttribute(root, "Version", 0.1);

  //  ...write data to XML-tree...
  proj.XMLOut(root,tree);

  //  ...if XML-file has been written...
  if (tree.write("out_projParam.xml") == 0){
    cout << "MIP projection parameter file written to out_projParam.xml" << endl;
    // ...clear tree
    tree.clear();
  } else {
    cout << "Failed to write MIP projection parameter file to "
         << "out_projParam.xml!" << endl;
  }
#endif

  if (usegui) cvDestroyAllWindows();
  delete cornersArray;
  delete corners3d;
  delete cornersFound;
  return 0;
}