Main MRPT website > C++ reference for MRPT 1.9.9
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-2018, 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 "poses-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>
17 #include <mrpt/math/wrap2pi.h>
18 #include <mrpt/config.h> // HAVE_SINCOS
19 #include <limits>
20 
21 using namespace mrpt;
22 using namespace mrpt::math;
23 using namespace mrpt::poses;
24 
26 
27 /*---------------------------------------------------------------
28  Constructors
29  ---------------------------------------------------------------*/
30 CPose2D::CPose2D() : m_phi(0), m_cossin_uptodate(false)
31 {
32  m_coords[0] = m_coords[1] = 0;
33 }
34 
35 CPose2D::CPose2D(const double x, const double y, const double _phi)
36  : 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 
57 {
58  // The coordinates:
59  out << m_coords[0] << m_coords[1] << m_phi;
60 }
62 {
63  switch (version)
64  {
65  case 0:
66  {
67  // The coordinates:
68  float x0, y0, phi0;
69  in >> x0 >> y0 >> phi0;
70  m_coords[0] = x0;
71  m_coords[1] = y0;
72  this->phi(phi0);
73  }
74  break;
75  case 1:
76  {
77  // The coordinates:
78  in >> m_coords[0] >> m_coords[1] >> m_phi;
79  m_cossin_uptodate = false;
80  }
81  break;
82  default:
84  };
85 }
86 
87 /** Textual output stream function.
88  */
89 std::ostream& mrpt::poses::operator<<(std::ostream& o, const CPose2D& p)
90 {
91  o << format("(%.03f,%.03f,%.02fdeg)", p.x(), p.y(), RAD2DEG(p.phi()));
92  return o;
93 }
94 
95 /*---------------------------------------------------------------
96 The operator a="this"+D is the pose compounding operator.
97  ---------------------------------------------------------------*/
99 {
101 
102  return CPose2D(
103  m_coords[0] + D.m_coords[0] * m_cosphi - D.m_coords[1] * m_sinphi,
104  m_coords[1] + D.m_coords[0] * m_sinphi + D.m_coords[1] * m_cosphi,
105  m_phi + D.m_phi);
106 }
107 
108 /*---------------------------------------------------------------
109  composeFrom
110  ---------------------------------------------------------------*/
111 void CPose2D::composeFrom(const CPose2D& A, const CPose2D& B)
112 {
114 
115  // Use temporary variables for the cases (A==this) or (B==this)
116  const double new_x =
117  A.m_coords[0] + B.m_coords[0] * A.m_cosphi - B.m_coords[1] * A.m_sinphi;
118  const double new_y =
119  A.m_coords[1] + B.m_coords[0] * A.m_sinphi + B.m_coords[1] * A.m_cosphi;
120  m_coords[0] = new_x;
121  m_coords[1] = new_y;
122 
123  m_phi = math::wrapToPi(A.m_phi + B.m_phi);
124  m_cossin_uptodate = false;
125 }
126 
127 /*---------------------------------------------------------------
128  getRotationMatrix
129  ---------------------------------------------------------------*/
131 {
133  R(0, 0) = m_cosphi;
134  R(0, 1) = -m_sinphi;
135  R(1, 0) = m_sinphi;
136  R(1, 1) = m_cosphi;
137 }
138 
140 {
142  R(0, 0) = m_cosphi;
143  R(0, 1) = -m_sinphi;
144  R(0, 2) = 0;
145  R(1, 0) = m_sinphi;
146  R(1, 1) = m_cosphi;
147  R(1, 2) = 0;
148  R(2, 0) = 0;
149  R(2, 1) = 0;
150  R(2, 2) = 1;
151 }
152 
153 /*---------------------------------------------------------------
154 The operator a="this"+D is the pose compounding operator.
155  ---------------------------------------------------------------*/
157 {
158  return CPose3D(*this) + D;
159 }
160 
161 /*---------------------------------------------------------------
162 The operator u'="this"+u is the pose/point compounding operator.
163  ---------------------------------------------------------------*/
165 {
167 
168  return CPoint2D(
169  m_coords[0] + u.x() * m_cosphi - u.y() * m_sinphi,
170  m_coords[1] + u.x() * m_sinphi + u.y() * m_cosphi);
171 }
172 
173 /** An alternative, slightly more efficient way of doing \f$ G = P \oplus L \f$
174  * with G and L being 2D points and P this 2D pose. */
175 void CPose2D::composePoint(double lx, double ly, double& gx, double& gy) const
176 {
178 
179  gx = m_coords[0] + lx * m_cosphi - ly * m_sinphi;
180  gy = m_coords[1] + lx * m_sinphi + ly * m_cosphi;
181 }
182 
185 {
186  this->composePoint(l.x, l.y, g.x, g.y);
187 }
188 
189 /** overload \f$ G = P \oplus L \f$ with G and L being 3D points and P this 2D
190  * pose (the "z" coordinate remains unmodified) */
193 {
194  this->composePoint(l.x, l.y, l.z, g.x, g.y, g.z);
195 }
196 
198  double lx, double ly, double lz, double& gx, double& gy, double& gz) const
199 {
201  gx = m_coords[0] + lx * m_cosphi - ly * m_sinphi;
202  gy = m_coords[1] + lx * m_sinphi + ly * m_cosphi;
203  gz = lz;
204 }
205 
207  const double gx, const double gy, double& lx, double& ly) const
208 {
210 
211  const double Ax = gx - m_coords[0];
212  const double Ay = gy - m_coords[1];
213 
214  lx = Ax * m_cosphi + Ay * m_sinphi;
215  ly = -Ax * m_sinphi + Ay * m_cosphi;
216 }
217 
218 /*---------------------------------------------------------------
219 The operator u'="this"+u is the pose/point compounding operator.
220  ---------------------------------------------------------------*/
222 {
224 
225  return CPoint3D(
226  m_coords[0] + u.x() * m_cosphi - u.y() * m_sinphi,
227  m_coords[1] + u.x() * m_sinphi + u.y() * m_cosphi, u.z());
228 }
229 
230 /*---------------------------------------------------------------
231 The operator D="this"-b is the pose inverse compounding operator.
232  The resulting pose "D" is the diference between this pose and "b"
233  ---------------------------------------------------------------*/
235 {
237 
238  m_coords[0] = (A.m_coords[0] - B.m_coords[0]) * B.m_cosphi +
239  (A.m_coords[1] - B.m_coords[1]) * B.m_sinphi;
240  m_coords[1] = -(A.m_coords[0] - B.m_coords[0]) * B.m_sinphi +
241  (A.m_coords[1] - B.m_coords[1]) * B.m_cosphi;
242  m_phi = math::wrapToPi(A.m_phi - B.m_phi);
243  m_cossin_uptodate = false;
244 }
245 
246 /*---------------------------------------------------------------
247  Scalar sum of components: This is diferent from poses
248  composition, which is implemented as "+" operators in "CPose" derived
249  classes.
250  ---------------------------------------------------------------*/
252 {
253  m_coords[0] += p.m_coords[0];
254  m_coords[1] += p.m_coords[1];
255  m_phi += p.m_phi;
256  m_cossin_uptodate = false;
257 }
258 
259 /*---------------------------------------------------------------
260  Scalar multiplication.
261  ---------------------------------------------------------------*/
262 void CPose2D::operator*=(const double s)
263 {
264  m_coords[0] *= s;
265  m_coords[1] *= s;
266  m_phi *= s;
267  m_cossin_uptodate = false;
268 }
269 
270 /*---------------------------------------------------------------
271  Returns the corresponding 4x4 homogeneous
272  transformation matrix for the point(translation),
273  or pose (translation+orientation).
274 ---------------------------------------------------------------*/
276 {
277  m.unit(4, 1.0);
278 
279  m.set_unsafe(0, 3, m_coords[0]);
280  m.set_unsafe(1, 3, m_coords[1]);
281 
283 
284  m.get_unsafe(0, 0) = m_cosphi;
285  m.get_unsafe(0, 1) = -m_sinphi;
286  m.get_unsafe(1, 0) = m_sinphi;
287  m.get_unsafe(1, 1) = m_cosphi;
288 }
289 
290 /** Forces "phi" to be in the range [-pi,pi];
291 */
293 {
295  m_cossin_uptodate = false;
296 }
297 
299  : m_phi(o.phi), m_cossin_uptodate(false)
300 {
301  m_coords[0] = o.x;
302  m_coords[1] = o.y;
303 }
304 
305 CPose2D::CPose2D(const CPoint3D& o) : m_phi(0), m_cossin_uptodate(false)
306 {
307  this->m_coords[0] = o.x();
308  this->m_coords[1] = o.y();
309  normalizePhi();
310 }
311 
312 /*---------------------------------------------------------------
313  unary -
314 ---------------------------------------------------------------*/
316 {
317  CPose2D ret = b;
318  ret.inverse();
319  return ret;
320 }
321 
322 /** Convert this pose into its inverse, saving the result in itself. \sa
323  * operator- */
325 {
327  const double x = m_coords[0];
328  const double y = m_coords[1];
329 
330  m_coords[0] = -x * m_cosphi - y * m_sinphi;
331  m_coords[1] = x * m_sinphi - y * m_cosphi;
332  m_phi = -m_phi;
333  m_cossin_uptodate = false;
334 }
335 
336 /*---------------------------------------------------------------
337  getAsVector
338 ---------------------------------------------------------------*/
340 {
341  v.resize(3);
342  v[0] = m_coords[0];
343  v[1] = m_coords[1];
344  v[2] = m_phi;
345 }
346 
348 {
349  v[0] = m_coords[0];
350  v[1] = m_coords[1];
351  v[2] = m_phi;
352 }
353 
354 bool mrpt::poses::operator==(const CPose2D& p1, const CPose2D& p2)
355 {
356  return (p1.x() == p2.x()) && (p1.y() == p2.y()) && (p1.phi() == p2.phi());
357 }
358 
359 bool mrpt::poses::operator!=(const CPose2D& p1, const CPose2D& p2)
360 {
361  return (p1.x() != p2.x()) || (p1.y() != p2.y()) || (p1.phi() != p2.phi());
362 }
363 
365 {
366  const double ccos = pose.phi_cos();
367  const double ssin = pose.phi_sin();
368  return TPoint2D(
369  pose.x() + u.x * ccos - u.y * ssin, pose.y() + u.x * ssin + u.y * ccos);
370 }
371 
372 /** Make \f$ this = this \oplus b \f$ */
374 {
375  composeFrom(*this, b);
376  return *this;
377 }
378 
380 {
381  CMatrixDouble m;
382  if (!m.fromMatlabStringFormat(s))
383  THROW_EXCEPTION("Malformed expression in ::fromString");
384  ASSERTMSG_(m.rows() == 1 && m.cols() == 3, "Expected vector length=3");
385  x(m.get_unsafe(0, 0));
386  y(m.get_unsafe(0, 1));
387  phi(DEG2RAD(m.get_unsafe(0, 2)));
388 }
389 
391 {
392  this->fromString("[" + s + "]");
393 }
394 
396 {
397  return std::sqrt(
398  square(p.x() - x()) + square(p.y() - y()) +
399  4 * (1 - cos(p.phi() - phi())));
400 }
401 
403 {
405  b.getInverseHomogeneousMatrix(B_INV);
407  this->getHomogeneousMatrix(HM);
409  RES.multiply(B_INV, HM);
410  return CPose3D(RES);
411 }
412 
414 {
415  return CPose2D(-m_coords[0], -m_coords[1], -m_phi);
416 }
417 
419 {
420  s = mrpt::format("[%f %f %fdeg]", x(), y(), mrpt::RAD2DEG(m_phi));
421 }
422 
424 {
425  for (int i = 0; i < 3; i++)
426  (*this)[i] = std::numeric_limits<double>::quiet_NaN();
427 }
428 
430 {
431  if (m_cossin_uptodate) return;
432 #ifdef HAVE_SINCOS
433  ::sincos(m_phi, &m_sinphi, &m_cosphi);
434 #else
435  m_cosphi = ::cos(m_phi);
436  m_sinphi = ::sin(m_phi);
437 #endif
438  m_cossin_uptodate = true;
439 }
440 
442 {
443  return mrpt::math::TPose2D(x(), y(), phi());
444 }
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:99
double x
X,Y coordinates.
double x
X,Y coordinates.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
#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:364
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:61
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:44
void operator*=(const double s)
Scalar multiplication.
Definition: CPose2D.cpp:262
void AddComponents(const CPose2D &p)
Scalar sum of components: This is diferent from poses composition, which is implemented as "+" operat...
Definition: CPose2D.cpp:251
mrpt::math::CArrayDouble< 2 > m_coords
[x,y]
Definition: CPose2D.h:47
double m_cosphi
Precomputed cos() & sin() of phi.
Definition: CPose2D.h:53
GLdouble s
Definition: glext.h:3676
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:175
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: CPose2D.cpp:55
mrpt::math::TPose2D asTPose() const
Definition: CPose2D.cpp:441
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
T square(const T x)
Inline function for the square of a number.
CPose2D operator-(const CPose2D &b) const
Compute .
Definition: CPose2D.h:205
void fromStringRaw(const std::string &s)
Same as fromString, but without requiring the square brackets in the string.
Definition: CPose2D.cpp:390
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:174
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CPose2D.cpp:56
double m_phi
The orientation of the pose, in radians.
Definition: CPose2D.h:51
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:315
void setToNaN() override
Set all data fields to quiet NaN.
Definition: CPose2D.cpp:423
GLubyte g
Definition: glext.h:6279
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
GLubyte GLubyte b
Definition: glext.h:6279
double x
X,Y,Z coordinates.
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:101
void inverse()
Convert this pose into its inverse, saving the result in itself.
Definition: CPose2D.cpp:324
CPose2D operator+(const CPose2D &D) const
The operator is the pose compounding operator.
Definition: CPose2D.cpp:98
GLsizei const GLchar ** string
Definition: glext.h:4101
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:53
A class used to store a 2D point.
Definition: CPoint2D.h:35
A class used to store a 3D point.
Definition: CPoint3D.h:33
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
MATRIX22 getRotationMatrix() const
Definition: CPose2D.h:137
std::string asString() const
Definition: CPose2D.h:243
const GLdouble * v
Definition: glext.h:3678
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:48
bool operator==(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
Definition: CPoint.h:166
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:40
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
CPose2D & operator+=(const CPose2D &b)
Make .
Definition: CPose2D.cpp:373
CPose2D getOppositeScalar() const
Return the opposite of the current pose instance by taking the negative of all its components individ...
Definition: CPose2D.cpp:413
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:275
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:82
void composeFrom(const CPose2D &A, const CPose2D &B)
Makes .
Definition: CPose2D.cpp:111
double phi_cos() const
Get a (cached) value of cos(phi), recomputing it only once when phi changes.
Definition: CPose2D.h:92
void inverseComposePoint(const double gx, const double gy, double &lx, double &ly) const
Computes the 2D point L such as .
Definition: CPose2D.cpp:206
GLuint in
Definition: glext.h:7274
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:379
Lightweight 2D pose.
GLenum GLint GLint y
Definition: glext.h:3538
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:395
GLenum GLint x
Definition: glext.h:3538
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:234
bool m_cossin_uptodate
Definition: CPose2D.h:54
void getAsVector(mrpt::math::CVectorDouble &v) const
Returns a 1x3 vector with [x y phi].
Definition: CPose2D.cpp:339
Lightweight 2D point.
GLfloat GLfloat p
Definition: glext.h:6305
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:292
void update_cached_cos_sin() const
Definition: CPose2D.cpp:429
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:140



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019