CovEuler2Quaternion.cpp

#include <Geometry/CovEuler2Quaternion.hh>
#include <Base/Debug/Exception.hh>
#include <Base/Geometry/RMatrixBase.hh>
#include <Base/Geometry/RMatrixBase.hh>
#include <Base/Geometry/Quaternion.hh>

using namespace BIAS;
using namespace std;

PoseParametrizationCovariance 
CovEuler2Quaternion::CTransform(const Matrix<double>& cov_pose,
                               const Vector<double>& pose)
{
  Matrix<double> ncov;
  Vector<double> npos;
  if (UnscentedTransform::Transform(pose, cov_pose, npos, ncov)!=0){
    BEXCEPTION("unable to transform");
  }
  return ncov;
}

int 
CovEuler2Quaternion::Transform_(const Vector<double>& src,
                                Vector<double>& dst) const
{ 
  Vector3<double> euler(src[3], src[4], src[5]);
  RMatrixBase R;
  R.SetZYX(euler);
  Quaternion<double> quat;
  R.GetQuaternion(quat);
  dst.newsize(7);
  for (int i=0; i<3; i++){
    dst[i] = src[i];
  }
  for (int i=3; i<7; i++){
    dst[i] = quat[i-3];
  }
  return 0;
}