PoseParametrization.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 __PoseParametrization_hh__
#define __PoseParametrization_hh__

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

#include <fstream>

namespace BIAS {

#define PP_TYPE double

  /** @class PoseParametrization
      @brief Slim class bundeling pose parametrization and associated 
      covariance matrix.

      The pose is parametrized as vector of dimension 7 where the first 3 
      entries represent the position (euclidean vector of dimension 3) and the
      last 4 entries represent the quaternion orientation.

      \author bartczak/woelk e.a. 04/2006 */
  class BIASGeometryBase_EXPORT PoseParametrization 
  {
  public:
    PoseParametrization(); 

    PoseParametrization(const Vector<PP_TYPE>& CQvec,
                        const Matrix<PP_TYPE>& covariance); 

    PoseParametrization(const Vector3<PP_TYPE>& position,
                        const Quaternion<PP_TYPE>& orientation,
                        const Matrix<PP_TYPE>& covariance);

    /** Set this from pose parametrization. Zeros associated
        covarinace matrix */
    void SetCQ(const Vector<PP_TYPE>& CQvec);

    /** Set this from position and orientation. Zeros associated
        covarinace matrix */
    void SetCQ(const Vector3<PP_TYPE>& position,
               const Quaternion<PP_TYPE>& orientation);

    /** Set this from position, orientation and covariance */
    void Set(const Vector3<PP_TYPE>& position,
             const Quaternion<PP_TYPE>& orientation,
             const Matrix<PP_TYPE>& covariance=Matrix<double>(7,7,MatrixZero));
 
    inline void Set(const Vector<PP_TYPE>& CQvec, const Matrix<PP_TYPE>& Cov)
    { SetCQ(CQvec); SetCovarianceMatrix(Cov); }
      
    /** Sets covariance matrix from Cov. Cov must be a 7x7 symmetric positiv 
        definit matrix (i.e. a covariance matrix) */
    void SetCovarianceMatrix(const Matrix<PP_TYPE>& Cov);
      
    /** returns a vector of dimension 7 where first 3 entries are
        the position and the last 4 entries are the quaternion orientation */
    inline const Vector<PP_TYPE>& GetCQ() const
    { return Pose_; }
    
    /** returns the covariance matrix associated with the CQ vector */
    inline const Matrix<PP_TYPE>& GetCovarianceMatrix() const 
    { return Cov_; }

    /** set position part of vector and zero covariance matrix */
    void SetPosition(const Vector3<PP_TYPE>& C);

    /** set orientation part of vector and zero covariance matrix */
    void SetOrientation(const Quaternion<PP_TYPE>& q);

    /** returns the position (first 3 entries) part of the pose 
        parametrization vector */
    Vector3<PP_TYPE> GetPosition() const;

    /** returns the quaternion (last 4 entries) part of the pose 
        parametrization vector */
    Quaternion<PP_TYPE> GetOrientation() const;
      
    bool Load(const std::string& file);
      
    bool Save(const std::string& file) const;

    /** Transforms the pose and associated covariance according to
        the point transformation parametrized as translation T and rotation Q
        @author woelk 05/2006 (untested) */
    void Transform(const Vector3<double>& T, const Quaternion<double>& q);

    /** Transforms the pose and associated covariance according to
        the point transformation parametrized as translation T, rotation Q
        and scale s
        @author bartczak 05/2006 (untested) */
    void Transform(const Vector3<double>& T, 
                   const Quaternion<double>& q, 
                   const double s);

  protected:
    /** first 3 entries relate to the position */
    Vector<PP_TYPE> Pose_;
    /** first columns relate to position */
    Matrix<PP_TYPE> Cov_;

    inline void Pose2CQ_(const Vector<PP_TYPE>& pose, 
                         Vector3<PP_TYPE>& position,
                         Quaternion<PP_TYPE>& orientation) const{
      position[0] = pose[0];
      position[1] = pose[1];
      position[2] = pose[2];
      orientation[0] = pose[3];
      orientation[1] = pose[4];
      orientation[2] = pose[5];
      orientation[3] = pose[6];
    };

    inline void CQ2Pose_(const Vector3<PP_TYPE>& position, 
                         const Quaternion<PP_TYPE>& orientation, 
                         Vector<PP_TYPE>& pose) const{
      pose[0] = position[0];
      pose[1] = position[1];
      pose[2] = position[2];
      pose[3] = orientation[0];
      pose[4] = orientation[1];
      pose[5] = orientation[2];
      pose[6] = orientation[3];
    };

  };

  BIASGeometryBase_EXPORT std::ostream& 
    operator<<(std::ostream &os, const PoseParametrization& p);

  BIASGeometryBase_EXPORT std::istream& 
    operator>>(std::istream &is, PoseParametrization& p);
    
}


#endif