PoseParametrization.cpp

#include "PoseParametrization.hh"

#include "PoseParametrizationCovariance.hh"

#include <Base/Debug/Exception.hh>
#include <Base/Common/CompareFloatingPoint.hh>
#include <Base/Math/Matrix4x4.hh>
#include <Base/Geometry/HomgPoint3D.hh>

using namespace BIAS;

PoseParametrization::
PoseParametrization()
  : Pose_(7), Cov_(7, 7, MatrixZero)
{
  Pose_.SetZero();
  // init with valid parameterizaton (norml2(quat)==1)
  Pose_[6]=1.0;
}


PoseParametrization::
PoseParametrization(const Vector<PP_TYPE>& CQvec,
                    const Matrix<PP_TYPE>& covariance)
 : Pose_(7), Cov_(7, 7, MatrixZero)
{
  Set(CQvec, covariance);
}


PoseParametrization::
PoseParametrization(const Vector3<PP_TYPE>& position,
                    const Quaternion<PP_TYPE>& orientation,
                    const Matrix<PP_TYPE>& covariance)
 : Pose_(7), Cov_(7, 7, MatrixZero) 
{
  Set(position, orientation, covariance);
}


void PoseParametrization::
Set(const Vector3<PP_TYPE>& position, const Quaternion<PP_TYPE>& orientation,
    const Matrix<PP_TYPE>& covariance)
{
  Vector<PP_TYPE> CQvec(7);
  CQ2Pose_(position, orientation, CQvec);
  Set(CQvec, covariance);
}

void PoseParametrization::
SetCQ(const Vector3<PP_TYPE>& position,
      const Quaternion<PP_TYPE>& orientation)
{
  Vector<PP_TYPE> CQvec(7);
  CQ2Pose_(position, orientation, CQvec);
  Pose_ = CQvec;         
  Cov_.SetZero();
}


void PoseParametrization::
SetCQ(const Vector<PP_TYPE>& CQvec) 
{
  if (CQvec.size() != 7){
    BEXCEPTION("invalid vector size, must be 7");
  }
  Pose_ = CQvec;         
  Cov_.SetZero();
}


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


Vector3<PP_TYPE> PoseParametrization::
GetPosition() const
{ 
  Vector3<PP_TYPE> c;
  Quaternion<PP_TYPE> q;
  Pose2CQ_(Pose_, c, q);
  return c; 
}


Quaternion<PP_TYPE> PoseParametrization::
GetOrientation() const
{ 
  Vector3<PP_TYPE> c;
  Quaternion<PP_TYPE> q;
  Pose2CQ_(Pose_, c, q);
  return q; 
}


void PoseParametrization::
SetPosition(const Vector3<PP_TYPE>& C) 
{
  Pose_[0] = C[0];
  Pose_[1] = C[1];
  Pose_[2] = C[2];
  Cov_.SetZero();
}


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


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


void BIAS::PoseParametrization::
Transform(const Vector3<PP_TYPE>& T, const Quaternion<PP_TYPE>& q)
{
  BIASERR("untested");
  Matrix4x4<PP_TYPE> Transf(MatrixIdentity);
  RMatrixBase R;
  R.SetFromQuaternion(q);
  for (int r=0; r<3; r++){
    Transf[r][3] = T[r];
    for (int c=0; c<3; c++){
      Transf[r][c] = R[r][c];
    }
  }

  Vector3<PP_TYPE> pos = GetPosition(), new_pos;
  HomgPoint3D src(pos), dst;
  dst = Transf * src;
  if (dst.IsAtInfinity()){
    BEXCEPTION("position is at infinity: "<<dst);
  } else {
    dst.GetEuclidean(new_pos);
  }
  
  Quaternion<PP_TYPE> o = GetOrientation();
  o = q * o;

  PoseParametrizationCovariance cov = GetCovarianceMatrix();
  cov.Transform(Transf);

  Set(new_pos, o, cov);
}

void BIAS::PoseParametrization::
Transform(const Vector3<PP_TYPE>& T, const Quaternion<PP_TYPE>& q, 
          const double s)
{
  BIASERR("untested");
  Matrix4x4<PP_TYPE> Transf(MatrixIdentity);
  RMatrixBase R;
  R.SetFromQuaternion(q);
  for (int r=0; r<3; r++){
    Transf[r][3] = T[r];
    for (int c=0; c<3; c++){
      Transf[r][c] = s*R[r][c];
    }
  }

  Vector3<PP_TYPE> pos = GetPosition(), new_pos;
  HomgPoint3D src(pos), dst;
  dst = Transf * src;
  if (dst.IsAtInfinity()){
    BEXCEPTION("position is at infinity: "<<dst);
  } else {
    dst.GetEuclidean(new_pos);
  }
  
  Quaternion<PP_TYPE> o = GetOrientation();
  o = q * o;

  PoseParametrizationCovariance cov = GetCovarianceMatrix();
  cov.Transform(Transf);

  Set(new_pos, o, cov);
}


std::ostream& BIAS::
operator<<(std::ostream &os, const PoseParametrization& p) 
{
  os<< "position= "<<p.GetPosition()<< " orientation= "<<p.GetOrientation()
    << " cov= "<<p.GetCovarianceMatrix();
  return os;  
}
 
 
std::istream& BIAS::
operator>>(std::istream &is, PoseParametrization& p)
{
  std::string temp;
  Vector3<PP_TYPE> position;
  Quaternion<PP_TYPE> orientation;
  Matrix<PP_TYPE> cov;
  is>>temp>>position>>temp>>orientation>>temp>>cov;
  p.Set(position, orientation, cov);
  return is;
}