CovTransformPose.cpp

#include "CovTransformPose.hh"

#include <Base/Math/Vector3.hh>
#include <Base/Math/Matrix.hh>
#include <Geometry/RMatrix.hh>
#include <Base/Geometry/Quaternion.hh>

using namespace std;
using namespace BIAS;

CovTransformPose::CovTransformPose()
{
  tType_ = NONE;
}

int CovTransformPose::
PoseQuatToEulerZYX(const Vector3<double> &posQ, const Quaternion<double> &rotQ,
                   const Matrix<double> &covQ,
                   Vector3<double> &posE, Vector3<double> &rotE,
                   Matrix<double> &covE)
{
  int res = 0;

  //tell Transform_ what we want to do
  tType_ = Quat2EulerZYX;

  //generate source vector
  Vector<double> src(7), dst;
  for (unsigned int i = 0; i < 3; i++) src[i] = posQ[i];
  for (unsigned int i = 0; i < 4; i++) src[i+3] = rotQ[i];
  BIASASSERT(covQ.num_rows() == 7 && covQ.num_cols() == 7);

  //call unscented transform (uses Transform_ to transform src and samplepoints)
  if (Transform(src, covQ, dst, covE) != 0) {
    BIASERR("Error in Unscented Transform!");
    res = -1;
  } else {
    //get resulting pose from target vector
    for (unsigned int i = 0; i < 3; i++) posE[i] = dst[i];
    for (unsigned int i = 0; i < 3; i++) rotE[i] = dst[i+3];
  }

  //transformation is done
  tType_ = NONE;

  return res;
}

int CovTransformPose::
PoseEulerZYXToQuat(const Vector3<double> &posE, const Vector3<double> &rotE,
                   const Matrix<double> &covE,
                   Vector3<double> &posQ, Quaternion<double> &rotQ,
                   Matrix<double> &covQ)
{
  int res = 0;

  //tell Transform_ what we want to do
  tType_ = EulerZYX2Quat;

  //generate source vector
  Vector<double> src(6), dst;
  for (unsigned int i = 0; i < 3; i++) src[i] = posE[i];
  for (unsigned int i = 0; i < 3; i++) src[i+3] = rotE[i];
  BIASASSERT(covE.num_rows() == 7 && covE.num_cols() == 7);

  //call unscented transform (uses Transform_ to transform src and samplepoints)
  if (Transform(src, covE, dst, covQ) != 0) {
    BIASERR("Error in Unscented Transform!");
    res = -1;
  } else {
    //get resulting pose from target vector
    for (unsigned int i = 0; i < 3; i++) posQ[i] = dst[i];
    for (unsigned int i = 0; i < 4; i++) rotQ[i] = dst[i+3];
  }

  //transformation is done
  tType_ = NONE;

  return res;
}


int CovTransformPose::
TransformPose(const Vector3<double> &inputPos,
              const Quaternion<double> &inputRot,
              const Matrix<double> &inputCov,
              const Vector3<double> &translate,
              const Quaternion<double> &rotate,
              const Matrix<double> &transformCov,
              Vector3<double> &resultPos,
              Quaternion<double> &resultRot,
              Matrix<double> &resultCov)
{
  int res = 0;

  //tell Transform_ what we want to do
  tType_ = PoseTransform;

  //generate source vector
  Vector<double> src(14), dst;
  for (unsigned int i = 0; i < 3; i++) {
    src[i]   = inputPos[i];
    src[i+7] = translate[i];
  }
  for (unsigned int i = 0; i < 4; i++) {
    src[i+3]  = inputRot[i];
    src[i+10] = rotate[i];
  }

  //generate source covariance matrix
  BIASASSERT(inputCov.num_rows() == 7 && inputCov.num_cols() == 7 &&
             transformCov.num_rows() == 7 && transformCov.num_cols() == 7);
  Matrix<double> srcCov(14, 14, MatrixZero), dstCov;
  for (unsigned int i = 0; i < 7; i++) {
    for (unsigned int j = 0; j < 7; j++) {
      srcCov[i][j] = inputCov[i][j];
      srcCov[i+7][j+7] = transformCov[i][j];
    }
  }

  //call unscented transform (uses Transform_ to transform src and samplepoints)
  if (Transform(src, srcCov, dst, dstCov) != 0) {
    BIASERR("Error in Unscented Transform!");
    res = -1;
  } else {
    //get resulting pose from target vector
    for (unsigned int i = 0; i < 3; i++) resultPos[i] = dst[i];
    for (unsigned int i = 0; i < 4; i++) resultRot[i] = dst[i+3];
    resultCov = dstCov;
  }

  //transformation is done
  tType_ = NONE;

  return res;
}


int CovTransformPose::
TransformPoint3D(const Vector3<double> &point3D,
                 const Matrix3x3<double> &cov3D,
                 const Vector3<double> &translate,
                 const Quaternion<double> &rotate,
                 const Matrix<double> &transformCov,
                 Vector3<double> &resultPoint3D,
                 Matrix3x3<double> &resultCov3D)
{
  int res = 0;

  //tell Transform_ what we want to do
  tType_ = Point3DTransform;

  //generate source vector
  Vector<double> src(10), dst;
  for (unsigned int i = 0; i < 3; i++) src[i] = point3D[i];
  for (unsigned int i = 0; i < 3; i++) src[i+3] = translate[i];
  for (unsigned int i = 0; i < 4; i++) src[i+6] = rotate[i];

  //generate source covariance matrix
  BIASASSERT(transformCov.num_rows() == 7 && transformCov.num_cols() == 7);
  Matrix<double> srcCov(10, 10, MatrixZero), dstCov;
  for (unsigned int i = 0; i < 3; i++) {
    for (unsigned int j = 0; j < 3; j++) {
      srcCov[i][j] = cov3D[i][j];
    }
  }
  for (unsigned int i = 0; i < 7; i++) {
    for (unsigned int j = 0; j < 7; j++) {
      srcCov[i+3][j+3] = transformCov[i][j];
    }
  }

  // [HACK] Enfore symmetry of covariance matrix!!
  srcCov.MakeSymmetric();

  //call unscented transform (uses Transform_ to transform src and samplepoints)
  if (Transform(src, srcCov, dst, dstCov) != 0) {
    BIASERR("Error in Unscented Transform!");
    res = -1;
  } else {
    //get resulting 3D point from target vector
    for (unsigned int i = 0; i < 3; i++) resultPoint3D[i] = dst[i];
    for (unsigned int i = 0; i < 3; i++) {
      for (unsigned int j = 0; j < 3; j++) {
        resultCov3D[i][j] = dstCov[i][j];
      }
    }
  }

  //transformation is done
  tType_ = NONE;

  return res;
}


int CovTransformPose::
Transform_(const Vector<double>& src, Vector<double>& dst) const
{
  int res = 0;

  //switch what's to do
  switch (tType_) {

  //Convert quaternion to euler-angles
  case Quat2EulerZYX:
    {
      //get rotation-matrix from quaternion
      RMatrix R;
      R.SetFromQuaternion(Quaternion<double>(src[3], src[4], src[5], src[6]));
      //get ratation-angles from RMatrix
      double xPhi, yPhi, zPhi;
      if (R.GetRotationAnglesZYX(xPhi, yPhi, zPhi) != 0) {
        //decomposition failed, return error
        BIASERR("Error in matrix decomposition!");
        res = -1;
      } else {
        //decomposition successfull, compose destination-vector...
        dst.newsize(6);
        //...copy position...
        for (unsigned int i=0; i<3; i++) dst[i] = src[i];
        //...and set euler-angles
        dst[3] = xPhi;
        dst[4] = yPhi;
        dst[5] = zPhi;
        res = 0;
      }
    }
    break;

  //Convert euler-angles to quaternion
  case EulerZYX2Quat:
    {
      //generate quaternion from given euler-angles...
      Quaternion<double> q;
      q.SetZYX(src[3], src[4], src[5]);
      //compose destination-vector...
      dst.newsize(7);
      //...copy position...
      for (unsigned int i=0; i<3; i++) dst[i] = src[i];
      //...set quaternion-values
      for (unsigned int i=0; i<4; i++) dst[i+3] = q[i];
      res = 0;
    }
    break;

  //Convert pose by a rigid transformation (rotation + translation)
  case PoseTransform:
    {
      //fetch old pose C,Q and rigid transformation dC,dQ
      Vector3<double> C(src[0],src[1],src[2]);
      Quaternion<double> Q(src[3],src[4],src[5],src[6]);
      Vector3<double> dt(src[7],src[8],src[9]);
      Quaternion<double> dQ(src[10],src[11],src[12],src[13]);
      //transform pose to dQ*Q, dQ*C*dQ'+dt
      Quaternion<double> resQ = dQ * Q;
      Vector3<double> resC = dQ.MultVec(C) + dt;
      dst.newsize(7);
      //...copy position...
      for (unsigned int i = 0; i < 3; i++) dst[i] = resC[i];
      //...set quaternion-values
      for (unsigned int i = 0; i < 4; i++) dst[i+3] = resQ[i];
      res = 0;      
    }
    break;

  //Convert 3D point by a rigid transformation (rotation + translation)
  case Point3DTransform:
    {
      //fetch 3D point X and rigid transformation Q,t
      Vector3<double> X(src[0],src[1],src[2]);
      Vector3<double> t(src[3],src[4],src[5]);
      Quaternion<double> Q(src[6],src[7],src[8],src[9]);
      //transform X to Q*X*Q'+t
      Vector3<double> resX = Q.MultVec(X) + t;
      dst.newsize(3);
      //copy 3D point
      for (unsigned int i = 0; i < 3; i++) dst[i] = resX[i];
      res = 0;      
    }
    break;

  default:
    BIASERR("We should't end up here!!!");
    BIASABORT;
  }

  return res;
}