Main MRPT website > C++ reference for MRPT 1.5.6
CPose2D.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, 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 
10 #include "base-precomp.h" // Precompiled headers
11 
12 #include <mrpt/poses/CPose2D.h>
13 #include <mrpt/poses/CPoint2D.h>
14 #include <mrpt/poses/CPose3D.h>
15 #include <mrpt/poses/CPoint3D.h>
16 #include <mrpt/utils/CStream.h>
17 #include <mrpt/math/wrap2pi.h>
18 #include <limits>
19 
20 using namespace mrpt;
21 using namespace mrpt::math;
22 using namespace mrpt::poses;
23 using namespace mrpt::utils;
24 
26 
27 /*---------------------------------------------------------------
28  Constructors
29  ---------------------------------------------------------------*/
30 CPose2D::CPose2D() : m_phi(0),m_cossin_uptodate(false)
31 {
32  m_coords[0] =
33  m_coords[1] = 0;
34 }
35 
36 CPose2D::CPose2D(const double x,const double y,const double _phi) : m_phi(_phi),m_cossin_uptodate(false)
37 {
38  m_coords[0] = x;
39  m_coords[1] = y;
40  normalizePhi();
41 }
42 
43 CPose2D::CPose2D(const CPoint2D &p) : m_phi(0),m_cossin_uptodate(false)
44 {
45  m_coords[0] = p.x();
46  m_coords[1] = p.y();
47 }
48 
49 CPose2D::CPose2D(const CPose3D &p) : m_phi(p.yaw()),m_cossin_uptodate(false)
50 {
51  m_coords[0] = p.x();
52  m_coords[1] = p.y();
53 }
54 
55 /*---------------------------------------------------------------
56  Implements the writing to a CStream capability of
57  CSerializable objects
58  ---------------------------------------------------------------*/
60 {
61  if (version)
62  *version = 1;
63  else
64  {
65  // The coordinates:
66  out << m_coords[0] << m_coords[1] << m_phi;
67  }
68 }
69 
70 /*---------------------------------------------------------------
71  Implements the reading from a CStream capability of
72  CSerializable objects
73  ---------------------------------------------------------------*/
75 {
76  switch(version)
77  {
78  case 0:
79  {
80  // The coordinates:
81  float x0,y0,phi0;
82  in >> x0 >> y0 >> phi0;
83  m_coords[0] = x0;
84  m_coords[1] = y0;
85  this->phi( phi0 );
86  } break;
87  case 1:
88  {
89  // The coordinates:
90  in >> m_coords[0] >> m_coords[1] >> m_phi;
91  m_cossin_uptodate=false;
92  } break;
93  default:
95  };
96 }
97 
98 /** Textual output stream function.
99  */
100 std::ostream& mrpt::poses::operator << (std::ostream& o, const CPose2D& p)
101 {
102  o << format("(%.03f,%.03f,%.02fdeg)",p.x(),p.y(),RAD2DEG(p.phi()));
103  return o;
104 }
105 
106 /*---------------------------------------------------------------
107 The operator a="this"+D is the pose compounding operator.
108  ---------------------------------------------------------------*/
110 {
112 
113  return CPose2D(
114  m_coords[0] + D.m_coords[0] * m_cosphi - D.m_coords[1] * m_sinphi,
115  m_coords[1] + D.m_coords[0] * m_sinphi + D.m_coords[1] * m_cosphi,
116  m_phi + D.m_phi );
117 }
118 
119 /*---------------------------------------------------------------
120  composeFrom
121  ---------------------------------------------------------------*/
122 void CPose2D::composeFrom(const CPose2D &A, const CPose2D &B)
123 {
125 
126  // Use temporary variables for the cases (A==this) or (B==this)
127  const double new_x = A.m_coords[0] + B.m_coords[0] * A.m_cosphi - B.m_coords[1] * A.m_sinphi;
128  const double new_y = A.m_coords[1] + B.m_coords[0] * A.m_sinphi + B.m_coords[1] * A.m_cosphi;
129  m_coords[0] = new_x;
130  m_coords[1] = new_y;
131 
132  m_phi = math::wrapToPi(A.m_phi + B.m_phi);
133  m_cossin_uptodate=false;
134 }
135 
136 /*---------------------------------------------------------------
137  getRotationMatrix
138  ---------------------------------------------------------------*/
140 {
142  R(0,0) = m_cosphi; R(0,1) = -m_sinphi;
143  R(1,0) = m_sinphi; R(1,1) = m_cosphi;
144 }
145 
147 {
149  R(0,0) = m_cosphi; R(0,1) = -m_sinphi; R(0,2)=0;
150  R(1,0) = m_sinphi; R(1,1) = m_cosphi; R(1,2)=0;
151  R(2,0) = 0; R(2,1)=0; R(2,2)=1;
152 }
153 
154 
155 
156 
157 /*---------------------------------------------------------------
158 The operator a="this"+D is the pose compounding operator.
159  ---------------------------------------------------------------*/
161 {
162  return CPose3D(*this) + D;
163 }
164 
165 /*---------------------------------------------------------------
166 The operator u'="this"+u is the pose/point compounding operator.
167  ---------------------------------------------------------------*/
169 {
171 
172  return CPoint2D(
173  m_coords[0] + u.x() * m_cosphi - u.y() * m_sinphi,
174  m_coords[1] + u.x() * m_sinphi + u.y() * m_cosphi );
175 }
176 
177 /** 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. */
178 void CPose2D::composePoint(double lx,double ly,double &gx, double &gy) const
179 {
181 
182  gx = m_coords[0] + lx * m_cosphi - ly * m_sinphi;
183  gy = m_coords[1] + lx * m_sinphi + ly * m_cosphi;
184 }
185 
187 {
188  this->composePoint(l.x,l.y, g.x,g.y);
189 }
190 
191 /** overload \f$ G = P \oplus L \f$ with G and L being 3D points and P this 2D pose (the "z" coordinate remains unmodified) */
193 {
194  this->composePoint(l.x,l.y,l.z, g.x,g.y,g.z);
195 }
196 
197 void CPose2D::composePoint(double lx,double ly,double lz, double &gx, double &gy, double &gz) const
198 {
200  gx = m_coords[0] + lx * m_cosphi - ly * m_sinphi;
201  gy = m_coords[1] + lx * m_sinphi + ly * m_cosphi;
202  gz = lz;
203 }
204 
205 void CPose2D::inverseComposePoint(const double gx,const double gy, double &lx,double &ly) const
206 {
208 
209  const double Ax = gx - m_coords[0];
210  const double Ay = gy - m_coords[1];
211 
212  lx = Ax * m_cosphi + Ay * m_sinphi;
213  ly =-Ax * m_sinphi + Ay * m_cosphi;
214 }
215 
216 /*---------------------------------------------------------------
217 The operator u'="this"+u is the pose/point compounding operator.
218  ---------------------------------------------------------------*/
220 {
222 
223  return CPoint3D(
224  m_coords[0] + u.x() * m_cosphi - u.y() * m_sinphi,
225  m_coords[1] + u.x() * m_sinphi + u.y() * m_cosphi,
226  u.z() );
227 }
228 
229 /*---------------------------------------------------------------
230 The operator D="this"-b is the pose inverse compounding operator.
231  The resulting pose "D" is the diference between this pose and "b"
232  ---------------------------------------------------------------*/
233 void CPose2D::inverseComposeFrom(const CPose2D& A, const CPose2D& B )
234 {
236 
237  m_coords[0] = (A.m_coords[0] - B.m_coords[0]) * B.m_cosphi + (A.m_coords[1] - B.m_coords[1]) * B.m_sinphi;
238  m_coords[1] =-(A.m_coords[0] - B.m_coords[0]) * B.m_sinphi + (A.m_coords[1] - B.m_coords[1]) * B.m_cosphi;
239  m_phi = math::wrapToPi(A.m_phi - B.m_phi);
240  m_cossin_uptodate=false;
241 }
242 
243 /*---------------------------------------------------------------
244  Scalar sum of components: This is diferent from poses
245  composition, which is implemented as "+" operators in "CPose" derived classes.
246  ---------------------------------------------------------------*/
248 {
249  m_coords[0]+=p.m_coords[0];
250  m_coords[1]+=p.m_coords[1];
251  m_phi+=p.m_phi;
252  m_cossin_uptodate=false;
253 }
254 
255 
256 /*---------------------------------------------------------------
257  Scalar multiplication.
258  ---------------------------------------------------------------*/
259 void CPose2D::operator *= (const double s)
260 {
261  m_coords[0]*=s;
262  m_coords[1]*=s;
263  m_phi*=s;
264  m_cossin_uptodate=false;
265 }
266 
267 /*---------------------------------------------------------------
268  Returns the corresponding 4x4 homogeneous
269  transformation matrix for the point(translation),
270  or pose (translation+orientation).
271 ---------------------------------------------------------------*/
273 {
274  m.unit(4,1.0);
275 
276  m.set_unsafe(0,3, m_coords[0] );
277  m.set_unsafe(1,3, m_coords[1] );
278 
280 
281  m.get_unsafe(0,0) = m_cosphi; m.get_unsafe(0,1)=-m_sinphi;
282  m.get_unsafe(1,0) = m_sinphi; m.get_unsafe(1,1)= m_cosphi;
283 }
284 
285 /** Forces "phi" to be in the range [-pi,pi];
286 */
288 {
290  m_cossin_uptodate=false;
291 }
292 
293 CPose2D::CPose2D(const mrpt::math::TPose2D &o):m_phi(o.phi),m_cossin_uptodate(false) {
294  m_coords[0]=o.x;
295  m_coords[1]=o.y;
296 }
297 
298 CPose2D::CPose2D(const CPoint3D &o): m_phi(0),m_cossin_uptodate(false) {
299  this->m_coords[0] = o.x();
300  this->m_coords[1] = o.y();
301  normalizePhi();
302 }
303 
304 /*---------------------------------------------------------------
305  unary -
306 ---------------------------------------------------------------*/
308 {
309  CPose2D ret = b;
310  ret.inverse();
311  return ret;
312 }
313 
314 /** Convert this pose into its inverse, saving the result in itself. \sa operator- */
316 {
318  const double x = m_coords[0];
319  const double y = m_coords[1];
320 
321  m_coords[0] = -x * m_cosphi - y * m_sinphi;
322  m_coords[1] = x * m_sinphi - y * m_cosphi;
323  m_phi = - m_phi;
324  m_cossin_uptodate=false;
325 }
326 
327 
328 /*---------------------------------------------------------------
329  getAsVector
330 ---------------------------------------------------------------*/
332 {
333  v.resize(3);
334  v[0]=m_coords[0];
335  v[1]=m_coords[1];
336  v[2]=m_phi;
337 }
338 
340 {
341  v[0]=m_coords[0];
342  v[1]=m_coords[1];
343  v[2]=m_phi;
344 }
345 
346 
347 
348 bool mrpt::poses::operator==(const CPose2D &p1,const CPose2D &p2)
349 {
350  return (p1.x()==p2.x())&&(p1.y()==p2.y())&&(p1.phi()==p2.phi());
351 }
352 
353 bool mrpt::poses::operator!=(const CPose2D &p1,const CPose2D &p2)
354 {
355  return (p1.x()!=p2.x())||(p1.y()!=p2.y())||(p1.phi()!=p2.phi());
356 }
357 
358 
360 {
361  const double ccos = pose.phi_cos();
362  const double ssin = pose.phi_sin();
363  return TPoint2D(
364  pose.x() + u.x * ccos - u.y * ssin,
365  pose.y() + u.x * ssin + u.y * ccos );
366 }
367 
368 
369 /** Make \f$ this = this \oplus b \f$ */
371 {
372  composeFrom(*this,b);
373  return *this;
374 }
375 
377 {
379  if (!m.fromMatlabStringFormat(s)) THROW_EXCEPTION("Malformed expression in ::fromString");
380  ASSERTMSG_(mrpt::math::size(m,1)==1 && mrpt::math::size(m,2)==3, "Wrong size of vector in ::fromString");
381  x( m.get_unsafe(0,0) );
382  y( m.get_unsafe(0,1) );
383  phi( DEG2RAD(m.get_unsafe(0,2)) );
384 }
385 
387 {
388  return std::sqrt(square(p.x()-x())+square(p.y()-y())+4*(1-cos(p.phi()-phi())));
389 }
390 
392 {
394  b.getInverseHomogeneousMatrix( B_INV );
396  this->getHomogeneousMatrix(HM);
398  RES.multiply(B_INV,HM);
399  return CPose3D( RES );
400 }
401 
402 
404 {
405  s = mrpt::format("[%f %f %fdeg]",x(),y(),mrpt::utils::RAD2DEG(m_phi));
406 }
407 
409 {
410  for (int i=0;i<3;i++)
411  (*this)[i] = std::numeric_limits<double>::quiet_NaN();
412 }
const GLdouble * v
Definition: glew.h:1296
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
std::string asString() const
Definition: CPose2D.h:192
double y
X,Y coordinates.
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
Definition: CPose2D.cpp:59
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPose2D.cpp:272
void setToNaN() MRPT_OVERRIDE
Set all data fields to quiet NaN.
Definition: CPose2D.cpp:408
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
GLboolean GLboolean g
Definition: glew.h:5406
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:39
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
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.
Definition: CPose2D.cpp:359
#define THROW_EXCEPTION(msg)
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
void operator*=(const double s)
Scalar multiplication.
Definition: CPose2D.cpp:259
void AddComponents(const CPose2D &p)
Scalar sum of components: This is diferent from poses composition, which is implemented as "+" operat...
Definition: CPose2D.cpp:247
mrpt::math::CArrayDouble< 2 > m_coords
[x,y]
Definition: CPose2D.h:43
double z
X,Y,Z coordinates.
CPose2D operator-(const CPose2D &b) const
Compute .
Definition: CPose2D.h:161
void getAsVector(mrpt::math::CVectorDouble &v) const
Returns a 1x3 vector with [x y phi].
Definition: CPose2D.cpp:331
double phi_cos() const
Get a (cached) value of cos(phi), recomputing it only once when phi changes.
Definition: CPose2D.h:89
GLuint in
Definition: glew.h:7146
GLdouble l
Definition: glew.h:5092
double m_sinphi
Precomputed cos() & sin() of phi.
Definition: CPose2D.h:47
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
void getInverseHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 inverse homogeneous transformation matrix for this point or pose...
Definition: CPoseOrPoint.h:201
GLdouble s
Definition: glew.h:1295
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
A numeric matrix of compile-time fixed size.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
double RAD2DEG(const double x)
Radians to degrees.
mrpt::math::CMatrixDouble22 getRotationMatrix() const
Definition: CPose2D.h:111
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
bool operator!=(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
Definition: CPoint.h:138
double distance2DFrobeniusTo(const CPose2D &p) const
Returns the 2D distance from this pose/point to a 2D pose using the Frobenius distance.
Definition: CPose2D.cpp:386
double m_phi
The orientation of the pose, in radians.
Definition: CPose2D.h:46
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...
Definition: CPose2D.cpp:307
void update_cached_cos_sin() const
Definition: CPose2D.h:50
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
int version
Definition: mrpt_jpeglib.h:898
void inverse()
Convert this pose into its inverse, saving the result in itself.
Definition: CPose2D.cpp:315
#define DEG2RAD
GLfloat GLfloat p
Definition: glew.h:10113
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:51
A class used to store a 2D point.
Definition: CPoint2D.h:36
A class used to store a 3D point.
Definition: CPoint3D.h:32
double y
X,Y coordinates.
#define RAD2DEG
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...
Definition: CPose2D.cpp:178
GLsizei const GLcharARB ** string
Definition: glew.h:3293
size_t size(const MATRIXLIKE &m, const int dim)
bool operator==(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
Definition: CPoint.h:130
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
CPose2D & operator+=(const CPose2D &b)
Make .
Definition: CPose2D.cpp:370
CPose2D operator+(const CPose2D &D) const
The operator is the pose compounding operator.
Definition: CPose2D.cpp:109
void composeFrom(const CPose2D &A, const CPose2D &B)
Makes .
Definition: CPose2D.cpp:122
double phi_sin() const
Get a (cached) value of sin(phi), recomputing it only once when phi changes.
Definition: CPose2D.h:91
void fromString(const std::string &s)
Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0...
Definition: CPose2D.cpp:376
Lightweight 2D pose.
void inverseComposePoint(const double gx, const double gy, double &lx, double &ly) const
Computes the 2D point L such as .
Definition: CPose2D.cpp:205
const GLdouble * m
Definition: glew.h:5094
GLdouble GLdouble GLdouble b
Definition: glew.h:5092
void readFromStream(mrpt::utils::CStream &in, int version)
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
Definition: CPose2D.cpp:74
Lightweight 3D point.
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...
Definition: CPose2D.cpp:233
bool m_cossin_uptodate
Definition: CPose2D.h:48
Lightweight 2D point.
#define ASSERTMSG_(f, __ERROR_MSG)
CPose2D()
Default constructor (all coordinates to 0)
Definition: CPose2D.cpp:30
void normalizePhi()
Forces "phi" to be in the range [-pi,pi];.
Definition: CPose2D.cpp:287
std::ostream & operator<<(std::ostream &o, const CPoint< DERIVEDCLASS > &p)
Dumps a point as a string [x,y] or [x,y,z].
Definition: CPoint.h:106



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018