ExampleEParametrization.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 ExampleEParametrization.cpp
   @relates EParametrization
   @brief Example for the parameter of the EMatrix and usage 
   @ingroup g_examples
   @author MIP  
*/

#include <Base/Math/Random.hh>

#include <Base/Geometry/EParametrization.hh>
#include <Base/Geometry/HomgPoint3D.hh>
#include <Base/Geometry/HomgPoint2D.hh>
#include <Base/Geometry/RMatrixBase.hh>
#include <Base/Geometry/PMatrixBase.hh>
#include <Base/Geometry/KMatrix.hh>

using namespace BIAS;
using namespace std;

int main(int argc, char *argv[])
{
  Random r;
  HomgPoint3D p3d;
  p3d[0] = r.GetUniformDistributed(-5.0, 5.0);
  p3d[1] = r.GetUniformDistributed(-5.0, 5.0);
  p3d[2] = r.GetUniformDistributed(10, 15.0);
  p3d[3] = 1.0;

  KMatrix K(MatrixIdentity);
  Vector3<double> C1(0,0,0), C2(1,0,0);
  RMatrixBase R1(MatrixIdentity), R2(MatrixIdentity), R;
  R2.SetXYZ(0.01, -0.01, 0.01);
  R = R1.Transpose() * R2;
  PMatrixBase P1(K, R1, C1), P2(K, R2, C2);
  HomgPoint2D p1=P1*p3d, p2=P2*p3d, epp=P1*HomgPoint3D(C2);
  cout << "p1: "<<p1<<"\tp2: "<<p2<<"\nepipole: "<<epp<<endl;
  Quaternion<double> q;
  R.GetQuaternion(q);
  EParametrization ep, epi;
  ep.SetEpipole(epp);
  ep.SetOrientation(q);
  epi = ep;
  epi.Invert();

  Matrix3x3<double> E = ep.GetEssentialMatrix(), Ei = epi.GetEssentialMatrix();

  cout << p1.ScalarProduct(E*p2) << "\t"
    //<< p2.ScalarProduct(E*p1) << "\n"
    //<< p1.ScalarProduct(Ei*p2) << "\t"
       << p2.ScalarProduct(Ei*p1) << "\n";
  
}