ExampleFMatrix2.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 ExampleFMatrix2
   @relates FMatrix
   @brief Example computing F matrix with 8-point, non-linear optimize, and gold standard
   @ingroup g_examples
   @author MIP
*/

#include "../FMatrix.hh"
#include "../FMatrixEstimation.hh"
#include "../PMatrixEstimation.hh"
#include "../PMatrixLinear.hh"
#include "../Triangulation.hh"
#include "../../Base/Math/Random.hh"

using namespace BIAS;
using namespace std;

// ---------------------------------------------------------------------------
// testing eight point and Gold Standard algorithm for F matrix estimation

int main() {
  BIAS::FMatrix FEightPoint, FOptimize, FGoldSt, TheoF;

  BIAS::PMatrix P1, P2;
  BIAS::Random Randomizer;

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

  // set rotation matrices
  R1.SetIdentity();
  //R2.SetXYZ(20.0*M_PI/180.0, 10.0*M_PI/180.0, -60*M_PI/180.0);
  R2.SetXYZ(0.0, 5.0*M_PI/180.0, 0.0);
  // camera center coordiantes
  C1[0] = 0.0;
  C1[1] = 0.0;
  C1[2] = 0.0;

  C2[0] = -1.0/sqrt(2.0);
  C2[1] = 1.0/sqrt(2.0);
  //  C2[0] = -1.0;
  //  C2[1] =  3.0;
  C2[2] = 0.0;

  //  // camera matrix for both cameras
  K.SetFx(400.0);
  K.SetFy(400.0);
  // in image coordinates
  K.SetHx(100.0);
  K.SetHy(100.0);
  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);

  P1.InvalidateDecomposition();
  P2.InvalidateDecomposition();

  // compute true F matrix
  TheoF.ComputeFromPMatrices(P1, P2);

  vector<HomgPoint3D> points;
  vector<HomgPoint2D> p1, p2;

  // noise is added to point correspondences
  vector<HomgPoint2D> p1Noisy, p2Noisy;
  double noiseB = 10;
  double testDist1, testDist2, testDist3, testDist4;

  unsigned int numPoints = 80;
  cout << "Randomized correspondences are "<<endl;
  for (unsigned int p=0; p<numPoints; p++) {
    cout << "number " << p << endl;
    points.push_back(HomgPoint3D(Randomizer.GetUniformDistributed(-1.0, 1.0),
        Randomizer.GetUniformDistributed(-1.0, 1.0), Randomizer.GetUniformDistributed(-1.0, 1.0)));
    p1.push_back(P1 * points[p]);
    p1[p].Homogenize();
    cout << "{ "<<p1[p]<<" ";
    p2.push_back(P2 * points[p]);
    p2[p].Homogenize();
    cout << p2[p]<<" }  "<<endl;

//    if (p < 8) {
      testDist1 = Randomizer.GetUniformDistributed(-noiseB, noiseB);
      testDist2 = Randomizer.GetUniformDistributed(-noiseB, noiseB);
      testDist3 = Randomizer.GetUniformDistributed(-noiseB, noiseB);
      testDist4 = Randomizer.GetUniformDistributed(-noiseB, noiseB);
      p1Noisy.push_back(HomgPoint2D((p1[p])[0] + testDist1, (p1[p])[1] + testDist2));
      p2Noisy.push_back(HomgPoint2D((p2[p])[0] + testDist3, (p2[p])[1] + testDist4));
//    } else {
//      p1Noisy.push_back(p1[p]);
//      p2Noisy.push_back(p2[p]);
//    }
    cout << "{ " << p1Noisy[p] << " " << p2Noisy[p] << " }" << endl;
  }

  bool NormalizeHartley = true;
  FMatrixEstimation FEstimator(NormalizeHartley);

  // vars for epipole computation
  HomgPoint2D e8Point1, e8Point2, theoe1, theoe2, eGoldSt1, eGoldSt2, eOpt1, eOpt2;
  double residualError8Point, residualErrorGoldStandard, residualErrorTest;
  double residualError8Point_n, residualErrorGoldStandard_n, residualErrorTest_n;
  double residualErrorOpt_n, residualErrorOpt;

  residualErrorTest_n = TheoF.GetResidualError(p1Noisy, p2Noisy);
  residualErrorTest = TheoF.GetResidualError(p1, p2);

  // use eight point algorithm and the optimize function applying the
  // levenberg marquardt algorithm to optimize the parameters of the Fmatrix
  // using the sum of squared distances to the epipolar line as an error function
  FEightPoint.SetIdentity();
  FEstimator.Linear(FEightPoint, p1Noisy, p2Noisy);
  residualError8Point_n = FEightPoint.GetResidualError(p1Noisy, p2Noisy);
  residualError8Point = FEightPoint.GetResidualError(p1, p2);
  FEightPoint.GetEpipolesHomogenized(e8Point1, e8Point2);
  e8Point1.Normalize();
  e8Point2.Normalize();

  // use optimize on 8Point result
  FOptimize = FEightPoint;
  FEstimator.Optimize(FOptimize, p1Noisy, p2Noisy);
  residualErrorOpt_n = FOptimize.GetResidualError(p1Noisy, p2Noisy);
  residualErrorOpt = FOptimize.GetResidualError(p1, p2);
  FOptimize.GetEpipolesHomogenized(eOpt1, eOpt2);
  if(!eOpt1.IsZero()) eOpt1.Normalize();
  if(!eOpt2.IsZero())eOpt2.Normalize();

  // use Gold Standard algorithm
  FGoldSt.SetIdentity();
  FEstimator.GoldStandard(FGoldSt, p1Noisy, p2Noisy);
  residualErrorGoldStandard_n = FGoldSt.GetResidualError(p1Noisy, p2Noisy);
  residualErrorGoldStandard = FGoldSt.GetResidualError(p1, p2);
  FGoldSt.GetEpipolesHomogenized(eGoldSt1, eGoldSt2);
  if(!eGoldSt1.IsZero()) eGoldSt1.Normalize();
  if(!eGoldSt2.IsZero()) eGoldSt2.Normalize();

  // cout results
  cout << "True F matrix " << 1/TheoF[2][2] * TheoF << endl;
  cout << "Residual Error " << residualErrorTest  << " noisy " << residualErrorTest_n << endl;
  TheoF.GetEpipolesHomogenized(theoe1, theoe2);
  theoe1.Normalize();
  theoe2.Normalize();
  cout << "True epipoles are "<<theoe1<<" and "<<theoe2<<endl;
  cout << endl;

  cout << "Eight point algorithm " << endl;
  cout << "Fmatrix " << 1/FEightPoint[2][2] * FEightPoint << endl;
  cout << "Residual Error " << residualError8Point << " noisy " << residualError8Point_n  << endl;
  cout << "est. epipoles are "<<e8Point1<<" and "<<e8Point2<<endl;

  cout << endl;

  cout << "Optimize algorithm " << endl;
  cout << "Fmatrix " << 1/FOptimize[2][2] * FOptimize << endl;
  cout << "Residual Error " << residualErrorOpt << " noisy " << residualErrorOpt_n  << endl;
  cout << "est. epipoles are "<<eOpt1<<" and "<<eOpt2<<endl;

  cout << endl;


  cout << "Gold Standard algorithm" << endl;
  cout << "Fmatrix " << 1/FGoldSt[2][2] * FGoldSt << endl;
  cout << "Residual Error " << residualErrorGoldStandard << " noisy " << residualErrorGoldStandard_n << endl;
  cout << "est. epipoles are "<<eGoldSt1<<" and "<<eGoldSt2<<endl;

  return 0;
}