Pose.hh

/*
This file is part of the BIAS library (Basic ImageAlgorithmS).

Copyright (C) 2003-2009    (see file CONTACT for details)
  Multimediale Systeme der Informationsverarbeitung
  Institut fuer Informatik
  Christian-Albrechts-Universitaet Kiel


BIAS is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation; either version 2.1 of the License, or
(at your option) any later version.

BIAS is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License
along with BIAS; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/


#ifndef __Pose_hh__
#define __Pose_hh__

#include <bias_config.h>
#include <Base/Geometry/Quaternion.hh>
#include <Base/Geometry/HomgPoint3D.hh>
#include <Base/Geometry/HomgPoint3DCov.hh>
#include <Base/Math/Matrix4x4.hh>
#include <Base/Math/Matrix3x4.hh>
#include <Base/Geometry/RMatrixBase.hh>
#include <Geometry/CoordinateTransform3D.hh>
#include <Base/Geometry/PoseParametrization.hh>
#ifdef BIAS_HAVE_XML2
#  include <Base/Common/XMLBase.hh>
#endif

#define POSE_TYPE double

namespace BIAS {

  /** @class Pose

      @brief Represents 3d pose transformations, parametrized as Euclidean
            translation and unit quaternion orientation.

      Pose transformations are special 3d coordinate transformations without
      scaling, extended by covariance matrices to measure uncertainty.

      They specify Euclidean transformations that can also be interpreted as
      measureable entities, i.e. a (camera) pose is measured within in the
      coordinate system A. In A the pose will define the transformation
      from coordinates in A to the pose (camera) coordinate system. This
      transformation does not contain scale. Imagine A being embedded in
      another coordinate system B. In order to find the pose in B the method
      TransformBy() can be used. The result will not be able to transform
      coordinates from A into the pose system. This transform can be found
      using the method CoordinateTransform3D::ConcatenateLocalTransform()
      on a pose object.

      @see BIAS::CoordinateTransform3D

      @ingroup g_geometry

      @author woelk/koeser/bartczak
      @date03/2006
  */

  class BIASGeometry_EXPORT Pose : public CoordinateTransform3D
#ifdef BIAS_HAVE_XML2
    , public XMLBase
#endif
  {
  public:

    /** @brief Default constructor creates identity pose transformation. */
    Pose();

    ~Pose();

    /** @brief Set pose from pose parametrization. */
    int Set(const PoseParametrization& pose);

    /** @brief Set rotation (resp. orientation) from unit quaternion and
               translation (resp. origin) from Euclidean vector.
        @attention Resets covariance matrix to zero!
    */
    void Set(const Quaternion<POSE_TYPE>& Q,
             const Vector3<POSE_TYPE>& C);

    /** @brief Set rotation (resp. orientation) from unit quaternion and
               translation (resp. origin) from Euclidean vector, uncertainty
               is set from 7x7 covariance matrix containing Cov([C Q]).
    */
    void Set(const Quaternion<POSE_TYPE>& Q,
             const Vector3<POSE_TYPE>& C,
             const Matrix<POSE_TYPE>& cov);

    /** @brief Set translation (resp. origin) from Euclidean vector, and
               set its uncertainty from 3x3 covariance matrix. */
    void SetC(const Vector3<POSE_TYPE>& C,
              const Matrix3x3<POSE_TYPE>& cov);

    //void SetC(const HomgPoint3D& C, const HomgPoint3DCov& cov);

    /** @brief Set rotation (resp. orientation) from unit quaternion, and
               set its uncertainty from 4x4 covariance matrix. */
    void SetQ(const Quaternion<POSE_TYPE>& Q,
              const Matrix4x4<POSE_TYPE>& cov);

    /** @brief Set rotation (resp. orientation) from unit quaternion.
        @attention Resets covariance matrix to zero! */
    void SetQ(const Quaternion<POSE_TYPE>& Q);


    /** @brief Set translation (resp. origin) from Euclidean vector.
        @attention Resets covariance matrix to zero! */
    void SetC(const Vector3<POSE_TYPE>& C);

    /** @brief Get rotation (resp. orientation) as rotation matrix. */
    inline void SetR(const RMatrixBase& R)
    {
      Quaternion<ROTATION_MATRIX_TYPE> Q;
      R.GetQuaternion(Q);
      SetQ(Quaternion<POSE_TYPE>(Q));
    }

    /** @brief Get pose parametrization representing this pose. */
    PoseParametrization GetPoseParameters() const;

    /** @brief Set covariance matrix explicitely from 7x7 matrix Cov([C Q]). */
    int SetCovariance(const Matrix<POSE_TYPE>& c);

    /** @brief Set covariance matrix explicitely from an Euclidean covariance
               matrix, i.e. a 6x6 matrix Cov([C alpha beta gamma]) where
               alpha, beta, gamma are rotation angles.
        @attention This method is not implemented yet!
    */
    int SetCovEuclidean(Matrix<POSE_TYPE> Cov);

    /** @brief Return the 7x7 covariance matrix Cov([C Q]). */
    inline const Matrix<POSE_TYPE>& GetCovariance() const
    { return Cov_; }

    /** @brief Return the 3x3 covariance matrix Cov(C) of the translation
               part C of this pose transformation. */
    Matrix3x3<POSE_TYPE> GetCovC() const;

    /** @brief Return the 4x4 covariance matrix Cov(Q) of the rotational part
               of this pose transformation described by unit quaternion Q. */
    Matrix4x4<POSE_TYPE> GetCovQ() const;

    /** @brief Load pose transformation from a text file using operator >>.
        @param[in] inputFile Name of file to read from
        @see CoordinateTransform3D::Load()
        @return Returns false if reading from file failed, true otherwise.
    */
    bool Load(const std::string& inputFile);
    
    /** @brief Write pose transformation to a text file using operator <<.
        @param[in] outputFile Name of file to write to
        @see CoordinateTransform3D::Save()
        @return Returns false if writing to file failed, true otherwise.
    */
    bool Save(const std::string& outputFile);

#ifdef BIAS_HAVE_XML2
    /** @brief Specialization of XML block name function. */
    virtual int XMLGetClassName(std::string& TopLevelTag,
                                double& Version) const;

    /** @brief Specialization of XML write function. */
    virtual int XMLOut(const xmlNodePtr Node, BIAS::XMLIO& XMLObject) const;

    /** @brief Specialization of XML read function. */
    virtual int XMLIn(const xmlNodePtr Node, BIAS::XMLIO& XMLObject);
#endif

    friend std::ostream& operator<<(std::ostream &os, const Pose& p);

    friend std::istream& operator>>(std::istream &is, Pose& p);
    
    /** @brief Copy from another pose transformation instance. */
    const Pose& operator=(const Pose& p) {
      CoordinateTransform3D::operator=(p);
      Cov_ = p.GetCovariance();
      return (*this);
    }

    /** @brief Transforms the position and orientation stored in this pose
        into another global coordinate system, i.e.: trsf * this  = new local
        coordinate system.
        @param[in] trsf Coordinate transformation describing the global coordinate
                        system for the current global coordinate system.
     */
    void TransformToNewGlobalBy(const CoordinateTransform3D& trsf);

    Vector3<double> GetZAxis() const;

    Vector3<double> GetXAxis() const;

    Vector3<double> GetYAxis() const;

    /** @brief Create pose of camera looking at given point, similar to gluLookAt().
        @param[in] eye Specifies the eye coordinates
        @param[in] center Specifies the 3d point to look at
        @param[in] up Specifies the up vector of the camera
        @return Returns true if everything went fine, otherwise false.
    */
    const inline bool LookAt(const Vector3<double> &eye, Vector3<double> center,
                             const Vector3<double> &up)
    {
      center.SubIP(eye);
      bool retVal = CheckLookAt_(center, up);
      RMatrixBase r;
      r.SetFromOriUp(center, up);
      SetR(r);
      return retVal;
    };

    /** @brief Create pose of camera looking at given point, similar to gluLookAt().
        @attention The camera center is not changed here!
        @param[in] center Specifies the 3d point to look at
        @param[in] up Specifies the up vector of the camera
        @return Returns true if everything went fine, otherwise false.
    */
    const inline bool LookAt(const Vector3<double> &center, const Vector3<double> &up)
    {
      return LookAt(GetC(), center, up);
    };

    /** @brief Create pose of camera looking at given point, similar to gluLookAt().
        @param[in] eye Specifies the eye coordinates
        @param[in] center Specifies the 3d point to look at
        @param[in] up Specifies the up vector of the camera
        @return Returns true if everything went fine, otherwise false.
    */
    const inline bool LookAtGL(const Vector3<double> &eye, Vector3<double> center,
                               const Vector3<double> &up)
    {
      return LookAt(eye, center, up);
    };

    /** @brief Looks at given point, similar to gluLookAt
        @param center 3D point to look at
        @param up Up vector of the camera
        @return true, if everythings fine - false, otherwise
    */
    const inline bool LookAtGL(const Vector3<double> &center, const Vector3<double> &up){
      return LookAtGL(GetC(), center, up);
    };

  protected:

    //using CoordinateTransform3D::SetScale;

    /** @brief Scale can not be set for poses but is fixed as 1.
        @attention Calling this method will result in a program abort!
    */
    virtual void SetScale(const double& scale) { BIASABORT; }

    /** @brief Checks constraints for parameter of LookAt(). */
    inline bool CheckLookAt_(Vector3<double> &center, const Vector3<double> &up)
    {
      if(center.NormL2() < std::numeric_limits<double>::epsilon()){
        BIASERR("Vectors eye and center are the same!")
        return false;
      }
      if(fabs(up.NormL2() - 1.0) > std::numeric_limits<double>::epsilon()){
        BIASERR("Vector up is not normalized!")
        return false;
      }
      return true;
    }

    /** @brief 7x7 covariance matrix for the 7-vector representation of
               this pose transformation (first C, then Q!)*/
    Matrix<POSE_TYPE> Cov_;
  };

  /** @brief Write pose transformation to stream.
      @see Pose::Save()
  */
  inline std::ostream& operator<<(std::ostream &os, const Pose& p) {
    os<<"Q_= "<<p.Q_<<" C_= "<<p.C_<<" scale_= "<<p.scale_<<" Cov_="<<p.Cov_;
    os<<"RAW";
    for(int i=0; i < 4; i++){
      for(int j=0; j < 4; j++){
        os<<p[i][j]<< " ";
      }
      os<<std::endl;
    }
    return os;
  }

  /** @brief Read pose transformation from stream.
      @see Pose::Load()
  */
  inline std::istream& operator>>(std::istream &is, Pose& p){
    std::string temp;
    is>>temp>>p.Q_>>temp>>p.C_>>temp>>p.scale_>>temp>>p.Cov_;
    if(is && p.scale_!=1.0) {
      BIASERR("Loaded transformation is not a pose - resetting scale!\n");
      p.scale_ = 1.0;
    }
    return is;
  }

}

#endif