Main MRPT website > C++ reference for MRPT 1.4.0
CPose2D.h
Go to the documentation of this file.
1/* +---------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | http://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file |
6 | See: http://www.mrpt.org/Authors - All rights reserved. |
7 | Released under BSD License. See details in http://www.mrpt.org/License |
8 +---------------------------------------------------------------------------+ */
9#ifndef CPOSE2D_H
10#define CPOSE2D_H
11
12#include <mrpt/poses/CPose.h>
14
15namespace mrpt
16{
17namespace poses
18{
20
21 /** A class used to store a 2D pose.
22 * A class used to store a 2D pose, including the 2D coordinate
23 * point and a heading (phi) angle.
24 *
25 * For a complete description of Points/Poses, see mrpt::poses::CPoseOrPoint, or refer
26 * to the <a href="http://www.mrpt.org/2D_3D_Geometry" >2D/3D Geometry tutorial</a> in the wiki.
27 *
28 * <div align=center>
29 * <img src="CPose2D.gif">
30 * </div>
31 *
32 * \note Read also: "A tutorial on SE(3) transformation parameterizations and on-manifold optimization", Jose-Luis Blanco. http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf
33 * \sa CPoseOrPoint,CPoint2D
34 * \ingroup poses_grp
35 */
36 class BASE_IMPEXP CPose2D : public CPose<CPose2D>, public mrpt::utils::CSerializable
37 {
38 public:
39 // This must be added to any CSerializable derived class:
41
42 public:
44
45 protected:
46 double m_phi; //!< The orientation of the pose, in radians.
47 mutable double m_cosphi, m_sinphi; //!< Precomputed cos() & sin() of phi.
48 mutable bool m_cossin_uptodate;
49
50 inline void update_cached_cos_sin() const {
51 if (m_cossin_uptodate) return;
52#ifdef HAVE_SINCOS
53 ::sincos(m_phi,&m_sinphi,&m_cosphi);
54#else
55 m_cosphi=::cos(m_phi);
56 m_sinphi=::sin(m_phi);
57#endif
58 m_cossin_uptodate=true;
59 }
60
61 public:
62 /** Default constructor (all coordinates to 0) */
64
65 /** Constructor from an initial value of the pose.*/
66 CPose2D(const double x,const double y,const double phi);
67
68 /** Constructor from a CPoint2D object. */
69 CPose2D(const CPoint2D &);
70
71 /** Aproximation!! Avoid its use, since information is lost. */
72 explicit CPose2D(const CPose3D &);
73
74 /** Constructor from lightweight object. */
76
77 /** Constructor from CPoint3D with information loss. */
78 explicit CPose2D(const CPoint3D &);
79
80 /** Fast constructor that leaves all the data uninitialized - call with UNINITIALIZED_POSE as argument */
81 inline CPose2D(TConstructorFlags_Poses ) : m_cossin_uptodate(false) { }
82
83 /** Get the phi angle of the 2D pose (in radians) */
84 inline const double &phi() const { return m_phi; }
85 //! \overload
86 inline double &phi() { m_cossin_uptodate=false; return m_phi; } //-V659
87
88 /** Get a (cached) value of cos(phi), recomputing it only once when phi changes. */
89 inline double phi_cos() const { update_cached_cos_sin(); return m_cosphi; }
90 /** Get a (cached) value of sin(phi), recomputing it only once when phi changes. */
91 inline double phi_sin() const { update_cached_cos_sin(); return m_sinphi; }
92
93 /** Set the phi angle of the 2D pose (in radians) */
94 inline void phi(double angle) { m_phi=angle; m_cossin_uptodate=false; }
95
96 inline void phi_incr(const double Aphi) { m_phi+=Aphi; m_cossin_uptodate=false; } //!< Increment the PHI angle (without checking the 2 PI range, call normalizePhi is needed)
97
98 /** Returns a 1x3 vector with [x y phi] */
100 /// \overload
102
103 /** Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (translation+orientation).
104 * \sa getInverseHomogeneousMatrix
105 */
107
108 void getRotationMatrix(mrpt::math::CMatrixDouble22 &R) const; //!< Returns the SE(2) 2x2 rotation matrix
109 void getRotationMatrix(mrpt::math::CMatrixDouble33 &R) const; //!< Returns the equivalent SE(3) 3x3 rotation matrix, with (2,2)=1.
110
113 getRotationMatrix(R);
114 return R;
115 }
116
117 /** The operator \f$ a = this \oplus D \f$ is the pose compounding operator. */
118 CPose2D operator + (const CPose2D& D) const ;
119
120 /** Makes \f$ this = A \oplus B \f$
121 * \note A or B can be "this" without problems. */
122 void composeFrom(const CPose2D &A, const CPose2D &B);
123
124 /** The operator \f$ a = this \oplus D \f$ is the pose compounding operator. */
125 CPose3D operator + (const CPose3D& D) const ;
126
127 /** The operator \f$ u' = this \oplus u \f$ is the pose/point compounding operator. */
128 CPoint2D operator + (const CPoint2D& u) const ;
129
130 /** An alternative, slightly more efficient way of doing \f$ G = P \oplus L \f$ with G and L being 2D points and P this 2D pose. */
131 void composePoint(double lx,double ly,double &gx, double &gy) const;
132
133 /** \overload \f$ G = P \oplus L \f$ with G and L being 2D points and P this 2D pose */
135
136 /** \overload \f$ G = P \oplus L \f$ with G and L being 3D points and P this 2D pose (the "z" coordinate remains unmodified) */
138 /** \overload (the "z" coordinate remains unmodified) */
139 void composePoint(double lx,double ly,double lz, double &gx, double &gy, double &gz) const;
140
141 /** The operator \f$ u' = this \oplus u \f$ is the pose/point compounding operator. */
142 CPoint3D operator + (const CPoint3D& u) const ;
143
144 /** Makes \f$ this = A \ominus B \f$ this method is slightly more efficient than "this= A - B;" since it avoids the temporary object.
145 * \note A or B can be "this" without problems.
146 * \sa composeFrom, composePoint
147 */
148 void inverseComposeFrom(const CPose2D& A, const CPose2D& B );
149
150 /** Convert this pose into its inverse, saving the result in itself. \sa operator- */
151 void inverse();
152
153 /** Compute \f$ RET = this \oplus b \f$ */
154 inline CPose2D operator - (const CPose2D& b) const
155 {
157 ret.inverseComposeFrom(*this,b);
158 return ret;
159 }
160
161 /** The operator \f$ a \ominus b \f$ is the pose inverse compounding operator. */
162 CPose3D operator -(const CPose3D& b) const;
163
164
165 /** Scalar sum of components: This is diferent from poses
166 * composition, which is implemented as "+" operators in "CPose" derived classes.
167 */
169
170 /** Scalar multiplication.
171 */
172 void operator *=(const double s);
173
174 /** Make \f$ this = this \oplus b \f$ */
175 CPose2D& operator += (const CPose2D& b);
176
177 /** Forces "phi" to be in the range [-pi,pi];
178 */
180
181 /** Returns a human-readable textual representation of the object (eg: "[x y yaw]", yaw in degrees)
182 * \sa fromString
183 */
184 void asString(std::string &s) const;
185 inline std::string asString() const { std::string s; asString(s); return s; }
186
187 /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0.8]" )
188 * \sa asString
189 * \exception std::exception On invalid format
190 */
191 void fromString(const std::string &s);
192
193 inline const double &operator[](unsigned int i)const
194 {
195 switch(i)
196 {
197 case 0:return m_coords[0];
198 case 1:return m_coords[1];
199 case 2:return m_phi;
200 default:
201 throw std::runtime_error("CPose2D::operator[]: Index of bounds.");
202 }
203 }
204 inline double &operator[](unsigned int i)
205 {
206 switch(i)
207 {
208 case 0:return m_coords[0];
209 case 1:return m_coords[1];
210 case 2:return m_phi;
211 default:
212 throw std::runtime_error("CPose2D::operator[]: Index of bounds.");
213 }
214 }
215
216 /** makes: this = p (+) this */
217 inline void changeCoordinatesReference( const CPose2D & p ) { composeFrom(p,CPose2D(*this)); }
218
219 /** Returns the 2D distance from this pose/point to a 2D pose using the Frobenius distance. */
220 double distance2DFrobeniusTo( const CPose2D & p) const;
221
222 typedef CPose2D type_value; //!< Used to emulate CPosePDF types, for example, in mrpt::graphs::CNetworkOfPoses
223 enum { is_3D_val = 0 };
224 static inline bool is_3D() { return is_3D_val!=0; }
225 enum { rotation_dimensions = 2 };
226 enum { is_PDF_val = 0 };
227 static inline bool is_PDF() { return is_PDF_val!=0; }
228
229 inline const type_value & getPoseMean() const { return *this; }
230 inline type_value & getPoseMean() { return *this; }
231
233
234 /** @name STL-like methods and typedefs
235 @{ */
236 typedef double value_type; //!< The type of the elements
237 typedef double& reference;
238 typedef const double& const_reference;
239 typedef std::size_t size_type;
240 typedef std::ptrdiff_t difference_type;
241
242 // size is constant
243 enum { static_size = 3 };
244 static inline size_type size() { return static_size; }
245 static inline bool empty() { return false; }
246 static inline size_type max_size() { return static_size; }
247 static inline void resize(const size_t n) { if (n!=static_size) throw std::logic_error(format("Try to change the size of CPose2D to %u.",static_cast<unsigned>(n))); }
248
249 /** @} */
250
251 }; // End of class def.
252 DEFINE_SERIALIZABLE_POST( CPose2D )
253
254
255 std::ostream BASE_IMPEXP & operator << (std::ostream& o, const CPose2D& p);
256
257 /** Unary - operator: return the inverse pose "-p" (Note that is NOT the same than a pose with negative x y phi) \sa CPose2D::inverse() */
258 CPose2D BASE_IMPEXP operator -(const CPose2D &p);
259
260 mrpt::math::TPoint2D BASE_IMPEXP operator +(const CPose2D &pose, const mrpt::math::TPoint2D &pnt); //!< Compose a 2D point from a new coordinate base given by a 2D pose.
261
262 bool BASE_IMPEXP operator==(const CPose2D &p1,const CPose2D &p2);
263 bool BASE_IMPEXP operator!=(const CPose2D &p1,const CPose2D &p2);
264
265 typedef mrpt::aligned_containers<CPose2D>::vector_t StdVector_CPose2D; //!< Eigen aligment-compatible container
266 typedef mrpt::aligned_containers<CPose2D>::deque_t StdDeque_CPose2D; //!< Eigen aligment-compatible container
267
268 } // End of namespace
269} // End of namespace
270
271#endif
#define DEFINE_SERIALIZABLE_PRE(class_name)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
#define DEFINE_SERIALIZABLE_POST(class_name)
A partial specialization of CArrayNumeric for double numbers.
Definition: CArrayNumeric.h:75
A numeric matrix of compile-time fixed size.
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction.
Definition: types_math.h:65
A class used to store a 2D point.
Definition: CPoint2D.h:37
A class used to store a 3D point.
Definition: CPoint3D.h:33
A class used to store a 2D pose.
Definition: CPose2D.h:37
CPose2D(const CPoint3D &)
Constructor from CPoint3D with information loss.
std::ptrdiff_t difference_type
Definition: CPose2D.h:240
type_value & getPoseMean()
Definition: CPose2D.h:230
void fromString(const std::string &s)
Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0....
const type_value & getPoseMean() const
Definition: CPose2D.h:229
static size_type max_size()
Definition: CPose2D.h:246
void phi(double angle)
Set the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:94
void composePoint(double lx, double ly, double &gx, double &gy) const
An alternative, slightly more efficient way of doing with G and L being 2D points and P this 2D pose...
void setToNaN() MRPT_OVERRIDE
Set all data fields to quiet NaN.
const double & const_reference
Definition: CPose2D.h:238
void inverseComposeFrom(const CPose2D &A, const CPose2D &B)
Makes this method is slightly more efficient than "this= A - B;" since it avoids the temporary objec...
void composeFrom(const CPose2D &A, const CPose2D &B)
Makes .
CPose2D type_value
Used to emulate CPosePDF types, for example, in mrpt::graphs::CNetworkOfPoses.
Definition: CPose2D.h:222
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
void getAsVector(mrpt::math::CVectorDouble &v) const
Returns a 1x3 vector with [x y phi].
mrpt::math::CArrayDouble< 2 > m_coords
[x,y]
Definition: CPose2D.h:43
void phi_incr(const double Aphi)
Increment the PHI angle (without checking the 2 PI range, call normalizePhi is needed)
Definition: CPose2D.h:96
void composePoint(const mrpt::math::TPoint3D &l, mrpt::math::TPoint3D &g) const
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz) const
CPose2D()
Default constructor (all coordinates to 0)
double phi_sin() const
Get a (cached) value of sin(phi), recomputing it only once when phi changes.
Definition: CPose2D.h:91
bool m_cossin_uptodate
Definition: CPose2D.h:48
void normalizePhi()
Forces "phi" to be in the range [-pi,pi];.
std::string asString() const
Definition: CPose2D.h:185
static void resize(const size_t n)
Definition: CPose2D.h:247
double & phi()
This is an overloaded member function, provided for convenience. It differs from the above function o...
Definition: CPose2D.h:86
void getRotationMatrix(mrpt::math::CMatrixDouble22 &R) const
Returns the SE(2) 2x2 rotation matrix.
CPose2D(const CPose3D &)
Aproximation!! Avoid its use, since information is lost.
std::size_t size_type
Definition: CPose2D.h:239
void changeCoordinatesReference(const CPose2D &p)
makes: this = p (+) this
Definition: CPose2D.h:217
const double & operator[](unsigned int i) const
Definition: CPose2D.h:193
void AddComponents(CPose2D &p)
Scalar sum of components: This is diferent from poses composition, which is implemented as "+" operat...
double distance2DFrobeniusTo(const CPose2D &p) const
Returns the 2D distance from this pose/point to a 2D pose using the Frobenius distance.
static bool empty()
Definition: CPose2D.h:245
void getAsVector(mrpt::math::CArrayDouble< 3 > &v) const
This is an overloaded member function, provided for convenience. It differs from the above function o...
static bool is_3D()
Definition: CPose2D.h:224
static size_type size()
Definition: CPose2D.h:244
double m_phi
The orientation of the pose, in radians.
Definition: CPose2D.h:46
static bool is_PDF()
Definition: CPose2D.h:227
void update_cached_cos_sin() const
Definition: CPose2D.h:50
mrpt::math::CMatrixDouble22 getRotationMatrix() const
Definition: CPose2D.h:111
double & operator[](unsigned int i)
Definition: CPose2D.h:204
void composePoint(const mrpt::math::TPoint2D &l, mrpt::math::TPoint2D &g) const
CPose2D(const double x, const double y, const double phi)
Constructor from an initial value of the pose.
CPose2D(TConstructorFlags_Poses)
Fast constructor that leaves all the data uninitialized - call with UNINITIALIZED_POSE as argument.
Definition: CPose2D.h:81
double phi_cos() const
Get a (cached) value of cos(phi), recomputing it only once when phi changes.
Definition: CPose2D.h:89
double value_type
The type of the elements.
Definition: CPose2D.h:236
CPose2D(const CPoint2D &)
Constructor from a CPoint2D object.
void asString(std::string &s) const
Returns a human-readable textual representation of the object (eg: "[x y yaw]", yaw in degrees)
CPose2D(const mrpt::math::TPose2D &)
Constructor from lightweight object.
void inverse()
Convert this pose into its inverse, saving the result in itself.
double & reference
Definition: CPose2D.h:237
void getRotationMatrix(mrpt::math::CMatrixDouble33 &R) const
Returns the equivalent SE(3) 3x3 rotation matrix, with (2,2)=1.
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:73
A base class for representing a pose in 2D or 3D.
Definition: CPose.h:26
@ static_size
Definition: eigen_plugins.h:17
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
Definition: mrpt_macros.h:28
@ UNINITIALIZED_MATRIX
Definition: math_frwds.h:75
mrpt::aligned_containers< CPose2D >::deque_t StdDeque_CPose2D
Eigen aligment-compatible container.
Definition: CPose2D.h:266
CPose2D BASE_IMPEXP operator-(const CPose2D &p)
Unary - operator: return the inverse pose "-p" (Note that is NOT the same than a pose with negative x...
mrpt::aligned_containers< CPose2D >::vector_t StdVector_CPose2D
Eigen aligment-compatible container.
Definition: CPose2D.h:265
@ UNINITIALIZED_POSE
Definition: CPoseOrPoint.h:35
mrpt::math::TPoint2D BASE_IMPEXP operator+(const CPose2D &pose, const mrpt::math::TPoint2D &pnt)
Compose a 2D point from a new coordinate base given by a 2D pose.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
STL namespace.
Helper types for STL containers with Eigen memory allocators.
Lightweight 2D point.
Lightweight 3D point.
Lightweight 2D pose.



Page generated by Doxygen 1.9.6 for MRPT 1.4.0 SVN: at Wed Mar 22 20:12:58 UTC 2023