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


#include "Parametrization.hh"
#include <Base/Common/BIASpragma.hh>
#include <Base/Geometry/Quaternion.hh>

#include <vector>

using namespace std;
using namespace BIAS;


Parametrization::Parametrization() {}
Parametrization::~Parametrization() {}

bool Parametrization::FMatrixToParams( const FMatrix &Mat, 
                                       vector<double> &Params )
{
  if ( Mat.IsZero() )
    {
      BIASERR("A parametrization of a null matrix doesn't make sense!");
      return false;
    }

  F_ = Mat; // set class member
  Params.clear();

  double scale = GetBiggestAbsEntry();
  Fscale_ = 1/scale;

  Mat.Scale(Fscale_, F_);

  double a,b,c;

  HomgPoint2D FEpipole1,FEpipole2;
  F_.GetEpipoles(FEpipole1,FEpipole2);
  FEpipole1.Homogenize();
  FEpipole2.Homogenize();

  if ( Frow_ == 0 && Fcol_ == 0 )
    {
      a=F_[0][1];
      b=F_[1][0];
      c=F_[1][1];
    }
  else if ( Frow_ == 1 && Fcol_ == 0 )
    {
      a=F_[0][0];
      b=F_[0][1];
      c=F_[1][1];
    }
  else if ( Frow_ == 0 && Fcol_ == 1 )
    {
      a=F_[0][0];
      b=F_[1][0];
      c=F_[1][1];
    }
  else if ( Frow_ == 1 && Fcol_ == 1 )
    {
      a=F_[0][0];
      b=F_[0][1];
      c=F_[1][0];
    }

  Params.push_back(a);
  Params.push_back(b);
  Params.push_back(c);

  Params.push_back(-FEpipole1[0]);
  Params.push_back(-FEpipole1[1]);
  Params.push_back(-FEpipole2[0]);
  Params.push_back(-FEpipole2[1]);

  return true;
}


bool Parametrization::ParamsToFMatrix( FMatrix &Mat,
                                       const vector<double> &Params )
{
  if ( Params.size() != 7 )
    {
      BIASERR("There must be 7 parameters to compose the F matrix!");
      return false;
    }
  FParams_ = Params; // set class member

  double a=0.0,b=0.0,c=0.0,d=0.0;
  double e10,e11,e20,e21;

  if ( Frow_ == 0 && Fcol_ == 0 )
    {
      a = 1;
      b = Params[0];
      c = Params[1];
      d = Params[2];
    }
  else if ( Frow_ == 1 && Fcol_ == 0 )
    {
      a = Params[0];
      b = Params[1];
      c = 1;
      d = Params[2];
    }
  else if ( Frow_ == 0 && Fcol_ == 1 )
    {
      a = Params[0];
      b = 1;
      c = Params[1];
      d = Params[2];
    }
  else if ( Frow_ == 1 && Fcol_ == 1 )
    {
      a = Params[0];
      b = Params[1];
      c = Params[2];
      d = 1;
    }

  e10 = Params[3];
  e11 = Params[4];
  e20 = Params[5];
  e21 = Params[6];

  Mat[0][0] = a;
  Mat[0][1] = b;
  Mat[0][2] = e10 * a + e11 * b;

  Mat[1][0] = c;
  Mat[1][1] = d;
  Mat[1][2] = e10 * c + e11 * d;

  Mat[2][0] = e20 * a + e21 * c;
  Mat[2][1] = e20 * b + e21 * d;
  Mat[2][2] = e20 * e10 * a + e20 * e11 * b + e21 * e10 * c + e21 * e11 * d;

  return true;
}


bool Parametrization::HMatrixToParams( const HMatrix &Mat,
                                       vector<double> &Params )
{
  if ( Mat.IsZero() )
    {
      BIASERR("A parametrization of a null matrix doesn't make sense!");
      return false;
    }

  H_ = Mat; // set class member
  Params.clear();

  // make sure that 3,3 element is positive
  if ( H_[2][2] < 0 )
    H_.Scale( -1, H_);

  // compute the Frobenius norm
  double  Hnorm = 
    H_[0][0] * H_[0][0] + H_[0][1] * H_[0][1] + H_[0][2] * H_[0][2] +
    H_[1][0] * H_[1][0] + H_[1][1] * H_[1][1] + H_[1][2] * H_[1][2] +
    H_[2][0] * H_[2][0] + H_[2][1] * H_[2][1] + H_[2][2] * H_[2][2];
  Hnorm = sqrt(Hnorm);

  Hscale_ = Hnorm;

  Params.push_back(H_[0][0]/Hnorm);
  Params.push_back(H_[0][1]/Hnorm);
  Params.push_back(H_[0][2]/Hnorm);
  Params.push_back(H_[1][0]/Hnorm);
  Params.push_back(H_[1][1]/Hnorm);
  Params.push_back(H_[1][2]/Hnorm);
  Params.push_back(H_[2][0]/Hnorm);
  Params.push_back(H_[2][1]/Hnorm);

  return true;
}


bool Parametrization::ParamsToHMatrix( HMatrix &Mat,
                                       const vector<double> &Params )
{
  if ( Params.size() != 8 )
    {
      BIASERR("There must be 8 parameters to compose the H matrix!");
      return false;
    }
  HParams_ = Params; // set class member

  Mat[0][0]=Params[0];
  Mat[0][1]=Params[1];
  Mat[0][2]=Params[2];
  Mat[1][0]=Params[3];
  Mat[1][1]=Params[4];
  Mat[1][2]=Params[5];
  Mat[2][0]=Params[6];
  Mat[2][1]=Params[7];

  /* compute the 3,3 element, using the property from parametrization
     where ||H|| = 1 (||.|| = Frobenius norm) */
  Mat[2][2]=sqrt(1 - 
    (Params[0] * Params[0] +
     Params[1] * Params[1] +
     Params[2] * Params[2] +
     Params[3] * Params[3] +
     Params[4] * Params[4] +
     Params[5] * Params[5] +
     Params[6] * Params[6] +
     Params[7] * Params[7] ));
  return true;
}


double Parametrization::GetBiggestAbsEntry()
{
  double max=0.0;
  for (int r=0;r<2;r++)
    for (int c=0;c<2;c++)
      if ( abs(F_[r][c])>abs(max) )
        {
          max = F_[r][c];
          Frow_ = r;
          Fcol_ = c;
        }
  return max;
}



bool Parametrization::EMatrixToParams( BIAS::EMatrix &Mat,
                                       Vector<double> &Params, 
                                       std::vector<BIAS::HomgPoint2D> &inlier1,
                                       std::vector<BIAS::HomgPoint2D> &inlier2,
                                       bool UseQuaternion)
{
  if (Params.size() != ((UseQuaternion)?6:NUM_PARAM_ESSENTIAL)){
    BIASERR (" parameter vector must have  length " 
             <<((UseQuaternion)?(6):(NUM_PARAM_ESSENTIAL))
             << " instead of " << Params.size());
    BIASABORT;
  }
  RMatrix R;
  Vector3<double> C;
  Mat.GetRotationTranslation(R,C,inlier1,inlier2);
  // now parameterization of translation
  Vector3<double> CSpherical(C.CoordEuclideanToSphere());
    
  // parameterize translation
  Params[0] = CSpherical[1];
  Params[1] = CSpherical[2];
  
  if (UseQuaternion){
    Quaternion<double> q;
    if (R.GetQuaternion(q)!=0){
      BIASERR("unable to extract quaternion from R");
      BIASABORT;
    }
    Params[2] = q[0]; Params[3]=q[1]; Params[4]=q[2]; Params[5]=q[3];
  } else {
    // now parameterization of rotation
    double angle;
    Vector3<double> rax;
    R.GetRotationAxisAngle(rax,angle); 
    if (fabs(angle)<MINIMUM_ROTATION){
      rax.Set(1.0, 0.0, 0.0);
      angle = 0;
    } 
    rax *= angle/rax.NormL2();


    // parameterize rotation
    Params[2] = rax[0]; Params[3]=rax[1]; Params[4]=rax[2];
  }
  //cout << "params: "<<Params<<endl;
  return true;
}
    
 
bool Parametrization::ParamsToEMatrix( BIAS::EMatrix &Mat,
                                       const Vector<double> &Params,
                                       bool UseQuaternion){
  double c1 = 1.0;//-Params[0]*Params[0]-Params[1]*Params[1];
    //  c3 = (c3 >= 0.0) ? sqrt(c3) : sqrt(-1.0*c3);  
  Vector3<double> C(c1, Params[0],Params[1]);
  C = C.CoordSphereToEuclidean();
  RMatrix R;
  if (UseQuaternion){
    if (Params.size() != 6){
      BIASERR("invalid length of parameter vector "<<Params.Size());
      BIASABORT;
    }
    Quaternion<double> q(Params[2],Params[3],Params[4], Params[5]);
    R.SetFromQuaternion(q);
  } else {
    if (Params.size() != NUM_PARAM_ESSENTIAL){
      BIASERR (" parameter vector has length "<<Params.size()<<" instead of "
               << NUM_PARAM_ESSENTIAL);
      BIASABORT;
    }
    Vector3<double> rax(Params[2],Params[3],Params[4]);
    double angle = rax.NormL2();

    if (fabs(angle)<MINIMUM_ROTATION){
      rax[0] = 1;
      rax[1] = 0;
      rax[2] = 0;
      angle = 0;
    } else {
      angle = rax.NormL2();
      rax /= rax.NormL2();
    }

    //RMatrix R(rax,angle);
    R.Set(rax, angle);
  }
  
  Mat.Set(R,C);
  //cout << "ParamsToEMatrix: Params="<<Params<<" R=" << R << ", C = " << C <<endl;
  Mat /= Mat.NormFrobenius();
 
  return true;

}