RParametrization.cpp

#include "RParametrization.hh"
#include <Base/Common/W32Compat.hh>

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

#include <fstream>

using namespace BIAS;
using namespace std;

RParametrization::
RParametrization()
  : Q_(4, 0.0), Cov_(4, 4, MatrixZero)
{
  Q_[3] = 1.0;
}


RParametrization::
RParametrization(const Vector<Q_TYPE>& Qvec,
                 const Matrix<Q_TYPE>& covariance)
{
  Set(Qvec, covariance);
}


void RParametrization::
Set(const Vector<Q_TYPE>& Qvec) 
{
  if (Qvec.size() != 4){
    BEXCEPTION("invalid vector size, must be 4");
  }
  Q_ = Qvec;         
  Cov_.SetZero();
}


void RParametrization::
SetCovarianceMatrix(const Matrix<Q_TYPE>& Cov) 
{
  if(Cov.num_rows() != 4 || Cov.num_cols() != 4) {
    BEXCEPTION("dimension of argument covariance matrix does not match!\n");
  }
  // todo: check symmetry and positiv definitness of cov
  Cov_ = Cov;
}


Quaternion<Q_TYPE> RParametrization::
GetQ() const
{
  Vector4<double> vec = GetVec();
  Quaternion<Q_TYPE> Q(vec);
  return Q;
}


RMatrixBase RParametrization::
GetRotationMatrix() const
{
  const Quaternion<Q_TYPE> Q = GetQ();
  RMatrixBase R;
  R.SetFromQuaternion(Q);

  return R;
}


bool RParametrization::
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 RParametrization::
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;
}


std::ostream& BIAS::
operator<<(std::ostream &os, const RParametrization& p) 
{
  os<< "orientation= "<<p.GetVec();
  os<< " cov= "<<p.GetCovarianceMatrix();
  return os;  
}
 
 
std::istream& BIAS::
operator>>(std::istream &is, RParametrization& p)
{
  std::string temp;
  Vector<Q_TYPE> orientation;
  Matrix<Q_TYPE> cov;
  is>>temp>>temp>>orientation>>temp>>cov;
  p.Set(orientation, cov);
  return is;
}