MRPT  1.9.9
CPose2D.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "poses-precomp.h" // Precompiled headers
11 
12 #include <mrpt/config.h> // HAVE_SINCOS
13 #include <mrpt/math/wrap2pi.h>
14 #include <mrpt/poses/CPoint2D.h>
15 #include <mrpt/poses/CPoint3D.h>
16 #include <mrpt/poses/CPose2D.h>
17 #include <mrpt/poses/CPose3D.h>
20 #include <limits>
21 
22 using namespace mrpt;
23 using namespace mrpt::math;
24 using namespace mrpt::poses;
25 
27 
28 /*---------------------------------------------------------------
29  Constructors
30  ---------------------------------------------------------------*/
31 CPose2D::CPose2D() { m_coords[0] = m_coords[1] = 0; }
32 CPose2D::CPose2D(const double x, const double y, const double _phi)
33  : m_phi(_phi), m_cossin_uptodate(false)
34 {
35  m_coords[0] = x;
36  m_coords[1] = y;
37  normalizePhi();
38 }
39 
40 CPose2D::CPose2D(const CPoint2D& p) : m_phi(0), m_cossin_uptodate(false)
41 {
42  m_coords[0] = p.x();
43  m_coords[1] = p.y();
44 }
45 
46 CPose2D::CPose2D(const CPose3D& p) : m_phi(p.yaw()), m_cossin_uptodate(false)
47 {
48  m_coords[0] = p.x();
49  m_coords[1] = p.y();
50 }
51 
54 {
55  // The coordinates:
56  out << m_coords[0] << m_coords[1] << m_phi;
57 }
59 {
60  switch (version)
61  {
62  case 0:
63  {
64  // The coordinates:
65  float x0, y0, phi0;
66  in >> x0 >> y0 >> phi0;
67  m_coords[0] = x0;
68  m_coords[1] = y0;
69  this->phi(phi0);
70  }
71  break;
72  case 1:
73  {
74  // The coordinates:
75  in >> m_coords[0] >> m_coords[1] >> m_phi;
76  m_cossin_uptodate = false;
77  }
78  break;
79  default:
81  };
82 }
83 
85 {
87  out["x"] = m_coords[0];
88  out["y"] = m_coords[1];
89  out["phi"] = m_phi;
90 }
92 {
93  uint8_t version;
95  switch (version)
96  {
97  case 1:
98  {
99  m_coords[0] = static_cast<double>(in["x"]);
100  m_coords[1] = static_cast<double>(in["y"]);
101  m_phi = static_cast<double>(in["phi"]);
102  }
103  break;
104  default:
106  }
107 }
108 
109 /** Textual output stream function.
110  */
111 std::ostream& mrpt::poses::operator<<(std::ostream& o, const CPose2D& p)
112 {
113  o << format("(%.03f,%.03f,%.02fdeg)", p.x(), p.y(), RAD2DEG(p.phi()));
114  return o;
115 }
116 
117 /*---------------------------------------------------------------
118 The operator a="this"+D is the pose compounding operator.
119  ---------------------------------------------------------------*/
121 {
123 
124  return CPose2D(
125  m_coords[0] + D.m_coords[0] * m_cosphi - D.m_coords[1] * m_sinphi,
126  m_coords[1] + D.m_coords[0] * m_sinphi + D.m_coords[1] * m_cosphi,
127  m_phi + D.m_phi);
128 }
129 
130 /*---------------------------------------------------------------
131  composeFrom
132  ---------------------------------------------------------------*/
133 void CPose2D::composeFrom(const CPose2D& A, const CPose2D& B)
134 {
135  A.update_cached_cos_sin();
136 
137  // Use temporary variables for the cases (A==this) or (B==this)
138  const double new_x =
139  A.m_coords[0] + B.m_coords[0] * A.m_cosphi - B.m_coords[1] * A.m_sinphi;
140  const double new_y =
141  A.m_coords[1] + B.m_coords[0] * A.m_sinphi + B.m_coords[1] * A.m_cosphi;
142  m_coords[0] = new_x;
143  m_coords[1] = new_y;
144 
145  m_phi = math::wrapToPi(A.m_phi + B.m_phi);
146  m_cossin_uptodate = false;
147 }
148 
149 /*---------------------------------------------------------------
150  getRotationMatrix
151  ---------------------------------------------------------------*/
153 {
155  R(0, 0) = m_cosphi;
156  R(0, 1) = -m_sinphi;
157  R(1, 0) = m_sinphi;
158  R(1, 1) = m_cosphi;
159 }
160 
162 {
164  R(0, 0) = m_cosphi;
165  R(0, 1) = -m_sinphi;
166  R(0, 2) = 0;
167  R(1, 0) = m_sinphi;
168  R(1, 1) = m_cosphi;
169  R(1, 2) = 0;
170  R(2, 0) = 0;
171  R(2, 1) = 0;
172  R(2, 2) = 1;
173 }
174 
175 /*---------------------------------------------------------------
176 The operator a="this"+D is the pose compounding operator.
177  ---------------------------------------------------------------*/
179 {
180  return CPose3D(*this) + D;
181 }
182 
183 /*---------------------------------------------------------------
184 The operator u'="this"+u is the pose/point compounding operator.
185  ---------------------------------------------------------------*/
187 {
189 
190  return CPoint2D(
191  m_coords[0] + u.x() * m_cosphi - u.y() * m_sinphi,
192  m_coords[1] + u.x() * m_sinphi + u.y() * m_cosphi);
193 }
194 
195 /** An alternative, slightly more efficient way of doing \f$ G = P \oplus L \f$
196  * with G and L being 2D points and P this 2D pose. */
197 void CPose2D::composePoint(double lx, double ly, double& gx, double& gy) const
198 {
200 
201  gx = m_coords[0] + lx * m_cosphi - ly * m_sinphi;
202  gy = m_coords[1] + lx * m_sinphi + ly * m_cosphi;
203 }
204 
207 {
208  this->composePoint(l.x, l.y, g.x, g.y);
209 }
210 
211 /** overload \f$ G = P \oplus L \f$ with G and L being 3D points and P this 2D
212  * pose (the "z" coordinate remains unmodified) */
215 {
216  this->composePoint(l.x, l.y, l.z, g.x, g.y, g.z);
217 }
218 
220  double lx, double ly, double lz, double& gx, double& gy, double& gz) const
221 {
223  gx = m_coords[0] + lx * m_cosphi - ly * m_sinphi;
224  gy = m_coords[1] + lx * m_sinphi + ly * m_cosphi;
225  gz = lz;
226 }
227 
229  const double gx, const double gy, double& lx, double& ly) const
230 {
232 
233  const double Ax = gx - m_coords[0];
234  const double Ay = gy - m_coords[1];
235 
236  lx = Ax * m_cosphi + Ay * m_sinphi;
237  ly = -Ax * m_sinphi + Ay * m_cosphi;
238 }
239 
240 /*---------------------------------------------------------------
241 The operator u'="this"+u is the pose/point compounding operator.
242  ---------------------------------------------------------------*/
244 {
246 
247  return CPoint3D(
248  m_coords[0] + u.x() * m_cosphi - u.y() * m_sinphi,
249  m_coords[1] + u.x() * m_sinphi + u.y() * m_cosphi, u.z());
250 }
251 
252 /*---------------------------------------------------------------
253 The operator D="this"-b is the pose inverse compounding operator.
254  The resulting pose "D" is the diference between this pose and "b"
255  ---------------------------------------------------------------*/
257 {
259 
260  m_coords[0] = (A.m_coords[0] - B.m_coords[0]) * B.m_cosphi +
261  (A.m_coords[1] - B.m_coords[1]) * B.m_sinphi;
262  m_coords[1] = -(A.m_coords[0] - B.m_coords[0]) * B.m_sinphi +
263  (A.m_coords[1] - B.m_coords[1]) * B.m_cosphi;
264  m_phi = math::wrapToPi(A.m_phi - B.m_phi);
265  m_cossin_uptodate = false;
266 }
267 
268 /*---------------------------------------------------------------
269  Scalar sum of components: This is diferent from poses
270  composition, which is implemented as "+" operators in "CPose" derived
271  classes.
272  ---------------------------------------------------------------*/
274 {
275  m_coords[0] += p.m_coords[0];
276  m_coords[1] += p.m_coords[1];
277  m_phi += p.m_phi;
278  m_cossin_uptodate = false;
279 }
280 
281 /*---------------------------------------------------------------
282  Scalar multiplication.
283  ---------------------------------------------------------------*/
284 void CPose2D::operator*=(const double s)
285 {
286  m_coords[0] *= s;
287  m_coords[1] *= s;
288  m_phi *= s;
289  m_cossin_uptodate = false;
290 }
291 
292 /*---------------------------------------------------------------
293  Returns the corresponding 4x4 homogeneous
294  transformation matrix for the point(translation),
295  or pose (translation+orientation).
296 ---------------------------------------------------------------*/
298 {
299  m.unit(4, 1.0);
300 
301  m.set_unsafe(0, 3, m_coords[0]);
302  m.set_unsafe(1, 3, m_coords[1]);
303 
305 
306  m.get_unsafe(0, 0) = m_cosphi;
307  m.get_unsafe(0, 1) = -m_sinphi;
308  m.get_unsafe(1, 0) = m_sinphi;
309  m.get_unsafe(1, 1) = m_cosphi;
310 }
311 
312 /** Forces "phi" to be in the range [-pi,pi];
313  */
315 {
317  m_cossin_uptodate = false;
318 }
319 
321  : m_phi(o.phi), m_cossin_uptodate(false)
322 {
323  m_coords[0] = o.x;
324  m_coords[1] = o.y;
325 }
326 
327 CPose2D::CPose2D(const CPoint3D& o) : m_phi(0), m_cossin_uptodate(false)
328 {
329  this->m_coords[0] = o.x();
330  this->m_coords[1] = o.y();
331  normalizePhi();
332 }
333 
334 /*---------------------------------------------------------------
335  unary -
336 ---------------------------------------------------------------*/
338 {
339  CPose2D ret = b;
340  ret.inverse();
341  return ret;
342 }
343 
344 /** Convert this pose into its inverse, saving the result in itself. \sa
345  * operator- */
347 {
349  const double x = m_coords[0];
350  const double y = m_coords[1];
351 
352  m_coords[0] = -x * m_cosphi - y * m_sinphi;
353  m_coords[1] = x * m_sinphi - y * m_cosphi;
354  m_phi = -m_phi;
355  m_cossin_uptodate = false;
356 }
357 
358 /*---------------------------------------------------------------
359  getAsVector
360 ---------------------------------------------------------------*/
362 {
363  v.resize(3);
364  v[0] = m_coords[0];
365  v[1] = m_coords[1];
366  v[2] = m_phi;
367 }
368 
370 {
371  v[0] = m_coords[0];
372  v[1] = m_coords[1];
373  v[2] = m_phi;
374 }
375 
376 bool mrpt::poses::operator==(const CPose2D& p1, const CPose2D& p2)
377 {
378  return (p1.x() == p2.x()) && (p1.y() == p2.y()) && (p1.phi() == p2.phi());
379 }
380 
381 bool mrpt::poses::operator!=(const CPose2D& p1, const CPose2D& p2)
382 {
383  return (p1.x() != p2.x()) || (p1.y() != p2.y()) || (p1.phi() != p2.phi());
384 }
385 
387 {
388  const double ccos = pose.phi_cos();
389  const double ssin = pose.phi_sin();
390  return TPoint2D(
391  pose.x() + u.x * ccos - u.y * ssin, pose.y() + u.x * ssin + u.y * ccos);
392 }
393 
394 /** Make \f$ this = this \oplus b \f$ */
396 {
397  composeFrom(*this, b);
398  return *this;
399 }
400 
402 {
403  CMatrixDouble m;
404  if (!m.fromMatlabStringFormat(s))
405  THROW_EXCEPTION("Malformed expression in ::fromString");
406  ASSERTMSG_(m.rows() == 1 && m.cols() == 3, "Expected vector length=3");
407  x(m.get_unsafe(0, 0));
408  y(m.get_unsafe(0, 1));
409  phi(DEG2RAD(m.get_unsafe(0, 2)));
410 }
411 
413 {
414  this->fromString("[" + s + "]");
415 }
416 
418 {
419  return std::sqrt(
420  square(p.x() - x()) + square(p.y() - y()) +
421  4 * (1 - cos(p.phi() - phi())));
422 }
423 
425 {
427  b.getInverseHomogeneousMatrix(B_INV);
429  this->getHomogeneousMatrix(HM);
431  RES.multiply(B_INV, HM);
432  return CPose3D(RES);
433 }
434 
436 {
437  return CPose2D(-m_coords[0], -m_coords[1], -m_phi);
438 }
439 
441 {
442  s = mrpt::format("[%f %f %fdeg]", x(), y(), mrpt::RAD2DEG(m_phi));
443 }
444 
446 {
447  for (int i = 0; i < 3; i++)
448  (*this)[i] = std::numeric_limits<double>::quiet_NaN();
449 }
450 
452 {
453  if (m_cossin_uptodate) return;
454 #ifdef HAVE_SINCOS
455  ::sincos(m_phi, &m_sinphi, &m_cosphi);
456 #else
457  m_cosphi = ::cos(m_phi);
458  m_sinphi = ::sin(m_phi);
459 #endif
460  m_cossin_uptodate = true;
461 }
462 
464 {
465  return mrpt::math::TPose2D(x(), y(), phi());
466 }
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
double RAD2DEG(const double x)
Radians to degrees.
double phi_sin() const
Get a (cached) value of sin(phi), recomputing it only once when phi changes.
Definition: CPose2D.h:101
double x
X,Y coordinates.
double x
X,Y coordinates.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:108
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
mrpt::math::TPoint2D 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:386
double DEG2RAD(const double x)
Degrees to radians.
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Definition: CPose2D.cpp:58
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...
Definition: CArrayNumeric.h:25
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:45
void operator*=(const double s)
Scalar multiplication.
Definition: CPose2D.cpp:284
void AddComponents(const CPose2D &p)
Scalar sum of components: This is diferent from poses composition, which is implemented as "+" operat...
Definition: CPose2D.cpp:273
mrpt::math::CArrayDouble< 2 > m_coords
[x,y]
Definition: CPose2D.h:46
double m_cosphi
Precomputed cos() & sin() of phi.
Definition: CPose2D.h:52
GLdouble s
Definition: glext.h:3682
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:197
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: CPose2D.cpp:52
mrpt::math::TPose2D asTPose() const
Definition: CPose2D.cpp:463
unsigned char uint8_t
Definition: rptypes.h:44
Virtual base class for "schematic archives" (JSON, XML,...)
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:138
T square(const T x)
Inline function for the square of a number.
CPose2D operator-(const CPose2D &b) const
Compute .
Definition: CPose2D.h:207
void fromStringRaw(const std::string &s)
Same as fromString, but without requiring the square brackets in the string.
Definition: CPose2D.cpp:412
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
bool operator!=(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
Definition: CPoint.h:171
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CPose2D.cpp:53
double m_phi
The orientation of the pose, in radians.
Definition: CPose2D.h:50
CPose2D 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:337
void setToNaN() override
Set all data fields to quiet NaN.
Definition: CPose2D.cpp:445
GLubyte g
Definition: glext.h:6372
GLubyte GLubyte b
Definition: glext.h:6372
double x
X,Y,Z coordinates.
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:149
void inverse()
Convert this pose into its inverse, saving the result in itself.
Definition: CPose2D.cpp:346
CPose2D operator+(const CPose2D &D) const
The operator is the pose compounding operator.
Definition: CPose2D.cpp:120
GLsizei const GLchar ** string
Definition: glext.h:4116
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:50
A class used to store a 2D point.
Definition: CPoint2D.h:32
A class used to store a 3D point.
Definition: CPoint3D.h:30
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
#define SCHEMA_DESERIALIZE_DATATYPE_VERSION()
For use inside serializeFrom(CSchemeArchiveBase) methods.
MATRIX22 getRotationMatrix() const
Definition: CPose2D.h:139
std::string asString() const
Definition: CPose2D.h:245
const GLdouble * v
Definition: glext.h:3684
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:52
bool operator==(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
Definition: CPoint.h:163
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:38
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:82
CPose2D & operator+=(const CPose2D &b)
Make .
Definition: CPose2D.cpp:395
CPose2D getOppositeScalar() const
Return the opposite of the current pose instance by taking the negative of all its components individ...
Definition: CPose2D.cpp:435
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
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:297
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
void composeFrom(const CPose2D &A, const CPose2D &B)
Makes .
Definition: CPose2D.cpp:133
double phi_cos() const
Get a (cached) value of cos(phi), recomputing it only once when phi changes.
Definition: CPose2D.h:94
void inverseComposePoint(const double gx, const double gy, double &lx, double &ly) const
Computes the 2D point L such as .
Definition: CPose2D.cpp:228
GLuint in
Definition: glext.h:7391
void fromString(const std::string &s)
Set the current object value from a string generated by &#39;asString&#39; (eg: "[0.02 1.04 -0...
Definition: CPose2D.cpp:401
Lightweight 2D pose.
GLenum GLint GLint y
Definition: glext.h:3542
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:417
#define SCHEMA_SERIALIZE_DATATYPE_VERSION(ser_version)
For use inside all serializeTo(CSchemeArchiveBase) methods.
GLenum GLint x
Definition: glext.h:3542
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:256
bool m_cossin_uptodate
Definition: CPose2D.h:53
void getAsVector(mrpt::math::CVectorDouble &v) const
Returns a 1x3 vector with [x y phi].
Definition: CPose2D.cpp:361
Lightweight 2D point.
GLfloat GLfloat p
Definition: glext.h:6398
CPose2D()
Default constructor (all coordinates to 0)
Definition: CPose2D.cpp:31
void normalizePhi()
Forces "phi" to be in the range [-pi,pi];.
Definition: CPose2D.cpp:314
void update_cached_cos_sin() const
Definition: CPose2D.cpp:451
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:137



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 5887d2b31 Wed Apr 24 13:03:27 2019 +0200 at miƩ abr 24 13:10:13 CEST 2019