PMatrixTest.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 PMatrixTest.cpp
   @relates PMatrix, PMatrixEstimation
   @brief example investigating P-Matrix
   @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"



#define FMATRIX_FILENAME_SUFFIX ".FMatrix"
#define CORRESPONDENCE_FILENAME_SUFFIX ".corresp"


using namespace BIAS;
using namespace std;

// ---------------------------------------------------------------------------
//int main(int argc, char *argv[]) 
int main() 
{   
  BIAS::FMatrix F;
  BIAS::PMatrix TheoP1,TheoP2;
  BIAS::Random Randomizer;
  double rangle = 0;
  for (unsigned int i=0; i<2; i++) {
   
    Vector3<double> RAxis(Randomizer.GetUniformDistributed(-1.0, 1.0),
                          Randomizer.GetUniformDistributed(-1.0, 1.0),
                          Randomizer.GetUniformDistributed(-1.0, 1.0));
    RAxis.Normalize();
    rangle = Randomizer.GetUniformDistributed(-M_PI, M_PI);

    Vector3<double> C(Randomizer.GetUniformDistributed(-20, 20),
                      Randomizer.GetUniformDistributed(-20, 20),
                      Randomizer.GetUniformDistributed(-20, 20)); 

    RMatrix R(RAxis, rangle);

    if (R[2][2] < 0.0) {
      for (unsigned int y=0;y<3;y++) {
        R[1][y] *= -1.0;
        R[2][y] *= -1.0;
      }
    }


    // check if opt axes "meet" in front of cams
    if (C[0] * R[0][2] >0.0) C[0] *= -1.0;
    if (C[1] * R[1][2] >0.0) C[1] *= -1.0;
    C.Normalize();

    KMatrix K;
    K.SetIdentity();
    // K[0][0] = Randomizer.GetUniformDistributed(100,1000);
    //K[1][1] = Randomizer.GetNormalDistributed(K[0][0], 0.1*K[0][0]);
    
    

    PMatrix TheoP2(K,R,C);

    // generate P1 as identity matrix
    TheoP1.set_identity();
    TheoP1 = K * TheoP1;
    
    FMatrix F(TheoP1, TheoP2);

    HomgPoint2D epi1, epi2;          
    F.GetEpipolesHomogenized(epi1, epi2);
    cout <<"epipoles are "<<epi1<<" and "<<epi2 <<endl;

    HomgPoint3D VC1, VC2;
    TheoP1.GetC(VC1);
    TheoP2.GetC(VC2);
    epi1 = TheoP1 * VC2; epi1.Homogenize();
    epi2 = TheoP2 * VC1; epi2.Homogenize();
    cout <<"validated epipoles are "<<epi1<<" and "<<epi2 <<endl;




    //F = epi1.GetSkewSymmetricMatrix() * epi2.GetSkewSymmetricMatrix();
    //cout <<"epipoles are now "<<epi1<<" and "<<epi2 <<endl;

  
    // P matrix computation
    BIAS::PMatrix P1,P2;
  
    BIAS::PMatrixEstimation PEst;
    //PEst.SetDebugLevel(BIAS_FQuasiEuklid|BIAS_FMATRIXDEBUG|BIAS_PMATRIXDEBUG);
    double BaselineMagnitude = (i==0)?1:-1;
    
    PEst.ComputeFromFQuasiEuklid(F, BaselineMagnitude,P1,P2);
    P2.Normalize();
    cout << " TheoP2 = " << TheoP2 << endl;
    //cout << "QuasiEu.  P2 = " << P2 << endl;
    //cout << "Diff is "<< P2-TheoP2<<endl;
    
    FMatrix FCheck(P1,P2);
    FCheck.GetEpipolesHomogenized(epi1, epi2);
    //cout <<"computed epipoles are "<<epi1<<" and "<<epi2 <<endl;


    PEst.ComputeFromFDirect(F, BaselineMagnitude,P1,P2);
    P2.Normalize();
    cout << "Direct   P2 = " << P2 << endl;
    //cout << "Diff is "<< P2-TheoP2<<endl;
    FCheck.ComputeFromPMatrices(P1,P2);
    FCheck.GetEpipolesHomogenized(epi1, epi2);
    cout <<"computed epipoles are "<<epi1<<" and "<<epi2 <<endl<<endl<<endl;     

  }
  return 0;
}