EParametrization.cpp

#include "EParametrization.hh"

#include <Base/Debug/Exception.hh>
#include <Base/Common/CompareFloatingPoint.hh>
#include <Base/Geometry/HomgPoint2D.hh>
#include <Base/Geometry/HomgPoint2DCov.hh>


using namespace BIAS;
using namespace std;

EParametrization::
EParametrization()
  : E_(7, 0.0), Cov_(7, 7, MatrixZero)
{
  E_[6] = 1.0;
}


EParametrization::
EParametrization(const Vector<EP_TYPE>& EQvec,
                 const Matrix<EP_TYPE>& covariance)
{
  Set(EQvec, covariance);
}


EParametrization::
EParametrization(const Vector3<EP_TYPE>& epipole,
                 const Quaternion<EP_TYPE>& orientation,
                 const Matrix<EP_TYPE>& covariance)
{
  Set(epipole, orientation, covariance);
}


void EParametrization::
Set(const Vector3<EP_TYPE>& epipole, const Quaternion<EP_TYPE>& orientation,
    const Matrix<EP_TYPE>& covariance)
{
  Vector<EP_TYPE> EQvec(7);
  EQ2E_(epipole, orientation, EQvec);
  Set(EQvec, covariance);
}

void EParametrization::
SetEQ(const Vector3<EP_TYPE>& epipole,
      const Quaternion<EP_TYPE>& orientation)
{
  Vector<EP_TYPE> EQvec(7);
  EQ2E_(epipole, orientation, EQvec);
  E_ = EQvec;
  Cov_.SetZero();
}


void EParametrization::
SetEQ(const Vector<EP_TYPE>& EQvec)
{
  if (EQvec.size() != 7){
    BEXCEPTION("Invalid vector size (must be 7, " << EQvec.size() << " given)!");
  }
  E_ = EQvec;
  Cov_.SetZero();
}


void EParametrization::
SetCovarianceMatrix(const Matrix<EP_TYPE>& Cov)
{
  if(Cov.num_rows() != 7 || Cov.num_cols() != 7) {
    BEXCEPTION("Dimension of given covariance matrix is invalid "
               "(7x7 matrix needed, " << Cov.num_rows() << "x"
               << Cov.num_cols() << " matrix given)!\n");
  }
  // todo: check if covariance matrix is symmetric and positive definite
  Cov_ = Cov;
}


Matrix<EP_TYPE> EParametrization::
GetCovarianceMatrix() const{
  Matrix<EP_TYPE> res = Cov_;
  return res;
}


Vector3<EP_TYPE> EParametrization::
GetEpipole3() const
{
  Vector3<EP_TYPE> e;
  Quaternion<EP_TYPE> q;
  E2EQ_(E_, e, q);
  return e;
}


void EParametrization::
GetEpipole(Vector3<EP_TYPE>& epipole) const
{
  Quaternion<EP_TYPE> q;
  E2EQ_(E_, epipole, q);
}


Quaternion<EP_TYPE> EParametrization::
GetOrientation() const
{
  Vector3<EP_TYPE> e;
  Quaternion<EP_TYPE> q;
  E2EQ_(E_, e, q);
  return q;
}


void EParametrization::
SetEpipole(const Vector3<EP_TYPE>& e)
{
  E_[0] = e[0];
  E_[1] = e[1];
  E_[2] = e[2];
  Cov_.SetZero();
}


void EParametrization::
SetOrientation(const Quaternion<EP_TYPE>& q)
{
  E_[3] = q[0];
  E_[4] = q[1];
  E_[5] = q[2];
  E_[6] = q[3];
  Cov_.SetZero();
}

void EParametrization::
GetEssentialMatrix(Matrix3x3<EP_TYPE>& E) const
{
  Vector3<EP_TYPE> epipole;
  GetEpipole(epipole);
  Quaternion<EP_TYPE> Q = GetOrientation();
  RMatrixBase R;
  R.SetFromQuaternion(Q);
  E.SetAsCrossProductMatrix(epipole);
  E = E * R;
}

Matrix3x3<EP_TYPE> EParametrization::
GetEssentialMatrix() const
{
  Matrix3x3<EP_TYPE> E;

  GetEssentialMatrix(E);

  return E;
}


bool EParametrization::
Load(const std::string& file)
{
  std::ifstream ifs(file.c_str());
  if(!ifs)
    return false;
  ifs>>*this;
  if(!ifs) {
    ifs.close();
    return false;
  }
  ifs.close();
  return true;
}


bool EParametrization::
Save(const std::string& file) const
{
  std::ofstream ofs(file.c_str());
  if(!ofs)
    return false;
  ofs<<*this;
  if(!ofs) {
    ofs.close();
    return false;
  }
  ofs.close();
  return true;
}


Matrix4x4<EP_TYPE> EParametrization::
GetOrientationCov() const
{
  Matrix4x4<EP_TYPE> res;
  for (int r=0; r<4; r++){
    for (int c=0; c<4; c++){
      res[r][c] = Cov_[r+3][c+3];
    }
  }
  return res;
}


void EParametrization::
GetEpipoleCov(Matrix3x3<EP_TYPE>& cov) const
{
  for (int r=0; r<3; r++){
    for (int c=0; c<3; c++){
      cov[r][c] = Cov_[r][c];
    }
  }
}

// Matrix3x3<EP_TYPE> EParametrization::
// GetEpipoleCov() const
// {
//   Matrix3x3<EP_TYPE> res;
//   for (int r=0; r<3; r++){
//     for (int c=0; c<3; c++){
//       res[r][c] = Cov_[r][c];
//     }
//   }
//   return res;
// }


void EParametrization::
Invert()
{
  Quaternion<EP_TYPE> q = GetOrientation();
  q.Invert();
  Vector3<EP_TYPE> ep, tmp;
  GetEpipole(tmp);
  q.MultVec(tmp, ep);
  ep *= -1.0;
  E_[0] = ep[0];
  E_[1] = ep[1];
  E_[2] = ep[2];
  E_[3] = q[0];
  E_[4] = q[1];
  E_[5] = q[2];
  E_[6] = q[3];
  // the covariance does not need to be modified
}


void EParametrization::
Normalize(const bool update_covariance)
{
  BIASASSERT(E_.Size()==7u);

  Vector4<double> q;
  double quat_norm = 0.0;
  for (int i=3; i<7; i++){
    quat_norm += E_[i] * E_[i];
    q[i-3] = E_[i];
  }
  quat_norm = sqrt(quat_norm);
  BIASASSERT(!Equal(quat_norm, 0.0));
  for (int i=3; i<7; i++){
    E_[i] /= quat_norm;
  }

  Vector3<double> e;
  double ep_norm = 0.0;
  for (int i=0; i<3; i++){
    ep_norm += E_[i] * E_[i];
    e[i] = E_[i];
  }
  ep_norm = sqrt(ep_norm);
  BIASASSERT(!Equal(ep_norm, 0.0));
  for (int i=0; i<3; i++){
    E_[i] /= ep_norm;
  }

  // Update covariance of normalized parameter vector
  // by linear approximation of the normalization function
  // @author esquivel 01/2010
  // @todo use Unscented Transform instead!
  if (update_covariance)
  {
    // The Jacobian of normalization is given by:
    // J = [ Je 0 
    //       0 Jq ] with
    // Je = 1/|e|(I - ee^T/e^Te) and
    // Jq = 1/|q|(I - qq^T/q^Tq) 
    // (compare Foerstner 2004, "Uncertainty and Projective Geometry")
    Matrix3x3<double> Je(MatrixIdentity);
    Matrix3x3<double> tmp_e = e.OuterProduct(e);
    Matrix4x4<double> Jq(MatrixIdentity);
    Matrix4x4<double> tmp_q = q.OuterProduct(q);
    tmp_e /= e.ScalarProduct(e);
    tmp_q /= q.ScalarProduct(q);
    Je -= tmp_e;
    Je /= ep_norm;
    Jq -= tmp_q;
    Jq /= quat_norm;
    Matrix<EP_TYPE> J(7, 7, MatrixZero);
    for (int i=0; i<3; i++) {
      for (int j=0; j<3; j++) {
        J[i][j] = Je[i][j];
      }
    }
    for (int i=0; i<4; i++) {
      for (int j=0; j<4; j++) {
        J[i+3][j+3] = Jq[i][j];
      }
    }

    // The covariance transforms as J * cov * J^T
    Cov_ = J * Cov_ * J.Transpose();

    //BIASERR("unfinished");
    //BIASABORT;
  }
}


std::ostream& BIAS::
operator<<(std::ostream &os, const EParametrization& p)
{
  os<< "epipole= "<< p.GetEpipole3()<< " orientation= "<< p.GetOrientation();
  os<< " cov= "<< p.GetCovarianceMatrix();
  return os;
}


std::istream& BIAS::
operator>>(std::istream &is, EParametrization& p)
{
  std::string temp;
  Vector3<EP_TYPE> epipole;
  Quaternion<EP_TYPE> orientation;
  Matrix<EP_TYPE> cov;
  is>>temp>>epipole>>temp>>orientation>>temp>>cov;
  p.Set(epipole, orientation, cov);
  return is;
}