CovTransformPose.hh

#ifndef __COVTRANSFORMPOSE_HH__
#define __COVTRANSFORMPOSE_HH__

#include <MathAlgo/UnscentedTransform.hh>

namespace BIAS {

  //forward declaration (reduce number of included headers)
  template <class T> class BIASMathBase_EXPORT Vector;
  template <class T> class BIASMathBase_EXPORT Vector3;
  template <class T> class BIASMathBase_EXPORT Matrix;
  template<class T> class BIASGeometryBase_EXPORT Quaternion;

  //types of transformations available (feel free to add some more!)
  enum ETransformType { NONE, Quat2EulerZYX, EulerZYX2Quat,
                        PoseTransform, Point3DTransform };

  /** @class CovTransformPose

      @brief Transformations between different representations of poses and
             their covariances (e.g. convert a pose with given cov. represented
             by position and euler-angles to a pose with according cov.
             represented by position and a unit-quaternion).

      @Author apetersen 04/2008
   */
  class BIASGeometry_EXPORT CovTransformPose : public BIAS::UnscentedTransform
  {
  public:
    /** @brief Standard Ctor, no params needed */
    CovTransformPose();

    virtual ~CovTransformPose(){}

    /** @brief Convert a pose given as a position, a quaternion and the 
               according 7x7-covariance-matrix to a pose given as position (same
               as bevor), rotation in euler-angles (order of rotation-axis:
               ZYX) and the according 6x6-covariance-matrix.
                         |   pos   rotXpos |
               cov-Mat = | posXrot   rot   |

        @return  0 in case of success
                -1 in case of any error
    */
    int PoseQuatToEulerZYX(const Vector3<double> &posQ,
                           const Quaternion<double> &rotQ,
                           const Matrix<double> &covQ,
                           Vector3<double> &posE, Vector3<double> &rotE,
                           Matrix<double> &covE);

    /** @brief Same as above, just other way around!

        @return  0 in case of success
                -1 in case of any error
    */
    int PoseEulerZYXToQuat(const Vector3<double> &posE,
                           const Vector3<double> &rotE,
                           const Matrix<double> &covE,
                           Vector3<double> &posQ, 
                           Quaternion<double> &rotQ,
                           Matrix<double> &covQ);

    /** @brief Transforms a pose Q,C by a rigid transformation dQ,dt such
               that the resulting pose is dQ*Q, dQ*C*dQ' + dt.

        @return  0 in case of success
                -1 in case of any error
    */
    int 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);

    /** @brief Transforms a 3D point X by a rigid transformation Q,t such
               that the resulting 3D point is Q*X*Q' + t.

        @return  0 in case of success
                -1 in case of any error
    */
    int 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);

  protected:
    //Performs the point transition for unscented transform
    virtual int Transform_(const Vector<double>& src, 
                           Vector<double>& dst) const;

    //Tells Transform_ what transformation is to be used
    ETransformType tType_;
  };


} // namespace

#endif // __COVTRANSFORMPOSE_HH__