lightweight_geom_data.h

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+ */
#ifndef LIGHTWEIGHT_GEOM_DATA_H
#define LIGHTWEIGHT_GEOM_DATA_H

#include <mrpt/utils/utils_defs.h>
#include <mrpt/config.h>
#include <mrpt/base/link_pragmas.h>
#include <mrpt/utils/TPixelCoord.h>
#include <mrpt/utils/TTypeName.h>
#include <mrpt/math/math_frwds.h>  // forward declarations
#include <mrpt/math/eigen_frwds.h>  // forward declarations
#include <mrpt/math/wrap2pi.h>
#include <vector>
#include <stdexcept>

namespace mrpt  {
namespace math  {
        /** \addtogroup geometry_grp
          * @{ */

        //Set of typedefs for lightweight geometric items.
        /**
          * Lightweight 2D point. Allows coordinate access using [] operator.
          * \sa mrpt::poses::CPoint2D
          */
        struct BASE_IMPEXP TPoint2D     {
                enum { static_size = 2 };
                double x, y; //!< X,Y coordinates
                /** Constructor from TPose2D, discarding phi.
                  * \sa TPose2D
                  */
                explicit TPoint2D(const TPose2D &p);
                /**
                  * Constructor from TPoint3D, discarding z.
                  * \sa TPoint3D
                  */
                explicit TPoint2D(const TPoint3D &p);
                /**
                  * Constructor from TPose3D, discarding z and the angular coordinates.
                  * \sa TPose3D
                  */
                explicit TPoint2D(const TPose3D &p);
                /**
                  * Constructor from CPoseOrPoint, perhaps losing 3D information
                  * \sa CPoseOrPoint mrpt::poses::CPoint3D,CPose2D,CPose3D
                  */
                template <class DERIVEDCLASS>
                explicit TPoint2D(const mrpt::poses::CPoseOrPoint<DERIVEDCLASS> &p) :x(p.x()),y(p.y())  {}

                /** Implicit transformation constructor from TPixelCoordf */
                inline TPoint2D(const mrpt::utils::TPixelCoordf &p) :x(p.x),y(p.y)      {}

                /** Implicit constructor from mrpt::poses::CPoint2D  */
                TPoint2D(const mrpt::poses::CPoint2D &p);
                /**
                  * Constructor from coordinates.
                  */
                inline TPoint2D(double xx,double yy):x(xx),y(yy)        {}
                /**
                  * Default fast constructor. Initializes to garbage.
                  */
                inline TPoint2D()       {}
                /** Coordinate access using operator[]. Order: x,y */
                inline double &operator[](size_t i)     { switch (i) { case 0: return x; case 1: return y; default: throw std::out_of_range("index out of range"); } }
                /** Coordinate access using operator[]. Order: x,y */
                inline const double &operator[](size_t i) const { switch (i) { case 0: return x; case 1: return y; default: throw std::out_of_range("index out of range"); } }
                /**
                  * Transformation into vector.
                  */
                inline void getAsVector(std::vector<double> &v) const   {
                        v.resize(2);
                        v[0]=x; v[1]=y;
                }

                bool operator<(const TPoint2D &p) const;

                inline TPoint2D &operator+=(const TPoint2D &p)  {
                        x+=p.x;
                        y+=p.y;
                        return *this;
                }

                inline TPoint2D &operator-=(const TPoint2D &p)  {
                        x-=p.x;
                        y-=p.y;
                        return *this;
                }

                inline TPoint2D &operator*=(double d)   {
                        x*=d;
                        y*=d;
                        return *this;
                }

                inline TPoint2D &operator/=(double d)   {
                        x/=d;
                        y/=d;
                        return *this;
                }

                inline TPoint2D operator+(const TPoint2D &p) const      {
                        TPoint2D r(*this);
                        return r+=p;
                }

                inline TPoint2D operator-(const TPoint2D &p) const      {
                        TPoint2D r(*this);
                        return r-=p;
                }

                inline TPoint2D operator*(double d) const       {
                        TPoint2D r(*this);
                        return r*=d;
                }

                inline TPoint2D operator/(double d) const       {
                        TPoint2D r(*this);
                        return r/=d;
                }
                 /** Returns a human-readable textual representation of the object (eg: "[0.02 1.04]" )
                   * \sa fromString
                   */
                void asString(std::string &s) const { s = mrpt::format("[%f %f]",x,y); }
                inline std::string asString() const { std::string s; asString(s); return s; }

                 /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04]" )
                   * \sa asString
                   * \exception std::exception On invalid format
                   */
                 void fromString(const std::string &s);
                 static size_t size() { return 2; }

                 /** Point norm. */
                inline double norm() const      { return sqrt(square(x)+square(y)); }
        };

        /**
          * Lightweight 2D pose. Allows coordinate access using [] operator.
          * \sa mrpt::poses::CPose2D
          */
        struct BASE_IMPEXP TPose2D      {
                enum { static_size = 3 };
                double x,y; //!< X,Y coordinates
                double phi; //!< Orientation (rads)
                /** Implicit constructor from TPoint2D. Zeroes the phi coordinate.
                  * \sa TPoint2D
                  */
                TPose2D(const TPoint2D &p);
                /**
                  * Constructor from TPoint3D, losing information. Zeroes the phi coordinate.
                  * \sa TPoint3D
                  */
                explicit TPose2D(const TPoint3D &p);
                /**
                  * Constructor from TPose3D, losing information. The phi corresponds to the original pose's yaw.
                  * \sa TPose3D
                  */
                explicit TPose2D(const TPose3D &p);
                /**
                  * Implicit constructor from heavyweight type.
                  * \sa mrpt::poses::CPose2D
                  */
                TPose2D(const mrpt::poses::CPose2D &p);
                /**
                  * Constructor from coordinates.
                  */
                inline TPose2D(double xx,double yy,double pphi):x(xx),y(yy),phi(pphi)   {}
                /**
                  * Default fast constructor. Initializes to garbage.
                  */
                inline TPose2D()        {}
                /** Coordinate access using operator[]. Order: x,y,phi */
                inline double &operator[](size_t i)     { switch (i) { case 0: return x; case 1: return y; case 2: return phi; default: throw std::out_of_range("index out of range");  } }
                /** Coordinate access using operator[]. Order: x,y,phi */
                inline const double &operator[](size_t i) const { switch (i) { case 0: return x; case 1: return y; case 2: return phi; default: throw std::out_of_range("index out of range");  } }
                /**
                  * Transformation into vector.
                  */
                inline void getAsVector(std::vector<double> &v) const   {
                        v.resize(3);
                        v[0]=x; v[1]=y; v[2]=phi;
                }
                 /** Returns a human-readable textual representation of the object (eg: "[x y yaw]", yaw in degrees)
                   * \sa fromString
                   */
                void asString(std::string &s) const;
                inline std::string asString() const { std::string s; asString(s); return s; }

                mrpt::math::TPose2D operator+(const mrpt::math::TPose2D& b) const; //!< Operator "oplus" pose composition: "ret=this \oplus b"  \sa CPose2D
                mrpt::math::TPose2D operator-(const mrpt::math::TPose2D& b) const; //!< Operator "ominus" pose composition: "ret=this \ominus b"  \sa CPose2D
                inline void composePoint(const TPoint2D l, TPoint2D &g) const {
                        const double ccos = ::cos(phi), csin=::sin(phi);
                        g.x = x+ l.x * ccos - l.y * csin;
                        g.y = y+ l.x * csin + l.y * ccos;
                }
                inline void inverseComposePoint(const TPoint2D g, TPoint2D &l) const {
                        const double Ax = g.x - x, Ay = g.y - y, ccos = ::cos(phi), csin=::sin(phi);
                        l.x = Ax * ccos + Ay * csin;
                        l.y =-Ax * csin + Ay * ccos;
                }
                /** Returns the norm of the (x,y) vector (phi is not used) */
                inline double norm() const {
                        return mrpt::math::hypot_fast(x, y);
                }

                 /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -45.0]" )
                   * \sa asString
                   * \exception std::exception On invalid format
                   */
                 void fromString(const std::string &s);
                 static size_t size() { return 3; }
        };

        /** Lightweight 3D point (float version).
          * \sa mrpt::poses::CPoint3D, mrpt::math::TPoint3D
          */
        struct BASE_IMPEXP TPoint3Df
        {
                enum { static_size = 3 };
                float x;
                float y;
                float z;

                inline TPoint3Df() { }
                inline TPoint3Df(const float xx,const float yy,const float zz) : x(xx), y(yy),z(zz) { }
                inline TPoint3Df & operator +=(const TPoint3Df &p) { x+=p.x; y+=p.y; z+=p.z; return *this; }
                inline TPoint3Df   operator *(const float s) { return TPoint3Df(x*s,y*s,z*s); }
                /** Coordinate access using operator[]. Order: x,y,z */
                inline float &operator[](size_t i)      { switch (i) { case 0: return x; case 1: return y; case 2: return z; default: throw std::out_of_range("index out of range");  } }

                /** Coordinate access using operator[]. Order: x,y,z */
                inline const float &operator[](size_t i) const  { switch (i) { case 0: return x; case 1: return y; case 2: return z; default: throw std::out_of_range("index out of range");  } }
        };

        /**
          * Lightweight 3D point. Allows coordinate access using [] operator.
          * \sa mrpt::poses::CPoint3D, mrpt::math::TPoint3Df
          */
        struct BASE_IMPEXP TPoint3D     {
                enum { static_size = 3 };
                double x,y,z; //!< X,Y,Z coordinates

                /** Constructor from coordinates.  */
                inline TPoint3D(double xx,double yy,double zz):x(xx),y(yy),z(zz)        {}
                /** Default fast constructor. Initializes to garbage. */
                inline TPoint3D()       {}
                /** Explicit constructor from coordinates.  */
                explicit inline TPoint3D(const TPoint3Df &p):x(p.x),y(p.y),z(p.z) {}

                /** Implicit constructor from TPoint2D. Zeroes the z.
                  * \sa TPoint2D
                  */
                TPoint3D(const TPoint2D &p);
                /**
                  * Constructor from TPose2D, losing information. Zeroes the z.
                  * \sa TPose2D
                  */
                explicit TPoint3D(const TPose2D &p);
                /**
                  * Constructor from TPose3D, losing information.
                  * \sa TPose3D
                  */
                explicit TPoint3D(const TPose3D &p);
                /**
                  * Implicit constructor from heavyweight type.
                  * \sa mrpt::poses::CPoint3D
                  */
                TPoint3D(const mrpt::poses::CPoint3D &p);
                /**
                  * Constructor from heavyweight 3D pose.
                  * \sa mrpt::poses::CPose3D.
                  */
                explicit TPoint3D(const mrpt::poses::CPose3D &p);
                /** Coordinate access using operator[]. Order: x,y,z */
                inline double &operator[](size_t i)     { switch (i) { case 0: return x; case 1: return y; case 2: return z; default: throw std::out_of_range("index out of range");  } }
                /** Coordinate access using operator[]. Order: x,y,z */
                inline const double &operator[](size_t i) const { switch (i) { case 0: return x; case 1: return y; case 2: return z; default: throw std::out_of_range("index out of range");  } }
                /**
                  * Point-to-point distance.
                  */
                inline double distanceTo(const TPoint3D &p) const       {
                        return sqrt(square(p.x-x)+square(p.y-y)+square(p.z-z));
                }
                /**
                  * Point-to-point distance, squared.
                  */
                inline double sqrDistanceTo(const TPoint3D &p) const    {
                        return square(p.x-x)+square(p.y-y)+square(p.z-z);
                }
                /**
                  * Point norm.
                  */
                inline double norm() const      {
                        return sqrt(square(x)+square(y)+square(z));
                }
                /**
                  * Point scale.
                  */
                inline TPoint3D &operator*=(const double f) {
                        x*=f;y*=f;z*=f;
                        return *this;
                }
                /**
                  * Transformation into vector.
                  */
                template <class VECTORLIKE>
                void getAsVector(VECTORLIKE &v) const {
                        v.resize(3);
                        v[0]=x; v[1]=y; v[2]=z;
                }
                /**
                  * Translation.
                  */
                inline TPoint3D &operator+=(const TPoint3D &p)  {
                        x+=p.x;
                        y+=p.y;
                        z+=p.z;
                        return *this;
                }
                /**
                  * Difference between points.
                  */
                inline TPoint3D &operator-=(const TPoint3D &p)  {
                        x-=p.x;
                        y-=p.y;
                        z-=p.z;
                        return *this;
                }
                /**
                  * Points addition.
                  */
                inline TPoint3D operator+(const TPoint3D &p) const      {
                        return TPoint3D(x+p.x,y+p.y,z+p.z);
                }
                /**
                  * Points substraction.
                  */
                inline TPoint3D operator-(const TPoint3D &p) const      {
                        return TPoint3D(x-p.x,y-p.y,z-p.z);
                }

                inline TPoint3D operator*(double d) const       {
                        return TPoint3D(x*d,y*d,z*d);
                }

                inline TPoint3D operator/(double d) const       {
                        return TPoint3D(x/d,y/d,z/d);
                }

                bool operator<(const TPoint3D &p) const;

                 /** Returns a human-readable textual representation of the object (eg: "[0.02 1.04 -0.8]" )
                   * \sa fromString
                   */
                void asString(std::string &s) const { s = mrpt::format("[%f %f %f]",x,y,z); }
                inline std::string asString() const { std::string s; asString(s); return s; }

                 /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0.8]" )
                   * \sa asString
                   * \exception std::exception On invalid format
                   */
                 void fromString(const std::string &s);
                 static size_t size() { return 3; }
        };

        /** XYZ point (double) + Intensity(u8) \sa mrpt::math::TPoint3D */
        struct BASE_IMPEXP TPointXYZIu8 {
                mrpt::math::TPoint3D pt;
                uint8_t intensity;
                inline TPointXYZIu8() : pt(), intensity(0) {}
                inline TPointXYZIu8(double x,double y,double z, uint8_t intensity_val) : pt(x,y,z),intensity(intensity_val) {}
        };
        /** XYZ point (double) + RGB(u8) \sa mrpt::math::TPoint3D */
        struct BASE_IMPEXP TPointXYZRGBu8 {
                mrpt::math::TPoint3D pt;
                uint8_t R,G,B;
                inline TPointXYZRGBu8() : pt(), R(0),G(0),B(0) {}
                inline TPointXYZRGBu8(double x,double y,double z, uint8_t R_val, uint8_t G_val, uint8_t B_val) : pt(x,y,z),R(R_val),G(G_val),B(B_val) {}
        };
        /** XYZ point (float) + Intensity(u8) \sa mrpt::math::TPoint3D */
        struct BASE_IMPEXP TPointXYZfIu8 {
                mrpt::math::TPoint3Df pt;
                uint8_t intensity;
                inline TPointXYZfIu8() : pt(), intensity(0) {}
                inline TPointXYZfIu8(float x,float y,float z, uint8_t intensity_val) : pt(x,y,z),intensity(intensity_val) {}
        };
        /** XYZ point (float) + RGB(u8) \sa mrpt::math::TPoint3D */
        struct BASE_IMPEXP TPointXYZfRGBu8 {
                mrpt::math::TPoint3Df pt;
                uint8_t R,G,B;
                inline TPointXYZfRGBu8() : pt(), R(0),G(0),B(0) {}
                inline TPointXYZfRGBu8(float x,float y,float z, uint8_t R_val, uint8_t G_val, uint8_t B_val) : pt(x,y,z),R(R_val),G(G_val),B(B_val) {}
        };

        /**
          * Lightweight 3D pose (three spatial coordinates, plus three angular coordinates). Allows coordinate access using [] operator.
          * \sa mrpt::poses::CPose3D
          */
        struct BASE_IMPEXP TPose3D      {
                enum { static_size = 6 };
                double x,y,z; //!< X,Y,Z, coords
                double yaw; //!< Yaw coordinate (rotation angle over Z axis).
                double pitch; //!< Pitch coordinate (rotation angle over Y axis).
                double roll; //!< Roll coordinate (rotation angle over X coordinate).
                /** Implicit constructor from TPoint2D. Zeroes all the unprovided information.
                  * \sa TPoint2D
                  */
                TPose3D(const TPoint2D &p);
                /**
                  * Implicit constructor from TPose2D. Gets the yaw from the 2D pose's phi, zeroing all the unprovided information.
                  * \sa TPose2D
                  */
                TPose3D(const TPose2D &p);
                /**
                  * Implicit constructor from TPoint3D. Zeroes angular information.
                  * \sa TPoint3D
                  */
                TPose3D(const TPoint3D &p);
                /**
                  * Implicit constructor from heavyweight type.
                  * \sa mrpt::poses::CPose3D
                  */
                TPose3D(const mrpt::poses::CPose3D &p);
                /**
                  * Constructor from coordinates.
                  */
                TPose3D(double _x,double _y,double _z,double _yaw,double _pitch,double _roll):x(_x),y(_y),z(_z),yaw(_yaw),pitch(_pitch),roll(_roll)     {}
                /**
                  * Default fast constructor. Initializes to garbage.
                  */
                inline TPose3D()        {}
                /** Coordinate access using operator[]. Order: x,y,z,yaw,pitch,roll */
                inline double &operator[](size_t i)     { switch (i) { case 0: return x; case 1: return y; case 2: return z; case 3: return yaw; case 4: return pitch; case 5: return roll; default: throw std::out_of_range("index out of range");  } }
                /** Coordinate access using operator[]. Order: x,y,z,yaw,pitch,roll */
                inline const double &operator[](size_t i) const { switch (i) { case 0: return x; case 1: return y; case 2: return z; case 3: return yaw; case 4: return pitch; case 5: return roll; default: throw std::out_of_range("index out of range");  } }
                /**
                  * Pose's spatial coordinates norm.
                  */
                double norm() const {
                        return std::sqrt(square(x)+square(y)+square(z));
                }
                /**
                  * Gets the pose as a vector of doubles.
                  */
                void getAsVector(std::vector<double> &v) const {
                        v.resize(6);
                        v[0]=x; v[1]=y; v[2]=z; v[3]=yaw; v[4]=pitch; v[5]=roll;
                }
                 /** 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;
                inline std::string asString() const { std::string s; asString(s); return s; }

                /** 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::CQuaternion<double> &q,
                        mrpt::math::CMatrixFixedNumeric<double, 4, 3>   *out_dq_dr = NULL
                ) const;

                 /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0.8]" )
                   * \sa asString
                   * \exception std::exception On invalid format
                   */
                 void fromString(const std::string &s);
                 static size_t size() { return 6; }
        };

        /** Lightweight 3D pose (three spatial coordinates, plus a quaternion ). Allows coordinate access using [] operator.
          * \sa mrpt::poses::CPose3DQuat
          */
        struct BASE_IMPEXP TPose3DQuat  {
                enum { static_size = 7 };
                double x,y,z;   //!< Translation in x,y,z
                double qr,qx,qy,qz;  //!< Unit quaternion part, qr,qx,qy,qz

                /** Constructor from coordinates. */
                inline TPose3DQuat(double _x,double _y,double _z,double _qr,double _qx, double _qy, double _qz):x(_x),y(_y),z(_z),qr(_qr),qx(_qx),qy(_qy),qz(_qz) {  }
                /** Default fast constructor. Initializes to garbage. */
                inline TPose3DQuat()    {}
                /** Constructor from a CPose3DQuat */
                TPose3DQuat(const mrpt::poses::CPose3DQuat &p);

                /** Coordinate access using operator[]. Order: x,y,z,qr,qx,qy,qz */
                inline double &operator[](size_t i)     { switch (i) { case 0: return x; case 1: return y; case 2: return z; case 3: return qr; case 4: return qx; case 5: return qy; case 6: return qz; default: throw std::out_of_range("index out of range");  } }
                /** Coordinate access using operator[]. Order: x,y,z,qr,qx,qy,qz */
                inline const double &operator[](size_t i) const { switch (i) { case 0: return x; case 1: return y; case 2: return z; case 3: return qr; case 4: return qx; case 5: return qy; case 6: return qz; default: throw std::out_of_range("index out of range");  } }
                /** Pose's spatial coordinates norm. */
                double norm() const {
                        return sqrt(square(x)+square(y)+square(z));
                }
                /** Gets the pose as a vector of doubles. */
                void getAsVector(std::vector<double> &v) const {
                        v.resize(7);
                        for (size_t i=0;i<7;i++) v[i]=(*this)[i];
                }
                 /** Returns a human-readable textual representation of the object as "[x y z qr qx qy qz]"
                   * \sa fromString
                   */
                void asString(std::string &s) const { s = mrpt::format("[%f %f %f %f %f %f %f]",x,y,z,qr,qx,qy,qz); }
                inline std::string asString() const { std::string s; asString(s); return s; }

                 /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0.8 1.0 0.0 0.0 0.0]" )
                   * \sa asString
                   * \exception std::exception On invalid format
                   */
                 void fromString(const std::string &s);
                 static size_t size() { return 7; }
        };

        // Text streaming functions:
        std::ostream BASE_IMPEXP & operator << (std::ostream& o, const TPoint2D & p);
        std::ostream BASE_IMPEXP & operator << (std::ostream& o, const TPoint3D & p);
        std::ostream BASE_IMPEXP & operator << (std::ostream& o, const TPose2D & p);
        std::ostream BASE_IMPEXP & operator << (std::ostream& o, const TPose3D & p);
        std::ostream BASE_IMPEXP & operator << (std::ostream& o, const TPose3DQuat & p);


        /**
          * Unary minus operator for 3D points.
          */
        inline TPoint3D operator-(const TPoint3D &p1)   {
                return TPoint3D(-p1.x,-p1.y,-p1.z);
        }
        /**
          * Exact comparison between 2D points.
          */
        inline bool operator==(const TPoint2D &p1,const TPoint2D &p2)   {
                return (p1.x==p2.x)&&(p1.y==p2.y); //-V550
        }
        /**
          * Exact comparison between 2D points.
          */
        inline bool operator!=(const TPoint2D &p1,const TPoint2D &p2)   {
                return (p1.x!=p2.x)||(p1.y!=p2.y); //-V550
        }
        /**
          * Exact comparison between 3D points.
          */
        inline bool operator==(const TPoint3D &p1,const TPoint3D &p2)   {
                return (p1.x==p2.x)&&(p1.y==p2.y)&&(p1.z==p2.z); //-V550
        }
        /**
          * Exact comparison between 3D points.
          */
        inline bool operator!=(const TPoint3D &p1,const TPoint3D &p2)   {
                return (p1.x!=p2.x)||(p1.y!=p2.y)||(p1.z!=p2.z); //-V550
        }
        /**
          * Exact comparison between 2D poses, taking possible cycles into account.
          */
        inline bool operator==(const TPose2D &p1,const TPose2D &p2)     {
                return (p1.x==p2.x)&&(p1.y==p2.y)&&(mrpt::math::wrapTo2Pi(p1.phi)==mrpt::math::wrapTo2Pi(p2.phi)); //-V550
        }
        /**
          * Exact comparison between 2D poses, taking possible cycles into account.
          */
        inline bool operator!=(const TPose2D &p1,const TPose2D &p2)     {
                return (p1.x!=p2.x)||(p1.y!=p2.y)||(mrpt::math::wrapTo2Pi(p1.phi)!=mrpt::math::wrapTo2Pi(p2.phi)); //-V550
        }
        /**
          * Exact comparison between 3D poses, taking possible cycles into account.
          */
        inline bool operator==(const TPose3D &p1,const TPose3D &p2)     {
                return (p1.x==p2.x)&&(p1.y==p2.y)&&(p1.z==p2.z)&&(mrpt::math::wrapTo2Pi(p1.yaw)==mrpt::math::wrapTo2Pi(p2.yaw))&&(mrpt::math::wrapTo2Pi(p1.pitch)==mrpt::math::wrapTo2Pi(p2.pitch))&&(mrpt::math::wrapTo2Pi(p1.roll)==mrpt::math::wrapTo2Pi(p2.roll)); //-V550
        }
        /**
          * Exact comparison between 3D poses, taking possible cycles into account.
          */
        inline bool operator!=(const TPose3D &p1,const TPose3D &p2)     {
                return (p1.x!=p2.x)||(p1.y!=p2.y)||(p1.z!=p2.z)||(mrpt::math::wrapTo2Pi(p1.yaw)!=mrpt::math::wrapTo2Pi(p2.yaw))||(mrpt::math::wrapTo2Pi(p1.pitch)!=mrpt::math::wrapTo2Pi(p2.pitch))||(mrpt::math::wrapTo2Pi(p1.roll)!=mrpt::math::wrapTo2Pi(p2.roll)); //-V550
        }
        //Forward declarations
        struct BASE_IMPEXP TSegment3D;
        struct BASE_IMPEXP TLine3D;
        class BASE_IMPEXP TPolygon3D;
        struct BASE_IMPEXP TObject3D;

        //Pragma defined to ensure no structure packing
        /**
          * 2D segment, consisting of two points.
          * \sa TSegment3D,TLine2D,TPolygon2D,TPoint2D
          */
        struct BASE_IMPEXP TSegment2D   {
        public:
                /**
                  * Origin point.
                  */
                TPoint2D point1;
                /**
                  * Destiny point.
                  */
                TPoint2D point2;
                /**
                  * Segment length.
                  */
                double length() const;
                /**
                  * Distance to point.
                  */
                double distance(const TPoint2D &point) const;
                /**
                  * Distance with sign to point (sign indicates which side the point is).
                  */
                double signedDistance(const TPoint2D &point) const;
                /**
                  * Check whether a point is inside a segment.
                  */
                bool contains(const TPoint2D &point) const;
                /** Access to points using operator[0-1] */
                inline TPoint2D &operator[](size_t i)   { switch (i) { case 0: return point1; case 1: return point2; default: throw std::out_of_range("index out of range");  } }
                /** Access to points using operator[0-1] */
                inline const TPoint2D &operator[](size_t i) const { switch (i) { case 0: return point1; case 1: return point2; default: throw std::out_of_range("index out of range");  } }
                /**
                  * Project into 3D space, setting the z to 0.
                  */
                void generate3DObject(TSegment3D &s) const;
                /**
                  * Segment's central point.
                  */
                inline void getCenter(TPoint2D &p) const        {
                        p.x=(point1.x+point2.x)/2;
                        p.y=(point1.y+point2.y)/2;
                }
                /**
                  * Constructor from both points.
                  */
                TSegment2D(const TPoint2D &p1,const TPoint2D &p2):point1(p1),point2(p2) {}
                /**
                  * Fast default constructor. Initializes to garbage.
                  */
                TSegment2D()    {}
                /**
                  * Explicit constructor from 3D object, discarding the z.
                  */
                explicit TSegment2D(const TSegment3D &s);

                bool operator<(const TSegment2D &s) const;
        };
        /**
          * 3D segment, consisting of two points.
          * \sa TSegment2D,TLine3D,TPlane,TPolygon3D,TPoint3D
          */
        struct BASE_IMPEXP TSegment3D   {
        public:
                /**
                  * Origin point.
                  */
                TPoint3D point1;
                /**
                  * Destiny point.
                  */
                TPoint3D point2;
                /**
                  * Segment length.
                  */
                double length() const;
                /**
                  * Distance to point.
                  */
                double distance(const TPoint3D &point) const;
                /**
                  * Distance to another segment.
                  */
                double distance(const TSegment3D &segment) const;
                /**
                  * Check whether a point is inside the segment.
                  */
                bool contains(const TPoint3D &point) const;
                /** Access to points using operator[0-1] */
                inline TPoint3D &operator[](size_t i) { switch (i) { case 0: return point1; case 1: return point2; default: throw std::out_of_range("index out of range");  } }
                /** Access to points using operator[0-1] */
                inline const TPoint3D &operator[](size_t i) const { switch (i) { case 0: return point1; case 1: return point2; default: throw std::out_of_range("index out of range");  } }
                /**
                  * Projection into 2D space, discarding the z.
                  */
                inline void generate2DObject(TSegment2D &s) const       {
                        s=TSegment2D(*this);
                }
                /**
                  * Segment's central point.
                  */
                inline void getCenter(TPoint3D &p) const        {
                        p.x=(point1.x+point2.x)/2;
                        p.y=(point1.y+point2.y)/2;
                        p.z=(point1.z+point2.z)/2;
                }
                /**
                  * Constructor from both points.
                  */
                TSegment3D(const TPoint3D &p1,const TPoint3D &p2):point1(p1),point2(p2) {}
                /**
                  * Fast default constructor. Initializes to garbage.
                  */
                TSegment3D()    {}
                /**
                  * Constructor from 2D object. Sets the z to zero.
                  */
                explicit TSegment3D(const TSegment2D &s):point1(s.point1),point2(s.point2)      {}

                bool operator<(const TSegment3D &s) const;
        };

        inline bool operator==(const TSegment2D &s1,const TSegment2D &s2)       {
                return (s1.point1==s2.point1)&&(s1.point2==s2.point2);
        }

        inline bool operator!=(const TSegment2D &s1,const TSegment2D &s2)       {
                return (s1.point1!=s2.point1)||(s1.point2!=s2.point2);
        }

        inline bool operator==(const TSegment3D &s1,const TSegment3D &s2)       {
                return (s1.point1==s2.point1)&&(s1.point2==s2.point2);
        }

        inline bool operator!=(const TSegment3D &s1,const TSegment3D &s2)       {
                return (s1.point1!=s2.point1)||(s1.point2!=s2.point2);
        }

        /**
          * 2D line without bounds, represented by its equation \f$Ax+By+C=0\f$.
          * \sa TLine3D,TSegment2D,TPolygon2D,TPoint2D
          */
        struct BASE_IMPEXP TLine2D      {
        public:
                /**
                  * Line coefficients, stored as an array: \f$\left[A,B,C\right]\f$.
                  */
                double coefs[3];
                /**
                  * Evaluate point in the line's equation.
                  */
                double evaluatePoint(const TPoint2D &point) const;
                /**
                  * Check whether a point is inside the line.
                  */
                bool contains(const TPoint2D &point) const;
                /**
                  * Distance from a given point.
                  */
                double distance(const TPoint2D &point) const;
                /**
                  * Distance with sign from a given point (sign indicates side).
                  */
                double signedDistance(const TPoint2D &point) const;
                /**
                  * Get line's normal vector.
                  */
                void getNormalVector(double (&vector)[2]) const;
                /**
                  * Unitarize line's normal vector.
                  */
                void unitarize();
                /**
                  * Get line's normal vector after unitarizing line.
                  */
                inline void getUnitaryNormalVector(double (&vector)[2]) {
                        unitarize();
                        getNormalVector(vector);
                }
                /**
                  * Get line's director vector.
                  */
                void getDirectorVector(double (&vector)[2]) const;
                /**
                  * Unitarize line and then get director vector.
                  */
                inline void getUnitaryDirectorVector(double (&vector)[2])       {
                        unitarize();
                        getDirectorVector(vector);
                }
                /**
                  * Project into 3D space, setting the z to 0.
                  */
                void generate3DObject(TLine3D &l) const;
                /**
                  * Get a pose2D whose X axis corresponds to the line.
                  * \sa mrpt::poses::CPose2D.
                  */
                void getAsPose2D(mrpt::poses::CPose2D &outPose) const;
                /**
                  * Get a pose2D whose X axis corresponds to the line, forcing the base point to one given.
                  * \throw logic_error if the point is not inside the line.
                  * \sa mrpt::poses::CPose2D.
                  */
                void getAsPose2DForcingOrigin(const TPoint2D &origin,mrpt::poses::CPose2D &outPose) const;
                /**
                  * Constructor from two points, through which the line will pass.
                  * \throw logic_error if both points are the same
                  */
                TLine2D(const TPoint2D &p1,const TPoint2D &p2);
                /**
                  * Constructor from a segment.
                  */
                explicit TLine2D(const TSegment2D &s);
                /**
                  * Fast default constructor. Initializes to garbage.
                  */
                TLine2D()       {}
                /**
                  * Constructor from line's coefficients.
                  */
                inline TLine2D(double A,double B,double C)      {
                        coefs[0]=A;
                        coefs[1]=B;
                        coefs[2]=C;
                }
                /**
                  * Construction from 3D object, discarding the Z.
                  * \throw std::logic_error if the line is normal to the XY plane.
                  */
                explicit TLine2D(const TLine3D &l);
        };

        /**
          * 3D line, represented by a base point and a director vector.
          * \sa TLine2D,TSegment3D,TPlane,TPolygon3D,TPoint3D
          */
        struct BASE_IMPEXP TLine3D      {
        public:
                /**
                  * Base point.
                  */
                TPoint3D pBase;
                /**
                  * Director vector.
                  */
                double director[3];
                /**
                  * Check whether a point is inside the line.
                  */
                bool contains(const TPoint3D &point) const;
                /**
                  * Distance between the line and a point.
                  */
                double distance(const TPoint3D &point) const;
                /**
                  * Unitarize director vector.
                  */
                void unitarize();
                /**
                  * Get director vector.
                  */
                inline void getDirectorVector(double (&vector)[3]) const        {
                        for (size_t i=0;i<3;i++) vector[i]=director[i];
                }
                /**
                  * Unitarize and then get director vector.
                  */
                inline void getUnitaryDirectorVector(double (&vector)[3])       {
                        unitarize();
                        getDirectorVector(vector);
                }
                /**
                  * Project into 2D space, discarding the Z coordinate.
                  * \throw std::logic_error if the line's director vector is orthogonal to the XY plane.
                  */
                inline void generate2DObject(TLine2D &l) const  {
                        l=TLine2D(*this);
                }
                /**
                  * Constructor from two points, through which the line will pass.
                  * \throw std::logic_error if both points are the same.
                  */
                TLine3D(const TPoint3D &p1,const TPoint3D &p2);
                /**
                  * Constructor from 3D segment.
                  */
                explicit TLine3D(const TSegment3D &s);
                /**
                  * Fast default constructor. Initializes to garbage.
                  */
                TLine3D()       {}
                /** Constructor from 2D object. Zeroes the z. */
                explicit TLine3D(const TLine2D &l);
        };

        /**
          * 3D Plane, represented by its equation \f$Ax+By+Cz+D=0\f$
          * \sa TSegment3D,TLine3D,TPolygon3D,TPoint3D
          */
        struct BASE_IMPEXP TPlane       {
        public:
                /**
                  * Plane coefficients, stored as an array: \f$\left[A,B,C,D\right]\f$
                  */
                double coefs[4];
                /**
                  * Evaluate a point in the plane's equation.
                  */
                double evaluatePoint(const TPoint3D &point) const;
                /**
                  * Check whether a point is contained into the plane.
                  */
                bool contains(const TPoint3D &point) const;
                /**
                  * Check whether a segment is fully contained into the plane.
                  */
                inline bool contains(const TSegment3D &segment) const   {
                        return contains(segment.point1)&&contains(segment.point2);
                }
                /**
                  * Check whether a line is fully contained into the plane.
                  */
                bool contains(const TLine3D &line) const;
                /**
                  * Distance to 3D point.
                  */
                double distance(const TPoint3D &point) const;
                /**
                  * Distance to 3D line. Will be zero if the line is not parallel to the plane.
                  */
                double distance(const TLine3D &line) const;
                /**
                  * Get plane's normal vector.
                  */
                void getNormalVector(double (&vec)[3]) const;
                /**
                  * Unitarize normal vector.
                  */
                void unitarize();
                /**
                  * Unitarize, then get normal vector.
                  */
                inline void getUnitaryNormalVector(double (&vec)[3])    {
                        unitarize();
                        getNormalVector(vec);
                }
                /**
                  * Gets a pose whose XY plane corresponds to this plane.
                  */
                void getAsPose3D(mrpt::poses::CPose3D &outPose);
                /**
                  * Gets a pose whose XY plane corresponds to this plane.
                  */
                inline void getAsPose3D(mrpt::poses::CPose3D &outPose) const    {
                        TPlane p=*this;
                        p.getAsPose3D(outPose);
                }
                /**
                  * Gets a pose whose XY plane corresponds to this, forcing an exact point as its spatial coordinates.
                  * \throw std::logic_error if the point is not inside the plane.
                  */
                void getAsPose3DForcingOrigin(const TPoint3D &newOrigin,mrpt::poses::CPose3D &pose);
                /**
                  * Gets a pose whose XY plane corresponds to this, forcing an exact point as its spatial coordinates.
                  * \throw std::logic_error if the point is not inside the plane.
                  */
                inline void getAsPose3DForcingOrigin(const TPoint3D &newOrigin,mrpt::poses::CPose3D &pose) const        {
                        TPlane p=*this;
                        p.getAsPose3DForcingOrigin(newOrigin,pose);
                }
                /**
                  * Gets a plane which contains these three points.
                  * \throw std::logic_error if the points are linearly dependants.
                  */
                TPlane(const TPoint3D &p1,const TPoint3D &p2,const TPoint3D &p3);
                /**
                  * Gets a plane which contains this point and this line.
                  * \throw std::logic_error if the point is inside the line.
                  */
                TPlane(const TPoint3D &p1,const TLine3D &r2);
                /**
                  * Gets a plane which contains the two lines.
                  * \throw std::logic_error if the lines do not cross.
                  */
                TPlane(const TLine3D &r1,const TLine3D &r2);
                /**
                  * Fast default constructor. Initializes to garbage.
                  */
                TPlane()        {}
                /**
                  * Constructor from plane coefficients.
                  */
                inline TPlane(double A,double B,double C,double D)      {
                        coefs[0]=A;
                        coefs[1]=B;
                        coefs[2]=C;
                        coefs[3]=D;
                }
                /**
                  * Constructor from an array of coefficients.
                  */
                inline TPlane(const double (&vec)[4])   {
                        for (size_t i=0;i<4;i++) coefs[i]=vec[i];
                }
        };

        typedef TPlane TPlane3D;

        /**
          * 2D polygon, inheriting from std::vector<TPoint2D>.
          * \sa TPolygon3D,TSegment2D,TLine2D,TPoint2D, CPolygon
          */
        class BASE_IMPEXP TPolygon2D:public std::vector<TPoint2D>       {
        public:
                double distance(const TPoint2D &point) const;            //!< Distance to a point (always >=0)
                bool contains(const TPoint2D &point) const;              //!< Check whether a point is inside (or within geometryEpsilon of a polygon edge). This works for concave or convex polygons.
                void getAsSegmentList(std::vector<TSegment2D> &v) const; //!< Gets as set of segments, instead of points.
                void generate3DObject(TPolygon3D &p) const;              //!< Projects into 3D space, zeroing the z.
                void getCenter(TPoint2D &p) const;                       //!< Polygon's central point.
                bool isConvex() const;                                   //!< Checks whether is convex.
                void removeRepeatedVertices();   //!< Erase repeated vertices.  \sa removeRedundantVertices
                void removeRedundantVertices();  //!< Erase every redundant vertex from the polygon, saving space. \sa removeRepeatedVertices
                void getPlotData(std::vector<double> &x,std::vector<double> &y) const; //!< Gets plot data, ready to use on a 2D plot. \sa mrpt::gui::CDisplayWindowPlots
                void getBoundingBox(TPoint2D &min_coords, TPoint2D&max_coords) const;  //!< Get polygon bounding box. \exception On empty polygon
                /** Default constructor  */
                TPolygon2D():std::vector<TPoint2D>() {}
                /** Constructor for a given number of vertices, intializing them as garbage. */
                explicit TPolygon2D(size_t N):std::vector<TPoint2D>(N)  {}
                /** Implicit constructor from a vector of 2D points */
                TPolygon2D(const std::vector<TPoint2D> &v):std::vector<TPoint2D>(v)     {}
                /** Constructor from a 3D object. */
                explicit TPolygon2D(const TPolygon3D &p);
                /** Static method to create a regular polygon, given its size and radius.
                  * \throw std::logic_error if radius is near zero or the number of edges is less than three.
                  */
                static void createRegularPolygon(size_t numEdges,double radius,TPolygon2D &poly);
                /** Static method to create a regular polygon from its size and radius. The center will correspond to the given pose.
                  * \throw std::logic_error if radius is near zero or the number of edges is less than three.
                  */
                static inline void createRegularPolygon(size_t numEdges,double radius,TPolygon2D &poly,const mrpt::poses::CPose2D &pose);
        };

        /**
          * 3D polygon, inheriting from std::vector<TPoint3D>
          * \sa TPolygon2D,TSegment3D,TLine3D,TPlane,TPoint3D
          */
        class BASE_IMPEXP TPolygon3D:public std::vector<TPoint3D>       {
        public:
                double distance(const TPoint3D &point) const;  //!< Distance to point (always >=0)
                bool contains(const TPoint3D &point) const;    //!< Check whether a point is inside (or within geometryEpsilon of a polygon edge). This works for concave or convex polygons.
                void getAsSegmentList(std::vector<TSegment3D> &v) const; //!< Gets as set of segments, instead of set of points.
                bool getPlane(TPlane &p) const; //!< Gets a plane which contains the polygon. Returns false if the polygon is skew and cannot be fit inside a plane.
                /** Gets the best fitting plane, disregarding whether the polygon actually fits inside or not. \sa getBestFittingPlane */
                void getBestFittingPlane(TPlane &p) const;
                /** Projects into a 2D space, discarding the z. \sa getPlane,isSkew */
                inline void generate2DObject(TPolygon2D &p) const       {
                        p=TPolygon2D(*this);
                }
                void getCenter(TPoint3D &p) const; //!< Get polygon's central point.
                bool isSkew() const; //!< Check whether the polygon is skew. Returns true if there doesn't exist a plane in which the polygon can fit. \sa getBestFittingPlane
                void removeRepeatedVertices(); //!< Remove polygon's repeated vertices.
                void removeRedundantVertices(); //!< Erase every redundant vertex, thus saving space.
                /** Default constructor. Creates a polygon with no vertices.  */
                TPolygon3D():std::vector<TPoint3D>()    {}
                /** Constructor for a given size. Creates a polygon with a fixed number of vertices, which are initialized to garbage. */
                explicit TPolygon3D(size_t N):std::vector<TPoint3D>(N)  {}
                /** Implicit constructor from a 3D points vector. */
                TPolygon3D(const std::vector<TPoint3D> &v):std::vector<TPoint3D>(v)     {}
                /** Constructor from a 2D object. Zeroes the z. */
                explicit TPolygon3D(const TPolygon2D &p);
                /** Static method to create a regular polygon, given its size and radius.
                  * \throw std::logic_error if number of edges is less than three, or radius is near zero. */
                static void createRegularPolygon(size_t numEdges,double radius,TPolygon3D &poly);
                /** Static method to create a regular polygon, given its size and radius. The center will be located on the given pose.
                  * \throw std::logic_error if number of edges is less than three, or radius is near zero.
                  */
                static inline void createRegularPolygon(size_t numEdges,double radius,TPolygon3D &poly,const mrpt::poses::CPose3D &pose);
        };

        /**
          * Object type identifier for TPoint2D or TPoint3D.
          * \sa TObject2D,TObject3D
          */
        const unsigned char GEOMETRIC_TYPE_POINT=0;
        /**
          * Object type identifier for TSegment2D or TSegment3D.
          * \sa TObject2D,TObject3D
          */
        const unsigned char GEOMETRIC_TYPE_SEGMENT=1;
        /**
          * Object type identifier for TLine2D or TLine3D.
          * \sa TObject2D,TObject3D
          */
        const unsigned char GEOMETRIC_TYPE_LINE=2;
        /**
          * Object type identifier for TPolygon2D or TPolygon3D.
          * \sa TObject2D,TObject3D
          */
        const unsigned char GEOMETRIC_TYPE_POLYGON=3;
        /**
          * Object type identifier for TPlane.
          * \sa TObject3D
          */
        const unsigned char GEOMETRIC_TYPE_PLANE=4;
        /**
          * Object type identifier for empty TObject2D or TObject3D.
          * \sa TObject2D,TObject3D
          */
        const unsigned char GEOMETRIC_TYPE_UNDEFINED=255;

        /**
          * Standard type for storing any lightweight 2D type. Do not inherit from this class.
          * \sa TPoint2D,TSegment2D,TLine2D,TPolygon2D
          */
        struct BASE_IMPEXP TObject2D    {
        private:
                /**
                  * Object type identifier.
                  */
                unsigned char type;
                /**
                  * Union type storing pointers to every allowed type.
                  */
                struct tobject2d_data_t {
                        TPoint2D point;
                        TSegment2D segment;
                        TLine2D line;
                        TPolygon2D *polygon;

                        tobject2d_data_t() : polygon(NULL) { }
                }       data;
                /**
                  * Destroys the object, releasing the pointer to the content (if any).
                  */
                inline void destroy()   {
                        if (type==GEOMETRIC_TYPE_POLYGON) delete data.polygon;
                        type=GEOMETRIC_TYPE_UNDEFINED;
                }
        public:
                /**
                  * Implicit constructor from point.
                  */
                inline TObject2D(const TPoint2D &p):type(GEOMETRIC_TYPE_POINT)  {
                        data.point=p;
                }
                /**
                  * Implicit constructor from segment.
                  */
                inline TObject2D(const TSegment2D &s):type(GEOMETRIC_TYPE_SEGMENT)      {
                        data.segment=s;
                }
                /**
                  * Implicit constructor from line.
                  */
                inline TObject2D(const TLine2D &r):type(GEOMETRIC_TYPE_LINE)    {
                        data.line=r;
                }
                /**
                  * Implicit constructor from polygon.
                  */
                inline TObject2D(const TPolygon2D &p):type(GEOMETRIC_TYPE_POLYGON)      {
                        data.polygon=new TPolygon2D(p);
                }
                /**
                  * Implicit constructor from polygon.
                  */
                TObject2D():type(GEOMETRIC_TYPE_UNDEFINED)      {}
                /**
                  * Object destruction.
                  */
                ~TObject2D()    {
                        destroy();
                }
                /**
                  * Checks whether content is a point.
                  */
                inline bool isPoint() const     {
                        return type==GEOMETRIC_TYPE_POINT;
                }
                /**
                  * Checks whether content is a segment.
                  */
                inline bool isSegment() const   {
                        return type==GEOMETRIC_TYPE_SEGMENT;
                }
                /**
                  * Checks whether content is a line.
                  */
                inline bool isLine() const      {
                        return type==GEOMETRIC_TYPE_LINE;
                }
                /**
                  * Checks whether content is a polygon.
                  */
                inline bool isPolygon() const   {
                        return type==GEOMETRIC_TYPE_POLYGON;
                }
                /**
                  * Gets content type.
                  */
                inline unsigned char getType() const    {
                        return type;
                }
                /**
                  * Gets the content as a point, returning false if the type is inadequate.
                  */
                inline bool getPoint(TPoint2D &p) const {
                        if (isPoint())  {
                                p=data.point;
                                return true;
                        }       else return false;
                }
                /**
                  * Gets the content as a segment, returning false if the type is inadequate.
                  */
                inline bool getSegment(TSegment2D &s) const     {
                        if (isSegment())        {
                                s=data.segment;
                                return true;
                        }       else return false;
                }
                /**
                  * Gets the content as a line, returning false if the type is inadequate.
                  */
                inline bool getLine(TLine2D &r) const   {
                        if (isLine())   {
                                r=data.line;
                                return true;
                        }       else return false;
                }
                /**
                  * Gets the content as a polygon, returning false if the type is inadequate.
                  */
                inline bool getPolygon(TPolygon2D &p) const     {
                        if (isPolygon())        {
                                p=*(data.polygon);
                                return true;
                        }       else return false;
                }
                /**
                  * Assign another TObject2D. Pointers are not shared.
                  */
                TObject2D & operator=(const TObject2D &obj)     {
                        if (this==&obj) return *this;
                        destroy();
                        switch (type=obj.type)  {
                                case GEOMETRIC_TYPE_POINT:
                                        data.point=obj.data.point;
                                        break;
                                case GEOMETRIC_TYPE_SEGMENT:
                                        data.segment=obj.data.segment;
                                        break;
                                case GEOMETRIC_TYPE_LINE:
                                        data.line=obj.data.line;
                                        break;
                                case GEOMETRIC_TYPE_POLYGON:
                                        data.polygon=new TPolygon2D(*(obj.data.polygon));
                                        break;
                        }
                        return *this;
                }
                /**
                  * Assign a point to this object.
                  */
                inline void operator=(const TPoint2D &p)        {
                        destroy();
                        type=GEOMETRIC_TYPE_POINT;
                        data.point=p;
                }
                /**
                  * Assign a segment to this object.
                  */
                inline void operator=(const TSegment2D &s)      {
                        destroy();
                        type=GEOMETRIC_TYPE_SEGMENT;
                        data.segment=s;
                }
                /**
                  * Assign a line to this object.
                  */
                inline void operator=(const TLine2D &l) {
                        destroy();
                        type=GEOMETRIC_TYPE_LINE;
                        data.line=l;
                }
                /**
                  * Assign a polygon to this object.
                  */
                inline void operator=(const TPolygon2D &p)      {
                        destroy();
                        type=GEOMETRIC_TYPE_POLYGON;
                        data.polygon=new TPolygon2D(p);
                }
                /**
                  * Project into 3D space.
                  */
                void generate3DObject(TObject3D &obj) const;
                /**
                  * Constructor from another TObject2D.
                  */
                TObject2D(const TObject2D &obj):type(GEOMETRIC_TYPE_UNDEFINED)  {
                        operator=(obj);
                }
                /**
                  * Static method to retrieve all the points in a vector of TObject2D.
                  */
                static void getPoints(const std::vector<TObject2D> &objs,std::vector<TPoint2D> &pnts);
                /**
                  * Static method to retrieve all the segments in a vector of TObject2D.
                  */
                static void getSegments(const std::vector<TObject2D> &objs,std::vector<TSegment2D> &sgms);
                /**
                  * Static method to retrieve all the lines in a vector of TObject2D.
                  */
                static void getLines(const std::vector<TObject2D> &objs,std::vector<TLine2D> &lins);
                /**
                  * Static method to retrieve all the polygons in a vector of TObject2D.
                  */
                static void getPolygons(const std::vector<TObject2D> &objs,std::vector<TPolygon2D> &polys);
                /**
                  * Static method to retrieve all the points in a vector of TObject2D, returning the remainder objects in another parameter.
                  */
                static void getPoints(const std::vector<TObject2D> &objs,std::vector<TPoint2D> &pnts,std::vector<TObject2D> &remainder);
                /**
                  * Static method to retrieve all the segments in a vector of TObject2D, returning the remainder objects in another parameter.
                  */
                static void getSegments(const std::vector<TObject2D> &objs,std::vector<TSegment2D> &sgms,std::vector<TObject2D> &remainder);
                /**
                  * Static method to retrieve all the lines in a vector of TObject2D, returning the remainder objects in another parameter.
                  */
                static void getLines(const std::vector<TObject2D> &objs,std::vector<TLine2D> &lins,std::vector<TObject2D> &remainder);
                /**
                  * Static method to retrieve all the polygons in a vector of TObject2D, returning the remainder objects in another parameter.
                  */
                static void getPolygons(const std::vector<TObject2D> &objs,std::vector<TPolygon2D> &polys,std::vector<TObject2D> &remainder);
        };
        /**
          * Standard object for storing any 3D lightweight object. Do not inherit from this class.
          * \sa TPoint3D,TSegment3D,TLine3D,TPlane,TPolygon3D
          */
        struct BASE_IMPEXP TObject3D    {
        private:
                /**
                  * Object type identifier.
                  */
                unsigned char type;
                /**
                  * Union containing pointer to actual data.
                  */
                struct tobject3d_data_t {
                        TPoint3D point;
                        TSegment3D segment;
                        TLine3D line;
                        TPolygon3D *polygon;
                        TPlane plane;

                        tobject3d_data_t() : polygon(NULL) { }
                }       data;
                /**
                  * Destroys the object and releases the pointer, if any.
                  */
                void destroy()  {
                        if (type==GEOMETRIC_TYPE_POLYGON) delete data.polygon;
                        type=GEOMETRIC_TYPE_UNDEFINED;
                }
        public:
                /**
                  * Constructor from point.
                  */
                TObject3D(const TPoint3D &p):type(GEOMETRIC_TYPE_POINT) {
                        data.point=p;
                }
                /**
                  * Constructor from segment.
                  */
                TObject3D(const TSegment3D &s):type(GEOMETRIC_TYPE_SEGMENT)     {
                        data.segment=s;
                }
                /**
                  * Constructor from line.
                  */
                TObject3D(const TLine3D &r):type(GEOMETRIC_TYPE_LINE)   {
                        data.line=r;
                }
                /**
                  * Constructor from polygon.
                  */
                TObject3D(const TPolygon3D &p):type(GEOMETRIC_TYPE_POLYGON)     {
                        data.polygon=new TPolygon3D(p);
                }
                /**
                  * Constructor from plane.
                  */
                TObject3D(const TPlane &p):type(GEOMETRIC_TYPE_PLANE)   {
                        data.plane=p;
                }
                /**
                  * Empty constructor.
                  */
                TObject3D():type(GEOMETRIC_TYPE_UNDEFINED)      {}
                /**
                  * Destructor.
                  */
                ~TObject3D()    {
                        destroy();
                }
                /**
                  * Checks whether content is a point.
                  */
                inline bool isPoint() const     {
                        return type==GEOMETRIC_TYPE_POINT;
                }
                /**
                  * Checks whether content is a segment.
                  */
                inline bool isSegment() const   {
                        return type==GEOMETRIC_TYPE_SEGMENT;
                }
                /**
                  * Checks whether content is a line.
                  */
                inline bool isLine() const      {
                        return type==GEOMETRIC_TYPE_LINE;
                }
                /**
                  * Checks whether content is a polygon.
                  */
                inline bool isPolygon() const   {
                        return type==GEOMETRIC_TYPE_POLYGON;
                }
                /**
                  * Checks whether content is a plane.
                  */
                inline bool isPlane() const     {
                        return type==GEOMETRIC_TYPE_PLANE;
                }
                /**
                  * Gets object type.
                  */
                inline unsigned char getType() const    {
                        return type;
                }
                /**
                  * Gets the content as a point, returning false if the type is not adequate.
                  */
                inline bool getPoint(TPoint3D &p) const {
                        if (isPoint())  {
                                p=data.point;
                                return true;
                        }       else return false;
                }
                /**
                  * Gets the content as a segment, returning false if the type is not adequate.
                  */
                inline bool getSegment(TSegment3D &s) const     {
                        if (isSegment())        {
                                s=data.segment;
                                return true;
                        }       else return false;
                }
                /**
                  * Gets the content as a line, returning false if the type is not adequate.
                  */
                inline bool getLine(TLine3D &r) const   {
                        if (isLine())   {
                                r=data.line;
                                return true;
                        }       else return false;
                }
                /**
                  * Gets the content as a polygon, returning false if the type is not adequate.
                  */
                inline bool getPolygon(TPolygon3D &p) const     {
                        if (isPolygon())        {
                                p=*(data.polygon);
                                return true;
                        }       else return false;
                }
                /**
                  * Gets the content as a plane, returning false if the type is not adequate.
                  */
                inline bool getPlane(TPlane &p) const   {
                        if (isPlane())  {
                                p=data.plane;
                                return true;
                        }       else return false;
                }
                /**
                  * Assigns another object, creating a new pointer if needed.
                  */
                TObject3D & operator=(const TObject3D &obj)     {
                        if (this==&obj) return *this;
                        destroy();
                        switch (type=obj.type)  {
                                case GEOMETRIC_TYPE_POINT:
                                        data.point=obj.data.point;
                                        break;
                                case GEOMETRIC_TYPE_SEGMENT:
                                        data.segment=obj.data.segment;
                                        break;
                                case GEOMETRIC_TYPE_LINE:
                                        data.line=obj.data.line;
                                        break;
                                case GEOMETRIC_TYPE_POLYGON:
                                        data.polygon=new TPolygon3D(*(obj.data.polygon));
                                        break;
                                case GEOMETRIC_TYPE_PLANE:
                                        data.plane=obj.data.plane;
                                        break;
                                case GEOMETRIC_TYPE_UNDEFINED:
                                        break;
                                default:
                                        THROW_EXCEPTION("Invalid TObject3D object");
                        }
                        return *this;
                }
                /**
                  * Assigns a point to this object.
                  */
                inline void operator=(const TPoint3D &p)        {
                        destroy();
                        type=GEOMETRIC_TYPE_POINT;
                        data.point=p;
                }
                /**
                  * Assigns a segment to this object.
                  */
                inline void operator=(const TSegment3D &s)      {
                        destroy();
                        type=GEOMETRIC_TYPE_SEGMENT;
                        data.segment=s;
                }
                /**
                  * Assigns a line to this object.
                  */
                inline void operator=(const TLine3D &l) {
                        destroy();
                        type=GEOMETRIC_TYPE_LINE;
                        data.line=l;
                }
                /**
                  * Assigns a polygon to this object.
                  */
                inline void operator=(const TPolygon3D &p)      {
                        destroy();
                        type=GEOMETRIC_TYPE_POLYGON;
                        data.polygon=new TPolygon3D(p);
                }
                /**
                  * Assigns a plane to this object.
                  */
                inline void operator=(const TPlane &p)  {
                        destroy();
                        type=GEOMETRIC_TYPE_PLANE;
                        data.plane=p;
                }
                /**
                  * Projects into 2D space.
                  * \throw std::logic_error if the 3D object loses its properties when projecting into 2D space (for example, it's a plane or a vertical line).
                  */
                inline void generate2DObject(TObject2D &obj) const      {
                        switch (type)   {
                                case GEOMETRIC_TYPE_POINT:
                                        obj=TPoint2D(data.point);
                                        break;
                                case GEOMETRIC_TYPE_SEGMENT:
                                        obj=TSegment2D(data.segment);
                                        break;
                                case GEOMETRIC_TYPE_LINE:
                                        obj=TLine2D(data.line);
                                        break;
                                case GEOMETRIC_TYPE_POLYGON:
                                        obj=TPolygon2D(*(data.polygon));
                                        break;
                                case GEOMETRIC_TYPE_PLANE:
                                        throw std::logic_error("Too many dimensions");
                                default:
                                        obj=TObject2D();
                                        break;
                        }
                }
                /**
                  * Constructs from another object.
                  */
                TObject3D(const TObject3D &obj):type(GEOMETRIC_TYPE_UNDEFINED)  {
                        operator=(obj);
                }
                /**
                  * Static method to retrieve every point included in a vector of objects.
                  */
                static void getPoints(const std::vector<TObject3D> &objs,std::vector<TPoint3D> &pnts);
                /**
                  * Static method to retrieve every segment included in a vector of objects.
                  */
                static void getSegments(const std::vector<TObject3D> &objs,std::vector<TSegment3D> &sgms);
                /**
                  * Static method to retrieve every line included in a vector of objects.
                  */
                static void getLines(const std::vector<TObject3D> &objs,std::vector<TLine3D> &lins);
                /**
                  * Static method to retrieve every plane included in a vector of objects.
                  */
                static void getPlanes(const std::vector<TObject3D> &objs,std::vector<TPlane> &plns);
                /**
                  * Static method to retrieve every polygon included in a vector of objects.
                  */
                static void getPolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys);
                /**
                  * Static method to retrieve every point included in a vector of objects, returning the remaining objects in another argument.
                  */
                static void getPoints(const std::vector<TObject3D> &objs,std::vector<TPoint3D> &pnts,std::vector<TObject3D> &remainder);
                /**
                  * Static method to retrieve every segment included in a vector of objects, returning the remaining objects in another argument.
                  */
                static void getSegments(const std::vector<TObject3D> &objs,std::vector<TSegment3D> &sgms,std::vector<TObject3D> &remainder);
                /**
                  * Static method to retrieve every line included in a vector of objects, returning the remaining objects in another argument.
                  */
                static void getLines(const std::vector<TObject3D> &objs,std::vector<TLine3D> &lins,std::vector<TObject3D> &remainder);
                /**
                  * Static method to retrieve every plane included in a vector of objects, returning the remaining objects in another argument.
                  */
                static void getPlanes(const std::vector<TObject3D> &objs,std::vector<TPlane> &plns,std::vector<TObject3D> &remainder);
                /**
                  * Static method to retrieve every polygon included in a vector of objects, returning the remaining objects in another argument.
                  */
                static void getPolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys,std::vector<TObject3D> &remainder);
        };

        /** 2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
          * \sa mrpt::math::TTwist3D, mrpt::math::TPose2D
          */
        struct BASE_IMPEXP TTwist2D {
                enum { static_size = 3 };
                double vx,vy; //!< Velocity components: X,Y (m/s)
                double omega; //!< Angular velocity (rad/s)

                /** Constructor from components */
                inline TTwist2D(double vx_,double vy_,double omega_):vx(vx_),vy(vy_),omega(omega_) {}
                /** Default fast constructor. Initializes to garbage  */
                inline TTwist2D()       {}
                /** Coordinate access using operator[]. Order: vx,vy,vphi */
                inline double &operator[](size_t i)     { switch (i) { case 0: return vx; case 1: return vy; case 2: return omega; default: throw std::out_of_range("index out of range");  } }
                /** Coordinate access using operator[]. Order: vx,vy,vphi */
                inline const double &operator[](size_t i) const { switch (i) { case 0: return vx; case 1: return vy; case 2: return omega; default: throw std::out_of_range("index out of range");  } }
                /** Transformation into vector */
                inline void getAsVector(std::vector<double> &v) const   {
                        v.resize(3);
                        v[0]=vx; v[1]=vy; v[2]=omega;
                }
                /** Transform the (vx,vy) components for a counterclockwise rotation of `ang` radians. */
                void rotate(const double ang);
                bool operator ==(const TTwist2D&o) const;
                bool operator !=(const TTwist2D&o) const;
                mrpt::math::TPose2D operator *(const double dt) const; //!< Returns the pose increment of multiplying each twist component times "dt" seconds.
                /** Returns a human-readable textual representation of the object (eg: "[vx vy omega]", omega in deg/s)
                   * \sa fromString
                   */
                void asString(std::string &s) const;
                inline std::string asString() const { std::string s; asString(s); return s; }

                 /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -45.0]" )
                   * \sa asString
                   * \exception std::exception On invalid format
                   */
                void fromString(const std::string &s);
                static size_t size() { return 3; }
        };

        /** 3D twist: 3D velocity vector (vx,vy,vz) + angular velocity (wx,wy,wz)
          * \sa mrpt::math::TTwist2D, mrpt::math::TPose3D
          */
        struct BASE_IMPEXP TTwist3D {
                enum { static_size = 6 };
                double vx,vy,vz; //!< Velocity components: X,Y (m/s)
                double wx,wy,wz; //!< Angular velocity (rad/s)

                /** Constructor from components */
                inline TTwist3D(double vx_,double vy_,double vz_,double wx_,double wy_,double wz_):vx(vx_),vy(vy_),vz(vz_),wx(wx_),wy(wy_),wz(wz_) {}
                /** Default fast constructor. Initializes to garbage  */
                inline TTwist3D()       {}
                /** Coordinate access using operator[]. Order: vx,vy,vphi */
                inline double &operator[](size_t i)     { switch (i) { case 0: return vx; case 1: return vy; case 2: return vz; case 3: return wx;  case 4: return wy;  case 5: return wz; default: throw std::out_of_range("index out of range");  } }
                /** Coordinate access using operator[]. Order: vx,vy,vphi */
                inline const double &operator[](size_t i) const { switch (i) { case 0: return vx; case 1: return vy; case 2: return vz; case 3: return wx;  case 4: return wy;  case 5: return wz; default: throw std::out_of_range("index out of range");  } }
                /** Transformation into vector */
                inline void getAsVector(std::vector<double> &v) const   {
                        v.resize(6);
                        for (int i=0;i<6;i++) v[i]=(*this)[i];
                }
                bool operator ==(const TTwist3D&o) const;
                bool operator !=(const TTwist3D&o) const;
                /** Returns a human-readable textual representation of the object (eg: "[vx vy vz wx wy wz]", omegas in deg/s)
                   * \sa fromString
                   */
                void asString(std::string &s) const;
                inline std::string asString() const { std::string s; asString(s); return s; }

                /** Transform all 6 components for a change of reference frame from "A" to
                  * another frame "B" whose rotation with respect to "A" is given by `rot`. The translational part of the pose is ignored */
                void rotate(const mrpt::poses::CPose3D & rot);

                 /** Set the current object value from a string generated by 'asString' (eg: "[vx vy vz wx wy wz]" )
                   * \sa asString
                   * \exception std::exception On invalid format
                   */
                 void fromString(const std::string &s);
                 static size_t size() { return 3; }
        };

        // Binary streaming functions
        BASE_IMPEXP mrpt::utils::CStream& operator>>(mrpt::utils::CStream& in,mrpt::math::TPoint2D &o);
        BASE_IMPEXP mrpt::utils::CStream& operator<<(mrpt::utils::CStream& out,const mrpt::math::TPoint2D &o);

        BASE_IMPEXP mrpt::utils::CStream& operator>>(mrpt::utils::CStream& in,mrpt::math::TPoint3D &o);
        BASE_IMPEXP mrpt::utils::CStream& operator<<(mrpt::utils::CStream& out,const mrpt::math::TPoint3D &o);

        BASE_IMPEXP mrpt::utils::CStream& operator>>(mrpt::utils::CStream& in,mrpt::math::TPose2D &o);
        BASE_IMPEXP mrpt::utils::CStream& operator<<(mrpt::utils::CStream& out,const mrpt::math::TPose2D &o);

        BASE_IMPEXP mrpt::utils::CStream& operator>>(mrpt::utils::CStream& in,mrpt::math::TPose3D &o);
        BASE_IMPEXP mrpt::utils::CStream& operator<<(mrpt::utils::CStream& out,const mrpt::math::TPose3D &o);

        BASE_IMPEXP mrpt::utils::CStream &operator>>(mrpt::utils::CStream &in,mrpt::math::TSegment2D &s);
        BASE_IMPEXP mrpt::utils::CStream &operator<<(mrpt::utils::CStream &out,const mrpt::math::TSegment2D &s);

        BASE_IMPEXP mrpt::utils::CStream &operator>>(mrpt::utils::CStream &in,mrpt::math::TLine2D &l);
        BASE_IMPEXP mrpt::utils::CStream &operator<<(mrpt::utils::CStream &out,const mrpt::math::TLine2D &l);

        BASE_IMPEXP mrpt::utils::CStream &operator>>(mrpt::utils::CStream &in,mrpt::math::TObject2D &o);
        BASE_IMPEXP mrpt::utils::CStream &operator<<(mrpt::utils::CStream &out,const mrpt::math::TObject2D &o);

        BASE_IMPEXP mrpt::utils::CStream &operator>>(mrpt::utils::CStream &in,mrpt::math::TSegment3D &s);
        BASE_IMPEXP mrpt::utils::CStream &operator<<(mrpt::utils::CStream &out,const mrpt::math::TSegment3D &s);

        BASE_IMPEXP mrpt::utils::CStream &operator>>(mrpt::utils::CStream &in,mrpt::math::TLine3D &l);
        BASE_IMPEXP mrpt::utils::CStream &operator<<(mrpt::utils::CStream &out,const mrpt::math::TLine3D &l);

        BASE_IMPEXP mrpt::utils::CStream &operator>>(mrpt::utils::CStream &in,mrpt::math::TPlane &p);
        BASE_IMPEXP mrpt::utils::CStream &operator<<(mrpt::utils::CStream &out,const mrpt::math::TPlane &p);

        BASE_IMPEXP mrpt::utils::CStream &operator>>(mrpt::utils::CStream &in,mrpt::math::TObject3D &o);
        BASE_IMPEXP mrpt::utils::CStream &operator<<(mrpt::utils::CStream &out,const mrpt::math::TObject3D &o);

        BASE_IMPEXP mrpt::utils::CStream& operator>>(mrpt::utils::CStream& in,mrpt::math::TTwist2D &o);
        BASE_IMPEXP mrpt::utils::CStream& operator<<(mrpt::utils::CStream& out,const mrpt::math::TTwist2D &o);

        BASE_IMPEXP mrpt::utils::CStream& operator>>(mrpt::utils::CStream& in,mrpt::math::TTwist3D &o);
        BASE_IMPEXP mrpt::utils::CStream& operator<<(mrpt::utils::CStream& out,const mrpt::math::TTwist3D &o);

        /** @} */ // end of grouping

        }       //end of namespace math

        namespace utils
        {
                // Specialization must occur in the same namespace
                MRPT_DECLARE_TTYPENAME_NAMESPACE(TPoint2D,mrpt::math)
                MRPT_DECLARE_TTYPENAME_NAMESPACE(TPoint3D,mrpt::math)
                MRPT_DECLARE_TTYPENAME_NAMESPACE(TPose2D,mrpt::math)
                MRPT_DECLARE_TTYPENAME_NAMESPACE(TPose3D,mrpt::math)
                MRPT_DECLARE_TTYPENAME_NAMESPACE(TSegment2D,mrpt::math)
                MRPT_DECLARE_TTYPENAME_NAMESPACE(TLine2D,mrpt::math)
                MRPT_DECLARE_TTYPENAME_NAMESPACE(TPolygon2D,mrpt::math)
                MRPT_DECLARE_TTYPENAME_NAMESPACE(TObject2D,mrpt::math)
                MRPT_DECLARE_TTYPENAME_NAMESPACE(TSegment3D,mrpt::math)
                MRPT_DECLARE_TTYPENAME_NAMESPACE(TLine3D,mrpt::math)
                MRPT_DECLARE_TTYPENAME_NAMESPACE(TPlane,mrpt::math)
                MRPT_DECLARE_TTYPENAME_NAMESPACE(TPolygon3D,mrpt::math)
                MRPT_DECLARE_TTYPENAME_NAMESPACE(TObject3D,mrpt::math)
                MRPT_DECLARE_TTYPENAME_NAMESPACE(TTwist2D,mrpt::math)
                MRPT_DECLARE_TTYPENAME_NAMESPACE(TTwist3D,mrpt::math)

        } // end of namespace utils

}       //end of namespace
#endif