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-2020, 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/TPose2D.h>
14 #include <mrpt/math/wrap2pi.h>
15 #include <mrpt/poses/CPoint2D.h>
16 #include <mrpt/poses/CPoint3D.h>
17 #include <mrpt/poses/CPose2D.h>
18 #include <mrpt/poses/CPose3D.h>
21 #include <Eigen/Dense>
22 #include <limits>
23 
24 using namespace mrpt;
25 using namespace mrpt::math;
26 using namespace mrpt::poses;
27 
29 
30 /*---------------------------------------------------------------
31  Constructors
32  ---------------------------------------------------------------*/
33 CPose2D::CPose2D() { m_coords[0] = m_coords[1] = 0; }
34 CPose2D::CPose2D(const double x, const double y, const double _phi)
35  : m_phi(_phi), m_cossin_uptodate(false)
36 {
37  m_coords[0] = x;
38  m_coords[1] = y;
39  normalizePhi();
40 }
41 
42 CPose2D::CPose2D(const CPoint2D& p) : m_phi(0), m_cossin_uptodate(false)
43 {
44  m_coords[0] = p.x();
45  m_coords[1] = p.y();
46 }
47 
48 CPose2D::CPose2D(const CPose3D& p) : m_phi(p.yaw()), m_cossin_uptodate(false)
49 {
50  m_coords[0] = p.x();
51  m_coords[1] = p.y();
52 }
53 
54 uint8_t CPose2D::serializeGetVersion() const { return 1; }
56 {
57  // The coordinates:
58  out << m_coords[0] << m_coords[1] << m_phi;
59 }
61 {
62  switch (version)
63  {
64  case 0:
65  {
66  // The coordinates:
67  float x0, y0, phi0;
68  in >> x0 >> y0 >> phi0;
69  m_coords[0] = x0;
70  m_coords[1] = y0;
71  this->phi(phi0);
72  }
73  break;
74  case 1:
75  {
76  // The coordinates:
77  in >> m_coords[0] >> m_coords[1] >> m_phi;
78  m_cossin_uptodate = false;
79  }
80  break;
81  default:
83  };
84 }
85 
87 {
89  out["x"] = m_coords[0];
90  out["y"] = m_coords[1];
91  out["phi"] = m_phi;
92 }
94 {
95  uint8_t version;
97  switch (version)
98  {
99  case 1:
100  {
101  m_coords[0] = static_cast<double>(in["x"]);
102  m_coords[1] = static_cast<double>(in["y"]);
103  m_phi = static_cast<double>(in["phi"]);
104  }
105  break;
106  default:
108  }
109 }
110 
111 /** Textual output stream function.
112  */
113 std::ostream& mrpt::poses::operator<<(std::ostream& o, const CPose2D& p)
114 {
115  o << format("(%.03f,%.03f,%.02fdeg)", p.x(), p.y(), RAD2DEG(p.phi()));
116  return o;
117 }
118 
119 /*---------------------------------------------------------------
120 The operator a="this"+D is the pose compounding operator.
121  ---------------------------------------------------------------*/
123 {
125 
126  return CPose2D(
127  m_coords[0] + D.m_coords[0] * m_cosphi - D.m_coords[1] * m_sinphi,
128  m_coords[1] + D.m_coords[0] * m_sinphi + D.m_coords[1] * m_cosphi,
129  m_phi + D.m_phi);
130 }
131 
132 /*---------------------------------------------------------------
133  composeFrom
134  ---------------------------------------------------------------*/
135 void CPose2D::composeFrom(const CPose2D& A, const CPose2D& B)
136 {
137  A.update_cached_cos_sin();
138 
139  // Use temporary variables for the cases (A==this) or (B==this)
140  const double new_x =
141  A.m_coords[0] + B.m_coords[0] * A.m_cosphi - B.m_coords[1] * A.m_sinphi;
142  const double new_y =
143  A.m_coords[1] + B.m_coords[0] * A.m_sinphi + B.m_coords[1] * A.m_cosphi;
144  m_coords[0] = new_x;
145  m_coords[1] = new_y;
146 
147  m_phi = math::wrapToPi(A.m_phi + B.m_phi);
148  m_cossin_uptodate = false;
149 }
150 
151 /*---------------------------------------------------------------
152  getRotationMatrix
153  ---------------------------------------------------------------*/
155 {
157  R(0, 0) = m_cosphi;
158  R(0, 1) = -m_sinphi;
159  R(1, 0) = m_sinphi;
160  R(1, 1) = m_cosphi;
161 }
162 
164 {
166  R(0, 0) = m_cosphi;
167  R(0, 1) = -m_sinphi;
168  R(0, 2) = 0;
169  R(1, 0) = m_sinphi;
170  R(1, 1) = m_cosphi;
171  R(1, 2) = 0;
172  R(2, 0) = 0;
173  R(2, 1) = 0;
174  R(2, 2) = 1;
175 }
176 
177 /*---------------------------------------------------------------
178 The operator a="this"+D is the pose compounding operator.
179  ---------------------------------------------------------------*/
181 {
182  return CPose3D(*this) + D;
183 }
184 
185 /*---------------------------------------------------------------
186 The operator u'="this"+u is the pose/point compounding operator.
187  ---------------------------------------------------------------*/
189 {
191 
192  return CPoint2D(
193  m_coords[0] + u.x() * m_cosphi - u.y() * m_sinphi,
194  m_coords[1] + u.x() * m_sinphi + u.y() * m_cosphi);
195 }
196 
197 /** An alternative, slightly more efficient way of doing \f$ G = P \oplus L \f$
198  * with G and L being 2D points and P this 2D pose. */
199 void CPose2D::composePoint(double lx, double ly, double& gx, double& gy) const
200 {
202 
203  gx = m_coords[0] + lx * m_cosphi - ly * m_sinphi;
204  gy = m_coords[1] + lx * m_sinphi + ly * m_cosphi;
205 }
206 
208  const mrpt::math::TPoint2D& l, mrpt::math::TPoint2D& g) const
209 {
210  this->composePoint(l.x, l.y, g.x, g.y);
211 }
212 
213 /** overload \f$ G = P \oplus L \f$ with G and L being 3D points and P this 2D
214  * pose (the "z" coordinate remains unmodified) */
216  const mrpt::math::TPoint3D& l, mrpt::math::TPoint3D& g) const
217 {
218  this->composePoint(l.x, l.y, l.z, g.x, g.y, g.z);
219 }
221 {
223  composePoint(l, g);
224  return g;
225 }
226 
228  double lx, double ly, double lz, double& gx, double& gy, double& gz) const
229 {
231  gx = m_coords[0] + lx * m_cosphi - ly * m_sinphi;
232  gy = m_coords[1] + lx * m_sinphi + ly * m_cosphi;
233  gz = lz;
234 }
235 
237  const double gx, const double gy, double& lx, double& ly) const
238 {
240 
241  const double Ax = gx - m_coords[0];
242  const double Ay = gy - m_coords[1];
243 
244  lx = Ax * m_cosphi + Ay * m_sinphi;
245  ly = -Ax * m_sinphi + Ay * m_cosphi;
246 }
248  const mrpt::math::TPoint2D& g, mrpt::math::TPoint2D& l) const
249 {
250  inverseComposePoint(g.x, g.y, l.x, l.y);
251 }
253  const mrpt::math::TPoint2D& g) const
254 {
256  inverseComposePoint(g, l);
257  return l;
258 }
259 
260 /*---------------------------------------------------------------
261 The operator u'="this"+u is the pose/point compounding operator.
262  ---------------------------------------------------------------*/
264 {
266 
267  return CPoint3D(
268  m_coords[0] + u.x() * m_cosphi - u.y() * m_sinphi,
269  m_coords[1] + u.x() * m_sinphi + u.y() * m_cosphi, u.z());
270 }
271 
272 /*---------------------------------------------------------------
273 The operator D="this"-b is the pose inverse compounding operator.
274  The resulting pose "D" is the diference between this pose and "b"
275  ---------------------------------------------------------------*/
277 {
279 
280  m_coords[0] = (A.m_coords[0] - B.m_coords[0]) * B.m_cosphi +
281  (A.m_coords[1] - B.m_coords[1]) * B.m_sinphi;
282  m_coords[1] = -(A.m_coords[0] - B.m_coords[0]) * B.m_sinphi +
283  (A.m_coords[1] - B.m_coords[1]) * B.m_cosphi;
284  m_phi = math::wrapToPi(A.m_phi - B.m_phi);
285  m_cossin_uptodate = false;
286 }
287 
288 /*---------------------------------------------------------------
289  Scalar sum of components: This is diferent from poses
290  composition, which is implemented as "+" operators in "CPose" derived
291  classes.
292  ---------------------------------------------------------------*/
294 {
295  m_coords[0] += p.m_coords[0];
296  m_coords[1] += p.m_coords[1];
297  m_phi += p.m_phi;
298  m_cossin_uptodate = false;
299 }
300 
301 /*---------------------------------------------------------------
302  Scalar multiplication.
303  ---------------------------------------------------------------*/
304 void CPose2D::operator*=(const double s)
305 {
306  m_coords[0] *= s;
307  m_coords[1] *= s;
308  m_phi *= s;
309  m_cossin_uptodate = false;
310 }
311 
312 /*---------------------------------------------------------------
313  Returns the corresponding 4x4 homogeneous
314  transformation matrix for the point(translation),
315  or pose (translation+orientation).
316 ---------------------------------------------------------------*/
318 {
319  m.setIdentity();
320  m(0, 3) = m_coords[0];
321  m(1, 3) = m_coords[1];
322 
324 
325  m(0, 0) = m_cosphi;
326  m(0, 1) = -m_sinphi;
327  m(1, 0) = m_sinphi;
328  m(1, 1) = m_cosphi;
329 }
330 
331 /** Forces "phi" to be in the range [-pi,pi];
332  */
334 {
336  m_cossin_uptodate = false;
337 }
338 
340  : m_phi(o.phi), m_cossin_uptodate(false)
341 {
342  m_coords[0] = o.x;
343  m_coords[1] = o.y;
344 }
345 
346 CPose2D::CPose2D(const CPoint3D& o) : m_phi(0), m_cossin_uptodate(false)
347 {
348  this->m_coords[0] = o.x();
349  this->m_coords[1] = o.y();
350  normalizePhi();
351 }
352 
353 /*---------------------------------------------------------------
354  unary -
355 ---------------------------------------------------------------*/
357 {
358  CPose2D ret = b;
359  ret.inverse();
360  return ret;
361 }
362 
363 /** Convert this pose into its inverse, saving the result in itself. \sa
364  * operator- */
366 {
368  const double x = m_coords[0];
369  const double y = m_coords[1];
370 
371  m_coords[0] = -x * m_cosphi - y * m_sinphi;
372  m_coords[1] = x * m_sinphi - y * m_cosphi;
373  m_phi = -m_phi;
374  m_cossin_uptodate = false;
375 }
376 
377 void CPose2D::asVector(vector_t& v) const
378 {
379  v[0] = m_coords[0];
380  v[1] = m_coords[1];
381  v[2] = m_phi;
382 }
383 
384 bool mrpt::poses::operator==(const CPose2D& p1, const CPose2D& p2)
385 {
386  return (p1.x() == p2.x()) && (p1.y() == p2.y()) && (p1.phi() == p2.phi());
387 }
388 
389 bool mrpt::poses::operator!=(const CPose2D& p1, const CPose2D& p2)
390 {
391  return (p1.x() != p2.x()) || (p1.y() != p2.y()) || (p1.phi() != p2.phi());
392 }
393 
395 {
396  const double ccos = pose.phi_cos();
397  const double ssin = pose.phi_sin();
398  return TPoint2D(
399  pose.x() + u.x * ccos - u.y * ssin, pose.y() + u.x * ssin + u.y * ccos);
400 }
401 
402 /** Make \f$ this = this \oplus b \f$ */
404 {
405  composeFrom(*this, b);
406  return *this;
407 }
408 
409 void CPose2D::fromString(const std::string& s)
410 {
411  CMatrixDouble m;
412  if (!m.fromMatlabStringFormat(s))
413  THROW_EXCEPTION("Malformed expression in ::fromString");
414  ASSERTMSG_(m.rows() == 1 && m.cols() == 3, "Expected vector length=3");
415  x(m(0, 0));
416  y(m(0, 1));
417  phi(DEG2RAD(m(0, 2)));
418 }
419 
420 void CPose2D::fromStringRaw(const std::string& s)
421 {
422  this->fromString("[" + s + "]");
423 }
424 
426 {
427  return std::sqrt(
428  square(p.x() - x()) + square(p.y() - y()) +
429  4 * (1 - cos(p.phi() - phi())));
430 }
431 
433 {
436  getHomogeneousMatrixVal<CMatrixDouble44>());
437  return CPose3D(RES);
438 }
439 
441 {
442  return CPose2D(-m_coords[0], -m_coords[1], -m_phi);
443 }
444 
445 void CPose2D::asString(std::string& s) const
446 {
447  s = mrpt::format("[%f %f %fdeg]", x(), y(), mrpt::RAD2DEG(m_phi));
448 }
449 
451 {
452  for (int i = 0; i < 3; i++)
453  (*this)[i] = std::numeric_limits<double>::quiet_NaN();
454 }
455 
457 {
458  if (m_cossin_uptodate) return;
459 #ifdef HAVE_SINCOS
460  ::sincos(m_phi, &m_sinphi, &m_cosphi);
461 #else
462  m_cosphi = ::cos(m_phi);
463  m_sinphi = ::sin(m_phi);
464 #endif
465  m_cossin_uptodate = true;
466 }
467 
469 {
470  return mrpt::math::TPose2D(x(), y(), phi());
471 }
void asVector(vector_t &v) const
Returns a 1x3 vector with [x y phi].
Definition: CPose2D.cpp:377
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
double phi_sin() const
Get a (cached) value of sin(phi), recomputing it only once when phi changes.
Definition: CPose2D.h:103
TPoint2D_< double > TPoint2D
Lightweight 2D point.
Definition: TPoint2D.h:213
double x
X,Y coordinates.
Definition: TPose2D.h:30
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
T x
X,Y coordinates.
Definition: TPoint2D.h:25
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to 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:394
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Definition: CPose2D.cpp:60
std::ostream & operator<<(std::ostream &o, const CPoint2D &p)
Dumps a point as a string (x,y)
Definition: CPoint2D.cpp:102
MATRIX44 getInverseHomogeneousMatrixVal() const
Definition: CPoseOrPoint.h:298
void operator*=(const double s)
Scalar multiplication.
Definition: CPose2D.cpp:304
void AddComponents(const CPose2D &p)
Scalar sum of components: This is diferent from poses composition, which is implemented as "+" operat...
Definition: CPose2D.cpp:293
double m_cosphi
Precomputed cos() & sin() of phi.
Definition: CPose2D.h:54
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:199
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: CPose2D.cpp:54
mrpt::math::TPose2D asTPose() const
Definition: CPose2D.cpp:468
mrpt::math::CVectorFixedDouble< 2 > m_coords
[x,y]
Definition: CPose2D.h:48
Virtual base class for "schematic archives" (JSON, XML,...)
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
CPose2D operator-(const CPose2D &b) const
Compute .
Definition: CPose2D.h:210
void fromStringRaw(const std::string &s)
Same as fromString, but without requiring the square brackets in the string.
Definition: CPose2D.cpp:420
This base provides a set of functions for maths stuff.
double phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
constexpr double DEG2RAD(const double x)
Degrees to radians.
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CPose2D.cpp:55
double m_phi
The orientation of the pose, in radians.
Definition: CPose2D.h:52
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:356
void setToNaN() override
Set all data fields to quiet NaN.
Definition: CPose2D.cpp:450
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
bool fromMatlabStringFormat(const std::string &s, mrpt::optional_ref< std::ostream > dump_errors_here=std::nullopt)
Reads a matrix from a string in Matlab-like format, for example: "[1 0 2; 0 4 -1]" The string must st...
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
void inverse()
Convert this pose into its inverse, saving the result in itself.
Definition: CPose2D.cpp:365
CPose2D operator+(const CPose2D &D) const
The operator is the pose compounding operator.
Definition: CPose2D.cpp:122
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:50
size_type rows() const
Number of rows in the matrix.
A class used to store a 2D point.
Definition: CPoint2D.h:32
A class used to store a 3D point.
Definition: CPoint3D.h:31
size_type cols() const
Number of columns in the matrix.
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:248
bool operator!=(const CPoint< DERIVEDCLASS, DIM > &p1, const CPoint< DERIVEDCLASS, DIM > &p2)
Definition: CPoint.h:128
return_t square(const num_t x)
Inline function for the square of a number.
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
bool operator==(const CPoint< DERIVEDCLASS, DIM > &p1, const CPoint< DERIVEDCLASS, DIM > &p2)
Definition: CPoint.h:119
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
CPose2D & operator+=(const CPose2D &b)
Make .
Definition: CPose2D.cpp:403
CPose2D getOppositeScalar() const
Return the opposite of the current pose instance by taking the negative of all its components individ...
Definition: CPose2D.cpp:440
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:317
constexpr double RAD2DEG(const double x)
Radians to degrees.
void composeFrom(const CPose2D &A, const CPose2D &B)
Makes .
Definition: CPose2D.cpp:135
double phi_cos() const
Get a (cached) value of cos(phi), recomputing it only once when phi changes.
Definition: CPose2D.h:96
void inverseComposePoint(const double gx, const double gy, double &lx, double &ly) const
Computes the 2D point L such as .
Definition: CPose2D.cpp:236
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:409
Lightweight 2D pose.
Definition: TPose2D.h:22
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:425
#define SCHEMA_SERIALIZE_DATATYPE_VERSION(ser_version)
For use inside all serializeTo(CSchemeArchiveBase) methods.
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:276
bool m_cossin_uptodate
Definition: CPose2D.h:55
CMatrixFixed< double, 4, 4 > CMatrixDouble44
Definition: CMatrixFixed.h:368
CPose2D()
Default constructor (all coordinates to 0)
Definition: CPose2D.cpp:33
void normalizePhi()
Forces "phi" to be in the range [-pi,pi];.
Definition: CPose2D.cpp:333
void update_cached_cos_sin() const
Definition: CPose2D.cpp:456



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020