CPose3D.h

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
   |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
   |                                                                           |
   |                       http://www.mrpt.org/                                |
   |                                                                           |
   |   Copyright (C) 2005-2011  University of Malaga                           |
   |                                                                           |
   |    This software was written by the Machine Perception and Intelligent    |
   |      Robotics Lab, University of Malaga (Spain).                          |
   |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
   |                                                                           |
   |  This file is part of the MRPT project.                                   |
   |                                                                           |
   |     MRPT is free software: you can redistribute it and/or modify          |
   |     it under the terms of the GNU General Public License as published by  |
   |     the Free Software Foundation, either version 3 of the License, or     |
   |     (at your option) any later version.                                   |
   |                                                                           |
   |   MRPT 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 General Public License for more details.                          |
   |                                                                           |
   |     You should have received a copy of the GNU General Public License     |
   |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
   |                                                                           |
   +---------------------------------------------------------------------------+ */
#ifndef CPOSE3D_H
#define CPOSE3D_H

#include <mrpt/poses/CPose.h>
#include <mrpt/math/CMatrixFixedNumeric.h>
#include <mrpt/math/CQuaternion.h>

namespace mrpt
{
namespace poses
{
        using namespace mrpt::math;

        class BASE_IMPEXP CPose3DQuat;

        DEFINE_SERIALIZABLE_PRE( CPose3D )

        /** A class used to store a 3D pose (a 3D translation + a rotation in 3D).
         *   The 6D transformation in SE(3) stored in this class is kept in two
         *   separate containers: a 3-array for the translation, and a 3x3 rotation matrix.
         *
         *  The 6D pose is parameterized as a 6-vector: [x y z yaw pitch roll]. Note however,
         *   that the yaw/pitch/roll angles are only computed (on-demand and transparently)
         *   when the user requests them. Normally, rotations are handled via the 3x3 rotation matrix only.
         *
         *  For a complete descriptionan of Points/Poses, see mrpt::poses::CPoseOrPoint, or refer
         *    to the <a href="http://www.mrpt.org/2D_3D_Geometry"> 2D/3D Geometry tutorial</a> in the wiki.
         *
         *  To change the individual components of the pose, use CPose3D::setFromValues. This class assures that the internal
         *   3x3 rotation matrix is always up-to-date with the "yaw pitch roll" members.
         *
         *  Rotations in 3D can be also represented by quaternions. See mrpt::math::CQuaternion, and method CPose3D::getAsQuaternion.
         *
         *  This class and CPose3DQuat are very similar, and they can be converted to the each other automatically via transformation constructors.
         *
         *  There are Lie algebra methods: \a exp and \a ln (see the methods for documentation).
         *
         *  <div align=center>
         *   <img src="CPose3D.gif">
         *  </div>
         *
         * \ingroup poses_grp
         * \sa CPoseOrPoint,CPoint3D, mrpt::math::CQuaternion
         */
        class BASE_IMPEXP CPose3D : public CPose<CPose3D>, public mrpt::utils::CSerializable
        {
                // This must be added to any CSerializable derived class:
                DEFINE_SERIALIZABLE( CPose3D )

        public:
                CArrayDouble<3>   m_coords; //!< The translation vector [x,y,z]
                CMatrixDouble33   m_ROT;    //!< The 3x3 rotation matrix

        protected:
                mutable bool    m_ypr_uptodate;                 //!< Whether yaw/pitch/roll members are up-to-date since the last rotation matrix update.
                mutable double  m_yaw, m_pitch, m_roll; //!< These variables are updated every time that the object rotation matrix is modified (construction, loading from values, pose composition, etc )

                /** Rebuild the homog matrix from the angles. */
                void  rebuildRotationMatrix();

                /** Updates Yaw/pitch/roll members from the m_ROT  */
                inline void updateYawPitchRoll() const { if (!m_ypr_uptodate) { m_ypr_uptodate=true; getYawPitchRoll( m_yaw, m_pitch, m_roll ); } }

         public:
                /** @name Constructors
                    @{ */

                /** Default constructor, with all the coordinates set to zero. */
                CPose3D();

                /** Constructor with initilization of the pose; (remember that angles are always given in radians!)  */
                CPose3D(const double x,const double  y,const double  z,const double  yaw=0, const double  pitch=0, const double roll=0);

                /** Constructor from a 4x4 homogeneous matrix - the passed matrix can be actually of any size larger than or equal 3x4, since only those first values are used (the last row of a homogeneous 4x4 matrix are always fixed). */
                explicit CPose3D(const math::CMatrixDouble &m);

                /** Constructor from a 4x4 homogeneous matrix: */
                explicit CPose3D(const math::CMatrixDouble44 &m);

                /** Constructor from a 3x3 rotation matrix and a the translation given as a 3-vector, a 3-array, a CPoint3D or a TPoint3D */
                template <class MATRIX33,class VECTOR3>
                inline CPose3D(const MATRIX33 &rot, const VECTOR3& xyz) : m_ROT(UNINITIALIZED_MATRIX), m_ypr_uptodate(false)
                {
                        ASSERT_EQUAL_(size(rot,1),3); ASSERT_EQUAL_(size(rot,2),3);ASSERT_EQUAL_(xyz.size(),3)
                        for (int r=0;r<3;r++)
                                for (int c=0;c<3;c++)
                                        m_ROT(r,c)=rot.get_unsafe(r,c);
                        for (int r=0;r<3;r++) m_coords[r]=xyz[r];
                }
                //! \overload
                inline CPose3D(const CMatrixDouble33 &rot, const CArrayDouble<3>& xyz) : m_coords(xyz),m_ROT(rot), m_ypr_uptodate(false)
                { }

                /** Constructor from a CPose2D object.
                */
                CPose3D(const CPose2D &);

                /** Constructor from a CPoint3D object.
                */
                CPose3D(const CPoint3D &);

                /** Constructor from lightweight object.
                */
                CPose3D(const mrpt::math::TPose3D &);

                /** Constructor from a quaternion (which only represents the 3D rotation part) and a 3D displacement. */
                CPose3D(const mrpt::math::CQuaternionDouble &q, const double x, const double y, const double z );

                /** Constructor from a CPose3DQuat. */
                CPose3D(const CPose3DQuat &);

                /** Fast constructor that leaves all the data uninitialized - call with UNINITIALIZED_POSE as argument */
                inline CPose3D(TConstructorFlags_Poses constructor_dummy_param) : m_ROT(UNINITIALIZED_MATRIX), m_ypr_uptodate(false) { }

                /** Constructor from an array with these 12 elements: [r11 r21 r31 r12 r22 r32 r13 r23 r33 tx ty tz]
                  *  where r{ij} are the entries of the 3x3 rotation matrix and t{x,y,z} are the 3D translation of the pose
                  *  \sa setFrom12Vector, getAs12Vector
                  */
                inline explicit CPose3D(const CArrayDouble<12> &vec12) : m_ROT( UNINITIALIZED_MATRIX ), m_ypr_uptodate(false) {
                        setFrom12Vector(vec12);
                }

                /** @} */  // end Constructors



                /** @name Access 3x3 rotation and 4x4 homogeneous matrices
                    @{ */

                /** Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (translation+orientation).
                  * \sa getInverseHomogeneousMatrix, getRotationMatrix
                  */
                inline void  getHomogeneousMatrix(CMatrixDouble44 & out_HM ) const
                {
                        out_HM.insertMatrix(0,0,m_ROT);
                        for (int i=0;i<3;i++) out_HM(i,3)=m_coords[i];
                        out_HM(3,0)=out_HM(3,1)=out_HM(3,2)=0.; out_HM(3,3)=1.;
                }

                inline CMatrixDouble44 getHomogeneousMatrixVal() const { CMatrixDouble44 M; getHomogeneousMatrix(M); return M;}

                /** Get the 3x3 rotation matrix \sa getHomogeneousMatrix  */
                inline void getRotationMatrix( mrpt::math::CMatrixDouble33 & ROT ) const { ROT = m_ROT; }
                //! \overload
                inline const mrpt::math::CMatrixDouble33 & getRotationMatrix() const { return m_ROT; }

                /** @} */  // end rot and HM


                /** @name Pose-pose and pose-point compositions and operators
                    @{ */

                /** The operator \f$ a \oplus b \f$ is the pose compounding operator. */
                inline CPose3D  operator + (const CPose3D& b) const
                {
                        CPose3D   ret(UNINITIALIZED_POSE);
                        ret.composeFrom(*this,b);
                        return ret;
                }

                /** The operator \f$ a \oplus b \f$ is the pose compounding operator. */
                CPoint3D  operator + (const CPoint3D& b) const;

                /** The operator \f$ a \oplus b \f$ is the pose compounding operator. */
                CPoint3D  operator + (const CPoint2D& b) const;

        /** Computes the spherical coordinates of a 3D point as seen from the 6D pose specified by this object. For the coordinate system see the top of this page. */
        void sphericalCoordinates(
            const TPoint3D &point,
            double &out_range,
            double &out_yaw,
            double &out_pitch ) const;

                /** An alternative, slightly more efficient way of doing \f$ G = P \oplus L \f$ with G and L being 3D points and P this 6D pose.
                  *  If pointers are provided, the corresponding Jacobians are returned.
                  *  "out_jacobian_df_dse3" stands for the Jacobian with respect to the 6D locally Euclidean vector in the tangent space of SE(3).
                  *  See <a href="http://www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty" >this report</a> for mathematical details.
                  *  \param  If set to true, the Jacobian "out_jacobian_df_dpose" uses a fastest linearized appoximation (valid only for small rotations!).
                  */
                void composePoint(double lx,double ly,double lz, double &gx, double &gy, double &gz,
                        mrpt::math::CMatrixFixedNumeric<double,3,3>  *out_jacobian_df_dpoint=NULL,
                        mrpt::math::CMatrixFixedNumeric<double,3,6>  *out_jacobian_df_dpose=NULL,
                        mrpt::math::CMatrixFixedNumeric<double,3,6>  *out_jacobian_df_dse3=NULL,
                        bool use_small_rot_approx = false) const;

                /** An alternative, slightly more efficient way of doing \f$ G = P \oplus L \f$ with G and L being 3D points and P this 6D pose.
                  * \note local_point is passed by value to allow global and local point to be the same variable
                  */
                inline void composePoint(const TPoint3D local_point, TPoint3D &global_point) const {
                        composePoint(local_point.x,local_point.y,local_point.z,  global_point.x,global_point.y,global_point.z );
                }

                /** An alternative, slightly more efficient way of doing \f$ G = P \oplus L \f$ with G and L being 3D points and P this 6D pose.  */
                inline void composePoint(double lx,double ly,double lz, float &gx, float &gy, float &gz ) const {
                        double ggx, ggy,ggz;
                        composePoint(lx,ly,lz,ggx,ggy,ggz);
                        gx = ggx; gy = ggy; gz = ggz;
                }

                /**  Computes the 3D point L such as \f$ L = G \ominus this \f$.
                  *  If pointers are provided, the corresponding Jacobians are returned.
                  *  "out_jacobian_df_dse3" stands for the Jacobian with respect to the 6D locally Euclidean vector in the tangent space of SE(3).
                  *  See <a href="http://www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty" >this report</a> for mathematical details.
                  * \sa composePoint, composeFrom
                  */
                void inverseComposePoint(const double gx,const double gy,const double gz,double &lx,double &ly,double &lz,
                        mrpt::math::CMatrixFixedNumeric<double,3,3>  *out_jacobian_df_dpoint=NULL,
                        mrpt::math::CMatrixFixedNumeric<double,3,6>  *out_jacobian_df_dpose=NULL,
                        mrpt::math::CMatrixFixedNumeric<double,3,6>  *out_jacobian_df_dse3=NULL ) const;

                /**  Makes "this = A (+) B"; this method is slightly more efficient than "this= A + B;" since it avoids the temporary object.
                  *  \note A or B can be "this" without problems.
                  */
                void composeFrom(const CPose3D& A, const CPose3D& B );

                /** Make \f$ this = this \oplus b \f$  (\a b can be "this" without problems) */
                inline CPose3D&  operator += (const CPose3D& b)
                {
                        composeFrom(*this,b);
                        return *this;
                }

                /**  Makes \f$ this = A \ominus B \f$ this method is slightly more efficient than "this= A - B;" since it avoids the temporary object.
                  *  \note A or B can be "this" without problems.
                  * \sa composeFrom, composePoint
                  */
                void inverseComposeFrom(const CPose3D& A, const CPose3D& B );

                /** Compute \f$ RET = this \oplus b \f$  */
                inline CPose3D  operator - (const CPose3D& b) const
                {
                        CPose3D ret(UNINITIALIZED_POSE);
                        ret.inverseComposeFrom(*this,b);
                        return ret;
                }

                /** Convert this pose into its inverse, saving the result in itself. \sa operator- */
                void inverse();

                /** makes: this = p (+) this */
                inline void  changeCoordinatesReference( const CPose3D & p ) { composeFrom(p,CPose3D(*this)); }

                /** @} */  // compositions


                /** @name Access and modify contents
                        @{ */

                /** Scalar sum of all 6 components: This is diferent from poses composition, which is implemented as "+" operators.
                  * \sa normalizeAngles
                  */
                void addComponents(const CPose3D &p);

                /** Rebuild the internal matrix & update the yaw/pitch/roll angles within the ]-PI,PI] range (Must be called after using addComponents)
                  * \sa addComponents
                  */
                void  normalizeAngles();

                /** Scalar multiplication of x,y,z,yaw,pitch & roll (angles will be wrapped to the ]-pi,pi] interval). */
                void operator *=(const double s);

                /** Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes the internal rotation matrix.
                  * \sa getYawPitchRoll, setYawPitchRoll
                  */
                void  setFromValues(
                        const double            x0,
                        const double            y0,
                        const double            z0,
                        const double            yaw=0,
                        const double            pitch=0,
                        const double            roll=0);

                /** Set the pose from a 3D position (meters) and a quaternion, stored as [x y z qr qx qy qz] in a 7-element vector.
                  * \sa setFromValues, getYawPitchRoll, setYawPitchRoll, CQuaternion, getAsQuaternion
                  */
                template <typename VECTORLIKE>
                inline void  setFromXYZQ(
                        const VECTORLIKE    &v,
                        const size_t        index_offset = 0)
                {
                        ASSERT_ABOVEEQ_(v.size(), 7+index_offset)
                        // The 3x3 rotation part:
                        mrpt::math::CQuaternion<typename VECTORLIKE::value_type> q( v[index_offset+3],v[index_offset+4],v[index_offset+5],v[index_offset+6] );
                        q.rotationMatrixNoResize(m_ROT);
                        m_ypr_uptodate=false;
                        m_coords[0] = v[index_offset+0];
                        m_coords[1] = v[index_offset+1];
                        m_coords[2] = v[index_offset+2];
                }

                /** Set the 3 angles of the 3D pose (in radians) - This method recomputes the internal rotation coordinates matrix.
                  * \sa getYawPitchRoll, setFromValues
                  */
                inline void  setYawPitchRoll(
                        const double            yaw_,
                        const double            pitch_,
                        const double            roll_)
                {
                        setFromValues(x(),y(),z(),yaw_,pitch_,roll_);
                }

                /** Set pose from an array with these 12 elements: [r11 r21 r31 r12 r22 r32 r13 r23 r33 tx ty tz]
                  *  where r{ij} are the entries of the 3x3 rotation matrix and t{x,y,z} are the 3D translation of the pose
                  *  \sa getAs12Vector
                  */
                template <class ARRAYORVECTOR>
                inline void setFrom12Vector(const ARRAYORVECTOR &vec12)
                {
                        m_ROT.set_unsafe(0,0, vec12[0]); m_ROT.set_unsafe(0,1, vec12[3]); m_ROT.set_unsafe(0,2, vec12[6]);
                        m_ROT.set_unsafe(1,0, vec12[1]); m_ROT.set_unsafe(1,1, vec12[4]); m_ROT.set_unsafe(1,2, vec12[7]);
                        m_ROT.set_unsafe(2,0, vec12[2]); m_ROT.set_unsafe(2,1, vec12[5]); m_ROT.set_unsafe(2,2, vec12[8]);
                        m_coords[0] = vec12[ 9];
                        m_coords[1] = vec12[10];
                        m_coords[2] = vec12[11];
                }

                /** Get the pose representation as an array with these 12 elements: [r11 r21 r31 r12 r22 r32 r13 r23 r33 tx ty tz]
                  *  where r{ij} are the entries of the 3x3 rotation matrix and t{x,y,z} are the 3D translation of the pose
                  *  \sa setFrom12Vector
                  */
                template <class ARRAYORVECTOR>
                inline void getAs12Vector(ARRAYORVECTOR &vec12) const
                {
                        vec12[0] = m_ROT.get_unsafe(0,0); vec12[3] = m_ROT.get_unsafe(0,1); vec12[6] = m_ROT.get_unsafe(0,2);
                        vec12[1] = m_ROT.get_unsafe(1,0); vec12[4] = m_ROT.get_unsafe(1,1); vec12[7] = m_ROT.get_unsafe(1,2);
                        vec12[2] = m_ROT.get_unsafe(2,0); vec12[5] = m_ROT.get_unsafe(2,1); vec12[8] = m_ROT.get_unsafe(2,2);
                        vec12[ 9] = m_coords[0];
                        vec12[10] = m_coords[1];
                        vec12[11] = m_coords[2];
                }

                /** Returns the three angles (yaw, pitch, roll), in radians, from the rotation matrix.
                  * \sa setFromValues, yaw, pitch, roll
                  */
                void  getYawPitchRoll( double &yaw, double &pitch, double &roll ) const;

                inline double yaw() const { updateYawPitchRoll(); return m_yaw; }  //!< Get the YAW angle (in radians)  \sa setFromValues
                inline double pitch() const { updateYawPitchRoll(); return m_pitch; }  //!< Get the PITCH angle (in radians) \sa setFromValues
                inline double roll() const { updateYawPitchRoll(); return m_roll; }  //!< Get the ROLL angle (in radians) \sa setFromValues

                /** Returns a 1x6 vector with [x y z yaw pitch roll] */
                void getAsVector(vector_double &v) const;

                /** Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored)
                  * \f[ \mathbf{q} = \left( \begin{array}{c} \cos (\phi /2) \cos (\theta /2) \cos (\psi /2) +  \sin (\phi /2) \sin (\theta /2) \sin (\psi /2) \\ \sin (\phi /2) \cos (\theta /2) \cos (\psi /2) -  \cos (\phi /2) \sin (\theta /2) \sin (\psi /2) \\ \cos (\phi /2) \sin (\theta /2) \cos (\psi /2) +  \sin (\phi /2) \cos (\theta /2) \sin (\psi /2) \\ \cos (\phi /2) \cos (\theta /2) \sin (\psi /2) -  \sin (\phi /2) \sin (\theta /2) \cos (\psi /2) \\ \end{array}\right) \f]
                  * With : \f$ \phi = roll \f$,  \f$ \theta = pitch \f$ and \f$ \psi = yaw \f$.
                  * \param out_dq_dr  If provided, the 4x3 Jacobian of the transformation will be computed and stored here. It's the Jacobian of the transformation from (yaw pitch roll) to (qr qx qy qz).
                  */
                void getAsQuaternion(
                        mrpt::math::CQuaternionDouble &q,
                        mrpt::math::CMatrixFixedNumeric<double,4,3>   *out_dq_dr = NULL
                        ) const;

                inline const double &operator[](unsigned int i) const
                {
                        updateYawPitchRoll();
                        switch(i)
                        {
                                case 0:return m_coords[0];
                                case 1:return m_coords[1];
                                case 2:return m_coords[2];
                                case 3:return m_yaw;
                                case 4:return m_pitch;
                                case 5:return m_roll;
                                default:
                                throw std::runtime_error("CPose3D::operator[]: Index of bounds.");
                        }
                }
                // CPose3D CANNOT have a write [] operator, since it'd leave the object in an inconsistent state (outdated rotation matrix).
                // Use setFromValues() instead.
                // inline double &operator[](unsigned int i)

                /** Returns a human-readable textual representation of the object (eg: "[x y z yaw pitch roll]", angles in degrees.)
                  * \sa fromString
                  */
                void asString(std::string &s) const { updateYawPitchRoll(); s = mrpt::format("[%f %f %f %f %f %f]",m_coords[0],m_coords[1],m_coords[2],RAD2DEG(m_yaw),RAD2DEG(m_pitch),RAD2DEG(m_roll)); }
                inline std::string asString() const { std::string s; asString(s); return s; }

                /** Set the current object value from a string generated by 'asString' (eg: "[x y z yaw pitch roll]", angles in deg. )
                  * \sa asString
                  * \exception std::exception On invalid format
                  */
                void fromString(const std::string &s) {
                        CMatrixDouble  m;
                        if (!m.fromMatlabStringFormat(s)) THROW_EXCEPTION("Malformed expression in ::fromString");
                        ASSERTMSG_(mrpt::math::size(m,1)==1 && mrpt::math::size(m,2)==6, "Wrong size of vector in ::fromString");
                        this->setFromValues(m.get_unsafe(0,0),m.get_unsafe(0,1),m.get_unsafe(0,2),DEG2RAD(m.get_unsafe(0,3)),DEG2RAD(m.get_unsafe(0,4)),DEG2RAD(m.get_unsafe(0,5)));
                 }

                /** Return true if the 6D pose represents a Z axis almost exactly vertical (upwards or downwards), with a given tolerance (if set to 0 exact horizontality is tested). */
                bool isHorizontal( const double tolerance=0) const;

                /** The euclidean distance between two poses taken as two 6-length vectors (angles in radians). */
                double distanceEuclidean6D( const CPose3D &o ) const;

                /** @} */  // modif. components



                /** @name Lie Algebra methods
                    @{ */

                /** Exponentiate a Vector in the SE(3) Lie Algebra to generate a new CPose3D (static method).
                  * \note Method from TooN (C) Tom Drummond (GNU GPL) */
                static CPose3D exp(const mrpt::math::CArrayNumeric<double,6> & vect);

                /** Exponentiate a vector in the Lie algebra to generate a new SO(3) (a 3x3 rotation matrix).
                  * \note Method from TooN (C) Tom Drummond (GNU GPL) */
                static CMatrixDouble33 exp_rotation(const mrpt::math::CArrayNumeric<double,3> & vect);


                /** Take the logarithm of the 3x4 matrix defined by this pose, generating the corresponding vector in the SE(3) Lie Algebra.
                  * \note Method from TooN (C) Tom Drummond (GNU GPL)
                  * \sa ln_jacob
                  */
                void ln(mrpt::math::CArrayDouble<6> &out_ln) const;

                /// \overload
                inline mrpt::math::CArrayDouble<6> ln() const { mrpt::math::CArrayDouble<6> ret; ln(ret); return ret; }

                /** Jacobian of the logarithm of the 3x4 matrix defined by this pose.
                  * \note Method from TooN (C) Tom Drummond (GNU GPL)
                  * \sa ln
                  */
                void ln_jacob(mrpt::math::CMatrixFixedNumeric<double,6,12> &J) const;

                /** Static function to compute the Jacobian of the SO(3) Logarithm function, evaluated at a given 3x3 rotation matrix R.
                  * \sa ln, ln_jacob
                  */
                static void ln_rot_jacob(const CMatrixDouble33 &R, CMatrixFixedNumeric<double,3,9> &M);

                /** Take the logarithm of the 3x3 rotation matrix, generating the corresponding vector in the Lie Algebra.
                  * \note Method from TooN (C) Tom Drummond (GNU GPL) */
                CArrayDouble<3> ln_rotation() const;

                /** @} */

                typedef CPose3D  type_value; //!< Used to emulate CPosePDF types, for example, in mrpt::graphs::CNetworkOfPoses
                enum { is_3D_val = 1 };
                static inline bool is_3D() { return is_3D_val!=0; }
                enum { rotation_dimensions = 3 };
                enum { is_PDF_val = 0 };
                static inline bool is_PDF() { return is_PDF_val!=0; }

                inline const type_value & getPoseMean() const { return *this; }
                inline       type_value & getPoseMean()       { return *this; }

                /** @name STL-like methods and typedefs
                   @{   */
                typedef double         value_type;              //!< The type of the elements
                typedef double&        reference;
                typedef const double&  const_reference;
                typedef std::size_t    size_type;
                typedef std::ptrdiff_t difference_type;


                // size is constant
                enum { static_size = 6 };
                static inline size_type size() { return static_size; }
                static inline bool empty() { return false; }
                static inline size_type max_size() { return static_size; }
                static inline void resize(const size_t n) { if (n!=static_size) throw std::logic_error(format("Try to change the size of CPose3D to %u.",static_cast<unsigned>(n))); }
                /** @} */

        }; // End of class def.


        std::ostream BASE_IMPEXP  & operator << (std::ostream& o, const CPose3D& p);

        /** Unary - operator: return the inverse pose "-p" (Note that is NOT the same than a pose with negative x y z yaw pitch roll) */
        CPose3D BASE_IMPEXP operator -(const CPose3D &p);

        bool BASE_IMPEXP operator==(const CPose3D &p1,const CPose3D &p2);
        bool BASE_IMPEXP operator!=(const CPose3D &p1,const CPose3D &p2);


        // This is a member of CPose<>, but has to be defined here since in its header CPose3D is not declared yet.
        /** The operator \f$ a \ominus b \f$ is the pose inverse compounding operator. */
        template <class DERIVEDCLASS> CPose3D CPose<DERIVEDCLASS>::operator -(const CPose3D& b) const
        {
                CMatrixDouble44 B_INV(UNINITIALIZED_MATRIX);
                b.getInverseHomogeneousMatrix( B_INV );
                CMatrixDouble44 HM(UNINITIALIZED_MATRIX);
                static_cast<const DERIVEDCLASS*>(this)->getHomogeneousMatrix(HM);
                CMatrixDouble44 RES(UNINITIALIZED_MATRIX);
                RES.multiply(B_INV,HM);
                return CPose3D( RES );
        }


        } // End of namespace
} // End of namespace

#endif