PoseParametrizationCovariance.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 __PoseParametrizationCovariance_hh__
#define __PoseParametrizationCovariance_hh__

#include <Base/Math/Matrix.hh>
#include <Base/Math/Matrix3x3.hh>
#include <Base/Math/Matrix4x4.hh>

#define POSECOVARIANCE_TYPE double

namespace BIAS {
  // forward declarations
  template <class T> class BIASMathBase_EXPORT Vector;
  class PoseParametrization;

  /** @class PoseParametrizationCovariance
      @ingroup g_math
      @ingroup g_geometry
      @brief class encapsulating the transfromations between different pose 
      parametrizations

      This class represents the covariance of a camerpose with
      translation and rotation. The pose is assumed to be
      tx,ty,tz,q1,q2,q3,q4 with translation t and rotation q as
      quaternion. The covariance therefore must be 7x7.
      @author woelk 03/2006 */
  class BIASGeometryBase_EXPORT PoseParametrizationCovariance 
    : public Matrix<POSECOVARIANCE_TYPE>
  {
  public:
    PoseParametrizationCovariance(MatrixInitType T)
      : Matrix<POSECOVARIANCE_TYPE>(7,7,T){};

    PoseParametrizationCovariance(); 

    explicit PoseParametrizationCovariance(const Matrix<POSECOVARIANCE_TYPE>& T)
      : Matrix<POSECOVARIANCE_TYPE>(T){};

    PoseParametrizationCovariance(const TNT::Matrix<POSECOVARIANCE_TYPE>& T)
      : Matrix<POSECOVARIANCE_TYPE>(T){};

    ~PoseParametrizationCovariance();
    
    Matrix3x3<POSECOVARIANCE_TYPE> GetCovC() const;

    Matrix4x4<POSECOVARIANCE_TYPE> GetCovQ() const;

    inline void Clear() {}
    
    /** Given a 6D pose (position and euler angles) and its 
        associated covariance, the function transforms the covaruiance into 
        pose and quaternion parametrization. The 1st 3 entries of the 6D pose 
        must be the euclidean position coordinates and the last 3 entries 
        must be the Euler angles [Ax, Ay, Az] such that the final rotation 
        matrix is composed from Rz * Ry * Rx. Where Rx denotes the rotation 
        matrix describing the rotation around the x axis, .... 
        @author woelk 03/2006 */
    //void SetFromEulerAngles(const Matrix<double>& cov_pose,
    //                        const Vector<double>& pose);

    /** Transfroms the covariance associated with pose according to 
        the point transformation transf. Only affine transformations are
        supported. result is "covRes = transf*this"
    */
    void Transform(const Matrix4x4<double>& transf);

    

  protected:
  }; // class



} // namespace


#endif // __PoseParametrizationCovariance_hh__