ExamplePMatrix.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 ExamplePMatrix.cpp
   @relates PMatrix
   @brief Example PMatrix
   @ingroup g_examples
   @author MIP
*/

#include <Base/Common/BIASpragma.hh>

#include <Base/Geometry/HomgPoint2D.hh>
#include <Geometry/PMatrix.hh>
#include <Geometry/RMatrix.hh>
#include <Base/Geometry/KMatrix.hh>
#include <Base/Math/Random.hh>

#include <vector>
#include <Geometry/PMatrixLinear.hh>


using namespace BIAS;
using namespace std;


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

  cout << "$$$$$$$$$$$$$$$$$$$$$$$$$$$$$ ExamplePMatrix $$$$$$$$$$$$$$$$$$$$$$"
       << endl;
  PMatrix P_current;
  P_current.SetIdentity();

  KMatrix K;
  K.SetIdentity();
  K *= 100.0;
  K[0][2] = 384;
  K[1][2] = 256;
  K[2][2] = 1.0;
  K.SetIdentity();


  Vector3<double> axis(1,2,3);
  axis.Normalize();
  double angle = 1.0;
  RMatrix R;
  R.Set(axis, angle);

  Vector3<double> C(1,2,3);
  Matrix3x3<double> Hinf;
  P_current.Compose(K,R,C);


  if (P_current.GetHinf(Hinf)!=0){
    BIASERR("Error computing Hinf");
  };
  cout<<"C is "<<P_current.GetC()<<" R is "<<P_current.GetR()
      <<" K is "<<P_current.GetK()<<" Hinf is "<<Hinf<<endl;
  PMatrix Pose=P_current;
  cout<<"C is "<<Pose.GetC()<<" R is "<<Pose.GetR()
      <<" K is "<<Pose.GetK()<<endl;
  
  bool TestP = false;
  if (TestP) {
   for (unsigned int i=0; i<4; i++)
      for (unsigned int j=0; j<3; j++) {
        double noise = 0.995+(0.01*rand()/(double)RAND_MAX);
        P_current[j][i] *= noise;
        cout << noise <<" ";
      }
    P_current *= 7.5;
    P_current.InvalidateDecomposition();
  
    cout <<"Pcurrent is "<<P_current<<endl; 
    P_current.Normalize();
    cout <<"Pcurrent after normalizing is "<<P_current<<endl; 

   if (P_current.GetHinf(Hinf)!=0){
      BIASERR("Error computing Hinf");
    };

    cout<<"C is "<<P_current.GetC()<<" R is "<<P_current.GetR()
        <<" K is "<<P_current.GetK()<<" Hinf is "<<Hinf<<endl;
  }
  cout <<" +++++++++++++++++++++++++++++++++++++++++++++++++++++++++"<<endl;
  cout <<" ++++++++++++++ linear p estimation ++++++++++++++++++++++"<<endl;
  BIAS::Random Rand;
  vector<HomgPoint3D> vecX;
  vector<HomgPoint2D> vecx;
  vector<HomgPoint3D*> vecpX;
  vector<HomgPoint2D*> vecpx;
  unsigned int n=6;
  vecX.reserve(n);
  vecx.reserve(n);
  bool WantPlane=true, WantNoise=true;
  unsigned int NumberOffPlane = 0;
  HomgPoint3D X, 
    a(Rand.GetUniformDistributed(-100.0,100.0),
      Rand.GetUniformDistributed(-100.0,100.0),
      Rand.GetUniformDistributed(1.0,100.0), 1.0),
    b(Rand.GetUniformDistributed(-100.0,100.0),
      Rand.GetUniformDistributed(-100.0,100.0),
      Rand.GetUniformDistributed(1.0,100.0), 1.0),
    c(Rand.GetUniformDistributed(-100.0,100.0),
      Rand.GetUniformDistributed(-100.0,100.0),
      Rand.GetUniformDistributed(1.0,100.0), 1.0);

  for (unsigned int i=0; i<n; i++) {
    if (WantPlane) {
      double weight1 = Rand.GetUniformDistributed(0.0,1.0);
      double weight2 = Rand.GetUniformDistributed(0.0,weight1);
      X = a*weight1 + b*weight2 + c*(1.0-weight1-weight2);
      X[3] = 1.0;
    } else {
      X.Set(Rand.GetUniformDistributed(-100.0,100.0),
            Rand.GetUniformDistributed(-100.0,100.0),
            Rand.GetUniformDistributed(1.0,100.0), 1.0);
    }
    if (i<NumberOffPlane) {
      X.Set(Rand.GetUniformDistributed(-100.0,100.0),
            Rand.GetUniformDistributed(-100.0,100.0),
            Rand.GetUniformDistributed(1.0,100.0), 1.0);
    }
    cout <<"using point X="<<X<<endl;
    HomgPoint2D x;
    x = P_current * X;
    x.Homogenize();
    if (WantNoise) {
      x[0] *= Rand.GetUniformDistributed(0.999, 1.001);
      x[1] *= Rand.GetUniformDistributed(0.999, 1.001);
    }
    vecX.push_back(X);
    vecx.push_back(x);
  }
  for (unsigned int i=0; i<n; i++) {
    vecpX.push_back(&vecX[i]);
    vecpx.push_back(&vecx[i]);
  }
 
  PMatrixLinear PLin;
  PLin.SetDebugLevel("PMATRIXLINEAR");

  Pose.SetZero();Pose.InvalidateDecomposition();
  cout <<"Pose estimation:"<<endl;
  if (!PLin.ComputeCalibrated(vecpX, vecpx, Pose)) BIASERR("failed !");

  cout <<"Pose      is "<<Pose<<endl;
  cout <<"P_current is "<<P_current<<endl;
  cout <<"Diff      is "<<Pose-P_current<<endl;
  cout<<endl;
  
  Pose.SetZero();Pose.InvalidateDecomposition();
  cout <<"complete PMatrix estimation:"<<endl;
  PLin.SetDebugLevel(0);
  if (!PLin.Compute(vecpX, vecpx, Pose)) BIASERR("failed !");
  Pose.Normalize();
  if (Pose[2][0]*P_current[2][0]<0) Pose *= -1.0;
  cout <<"Pose      is "<<Pose<<endl;
  cout <<"P_current is "<<P_current<<endl;
  cout <<"Diff      is "<<Pose-P_current<<endl;
  cout<<endl;
  
  return 0;
}