ExampleAutoCalib.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
 */


/**
   @example ExampleAutoCalib.cpp
   @relates FMatrix,  RMatrix , EMatrix, FMatrixEstimation,  PMatrixEstimation
   @brief Example for self calibration
   @ingroup g_examples
   @author MIP
*/


#include "Geometry/FMatrix.hh"
#include "Geometry/RMatrix.hh"
#include "Geometry/EMatrix.hh"
#include "Base/Geometry/KMatrix.hh"
#include "../FMatrixEstimation.hh"
#include "../PMatrixEstimation.hh"
#include "../PMatrixLinear.hh"
#include "../Triangulation.hh"
#include "../../Base/Math/Random.hh"
#include "Utils/ThreeDOut.hh"
#include "Utils/Param.hh"

using namespace BIAS;
using namespace std;

// ---------------------------------------------------------------------------
int main(int argc, char *argv[]) {

  Param *p = new Param();
  p->AddParamInt("imgWidth", "image width", 400);
  p->AddParamInt("imgHeight", "image height", 400);
  p->AddParamDouble("focalLength", "Focal length of camera", 400.0);
  p->AddParamDouble("princPointX", "x-coordinate of principle point", 199.5);
  p->AddParamDouble("princPointY", "y_coordinate of principle point", 199.5);

  p->AddParamDouble("rotationX", "Rotation of second cam in x-direction", 12.0);
  p->AddParamDouble("rotationY", "Rotation of second cam in y-direction", 20.0);
  p->AddParamDouble("rotationZ", "Rotation of second cam in z-direction", -2.0);
  p->AddParamDouble("transX", "Translation of second cam in x-direction", -1.0/sqrt(2.0));
  p->AddParamDouble("transY", "Translation of second cam in y-direction", 1.0 /sqrt(2.0));
  p->AddParamDouble("transZ", "Translation of second cam in z-direction", 0.0);

  p->AddParamDouble("noiseUpper", "Upper bound for noise on points in pixels", 0.0);
  p->AddParamDouble("noiseLower", "Lower bound for noise on points in pixels", -0.0);

  if (argc == 2) {
    int res = p->ReadParameter(string(argv[1]));
    if (res != 0) {
      cout << "Could not read parameter file, using default parameters instead " <<res << endl;
    }
  }

  if (argc == 2) {
    int res = p->WriteParameter(string(argv[1]));
    if (res != 0) {
      BIASERR("Error could not write parameter file to: " << argv[1] << res);
    }
  } else {
    int res = p->WriteParameter("autoCalib.param");
    if (res != 0) {
      BIASERR("Error could not write parameter file to: " << "autoCalib.param" << res);
    }
  }
  // write the parameters used to console
  p->ShowData();

  BIAS::FMatrix F, TheoF;
  std::vector<FMatrix> vecF;
  BIAS::PMatrix P1, P2, P1Comp, P2Comp;
  BIAS::Random Randomizer;

  BIAS::RMatrix R1, R2;
  BIAS::KMatrix K(MatrixIdentity);
  BIAS::Vector3<double> C1, C2;

  // set rotation matrices
  R1.SetIdentity();
  R2.SetXYZ((*p->GetParamDouble("rotationX"))*M_PI/180.0, (*p->GetParamDouble("rotationY"))*M_PI
      /180.0, (*p->GetParamDouble("rotationZ"))*M_PI/180.0);

  // set image size
  int width = *p->GetParamInt("imgWidth");
  int height =*p->GetParamInt("imgHeight") ;
  double focalLength = *p->GetParamDouble("focalLength");

  // camera center coordiantes
  C1[0] = 0.0;
  C1[1] = 0.0;
  C1[2] = 0.0;

  C2[0] = *p->GetParamDouble("transX");
  C2[1] = *p->GetParamDouble("transY");
  //    C2[0] = -1.0;
  //    C2[1] = 0.0;
  C2[2] = *p->GetParamDouble("transZ");

  // camera matrix for both cameras
  K.SetFx(focalLength);
  K.SetFy(focalLength);
  // in image coordinates
  K.SetHx(*p->GetParamDouble("princPointX"));
  K.SetHy(*p->GetParamDouble("princPointY"));
  K.SetSkew(0.0);

  K[2][2] = 1;

  // combine camera matrix, rotation matrix and camera center to  projection matrices
  P1.Compose(K, R1, C1);
  P2.Compose(K, R2, C2);

  cout << "R1 " << R1 << endl;
  cout << "R2 " << R2 << endl;
  cout << "C1 " << C1 << endl;
  cout << "C2 " << C2 << endl;
  cout << "K " << K << endl;
  cout << "P1 " << P1 << endl;
  cout << "P2 " << P2 << endl;

  // compute randomized correspondences
  // later compute more and put them on planes, check angle of reconstruction
  vector<HomgPoint3D> points, pointsComp;
  vector<HomgPoint2D> p1, p2;

  int numPoints = 50;

  for (int p=0; p<numPoints; p++) {
    points.push_back(HomgPoint3D(Randomizer.GetUniformDistributed(-1.0, 1.0),
        Randomizer.GetUniformDistributed(-1.0, 1.0), Randomizer.GetUniformDistributed(3.0, 5.0)));
    cout << "NO. " << points[p] << endl;
    p1.push_back(P1 * points[p]);
    p1[p].Homogenize();
    //      cout << "No. " << p << " : "<< p1[p][0] << " " << p1[p][1] << endl;
    p2.push_back(P2 * points[p]);
    p2[p].Homogenize();
  }

  // add noise to points
  for (int i = 0; i < numPoints; i++) {
    (p1[i])[0] += Randomizer.GetUniformDistributed( *p->GetParamDouble("noiseLower"),
        *p->GetParamDouble("noiseUpper"));
    (p1[i])[1] += Randomizer.GetUniformDistributed( *p->GetParamDouble("noiseLower"),
        *p->GetParamDouble("noiseUpper"));
    (p2[i])[0] += Randomizer.GetUniformDistributed( *p->GetParamDouble("noiseLower"),
        *p->GetParamDouble("noiseUpper"));
    (p2[i])[1] += Randomizer.GetUniformDistributed( *p->GetParamDouble("noiseLower"),
        *p->GetParamDouble("noiseUpper"));
  }

  //     compute points on line with right angle - are not used for eight point algorithm
  //    int numPointsOnLines = 100;
  //    vector<HomgPoint3D> line1, line2, line3, line1Comp, line2Comp, line3Comp;
  //    vector<HomgPoint2D> l1P1, l1P2, l2P1, l2P2, l3P1, l3P2;
  //    double randLambda = 0.0;
  //    for (int i = 0; i < numPointsOnLines; i++) {
  //        randLambda = Randomizer.GetUniformDistributed(-2.0, 2.0);
  //        line1.push_back(HomgPoint3D(-1.0 * randLambda, 0.0 * randLambda, 8.0,
  //                1.0));
  //        l1P1.push_back(P1 * line1[i]);
  //        l1P1[i].Homogenize();
  //        l1P2.push_back(P2 * line1[i]);
  //        l1P2[i].Homogenize();
  //        randLambda = Randomizer.GetUniformDistributed(-2.0, 2.0);
  //        line2.push_back(HomgPoint3D(0.0 * randLambda, -1.0 * randLambda, 8.0,
  //                1.0));
  //        l2P1.push_back(P1 * line2[i]);
  //        l2P1[i].Homogenize();
  //        l2P2.push_back(P2 * line2[i]);
  //        l2P2[i].Homogenize();
  //
  //        randLambda = Randomizer.GetUniformDistributed(0.0, 8.0);
  //        line3.push_back(HomgPoint3D(0.0 * randLambda, 0.0 * randLambda, randLambda * 1.0,
  //                        1.0));
  //        l3P1.push_back(P1 * line3[i]);
  //        l3P1[i].Homogenize();
  //        l3P2.push_back(P2 * line3[i]);
  //        l3P2[i].Homogenize();
  //    }

  // Compute FMatrix from correspondences
  bool NormalizeHartley = true;
  FMatrixEstimation FEstimator(NormalizeHartley);
  // vector containing FMatrices
  vecF.resize(1);
  vecF.clear();
  FEstimator.EightPoint(F, p1, p2);

  // true fmatrix
  FMatrix F_gt;
  F_gt.ComputeFromPMatrices(P1, P2);
  cout << "True F matrix ----------- residual error " << F_gt.GetResidualError(p1, p2) << endl;
  cout << "Estimated F matrix ------ residual error " << F.GetResidualError(p1, p2) << endl;

  // use AutoCalib function to determine focal length and both projection matrices
  PMatrixEstimation pEstimator;
  int res = pEstimator.AutoCalib(F, width, height, p1, p2, P1Comp, P2Comp);
  if (res != 0) {
    BIASERR("Couldn't determine f, P1 and P2" << res);
    return -1;
  }

  // cout results
  cout << "Results of AutoCalib: (true angle of view " << atan(width/(2.0*focalLength)) *180.0/M_PI
      << ")" << endl;
  cout << "P1 " << P1Comp << endl;
  cout << "P2 " << P2Comp << endl;
  cout << "Resuting Kmatrix " << P2Comp.GetK() << endl;
  cout << "Resulting C vectors " << P1Comp.GetC() <<" "<< P2Comp.GetC() <<endl;
  cout << "Resulting Rmatrix " << P2Comp.GetR() << endl;

  RMatrix REst;
  REst = (RMatrix) P2Comp.GetR();
  double phiX, phiY, phiZ;
  REst.GetRotationAnglesXYZ(phiX, phiY, phiZ);
  cout << "Rotation angles " << phiX*180.0/M_PI << " " << phiY*180.0/M_PI << " " << phiZ*180.0/M_PI
      << endl;

  cout << "ERRORS " << endl;
  cout << "Focal length " << (P2Comp.GetK()).GetFx() << endl;
  cout << "Diff P2 " << P2Comp - P2 << endl;

  // triangulate using the new projection matrices - are for example lines still lines?
  Triangulation triangulator;
  Vector3<double> tempPoint;
  for (int i = 0; i < numPoints; i++) {
    triangulator.Triangulate(P1Comp, P2Comp, p1[i], p2[i], tempPoint);
    pointsComp.push_back(HomgPoint3D(tempPoint));
  }
  // triangulate lines
/*
  for (int i = 0; i < numPointsOnLines; i++) {
    triangulator.Triangulate(P1Comp, P2Comp, l1P1[i], l1P2[i], tempPoint);
    line1Comp.push_back(HomgPoint3D(tempPoint));
    triangulator.Triangulate(P1Comp, P2Comp, l2P1[i], l2P2[i], tempPoint);
    line2Comp.push_back(HomgPoint3D(tempPoint));
    triangulator.Triangulate(P1Comp, P2Comp, l3P1[i], l3P2[i], tempPoint);
    line3Comp.push_back(HomgPoint3D(tempPoint));
  }
*/
  ThreeDOutParameters param3DO;
  param3DO.CameraStyle = PyramidMesh;
  param3DO.PointStyle = Box;
  param3DO.PointSize = 0.1;
  param3DO.LineStyle = Solid;
  param3DO.DrawUncertaintyEllipsoids = false;
  ThreeDOut threeDOut(param3DO);

  for (int i = 0; i < numPoints; i++) {
    threeDOut.AddPoint(points[i], RGBAuc(0, 0, 255, 127));
  }

  for (int i = 0; i < numPoints; i++) {
    threeDOut.AddPoint(pointsComp[i], RGBAuc(255, 0, 0, 127));
  }
  //    for (int i = 0; i < numPointsOnLines; i++) {
  //        threeDOut.AddPoint(line1[i], RGBAuc(0, 255, 255, 127));
  //        threeDOut.AddPoint(line2[i], RGBAuc(255, 255, 255, 127));
  //        threeDOut.AddPoint(line3[i], RGBAuc(255, 255, 255, 127));
  //    }
  //
  //    for (int i = 0; i < numPointsOnLines; i++) {
  //        threeDOut.AddPoint(line1Comp[i], RGBAuc(255, 255, 0, 127));
  //        threeDOut.AddPoint(line2Comp[i], RGBAuc(255, 0, 255, 127));
  //        threeDOut.AddPoint(line3Comp[i], RGBAuc(255, 0, 255, 127));
  //    }

  threeDOut.AddPMatrix(P1, 400, 400, RGBAuc_WHITE_OPAQUE, 0.1);
  threeDOut.AddPMatrix(P2, 400, 400, RGBAuc(255, 0, 0, 127), 0.1);
  threeDOut.AddPMatrix(P1Comp, 400, 400, RGBAuc(0, 255, 255, 127), 0.1);
  threeDOut.AddPMatrix(P2Comp, 400, 400, RGBAuc(255, 0, 255, 127), 0.1);
  Vector3<double> start(0.0, 0.0, 0.0);
  Vector3<double> end(1.0, 0.0, 0.0);

  threeDOut.AddLine(start, end, RGBAuc(255, 0, 0, 127));
  end.Set(0.0, 1.0, 0.0);
  threeDOut.AddLine(start, end, RGBAuc(0, 255, 0, 127));
  end.Set(0.0, 0.0, 1.0);
  threeDOut.AddLine(start, end, RGBAuc(0, 0, 255, 127));
  threeDOut.VRMLOut("test.wrl");
  //    threeDOut.Dump();

  return 0;
}