MRPT  2.0.5
TPose2D.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 "math-precomp.h" // Precompiled headers
11 
12 #include <mrpt/math/TPoint2D.h>
13 #include <mrpt/math/TPoint3D.h>
14 #include <mrpt/math/TPose2D.h>
15 #include <mrpt/math/TPose3D.h>
16 
17 using namespace mrpt::math;
18 
19 static_assert(std::is_trivially_copyable_v<TPose2D>);
20 
21 TPose2D::TPose2D(const TPoint2D& p) : x(p.x), y(p.y), phi(0.0) {}
22 TPose2D::TPose2D(const TPoint3D& p) : x(p.x), y(p.y), phi(0.0) {}
23 TPose2D::TPose2D(const TPose3D& p) : x(p.x), y(p.y), phi(p.yaw) {}
24 void TPose2D::asString(std::string& s) const
25 {
26  s = mrpt::format("[%f %f %f]", x, y, RAD2DEG(phi));
27 }
28 void TPose2D::fromString(const std::string& s)
29 {
30  CMatrixDouble m;
31  if (!m.fromMatlabStringFormat(s))
32  THROW_EXCEPTION("Malformed expression in ::fromString");
33  ASSERTMSG_(
34  m.rows() == 1 && m.cols() == 3, "Wrong size of vector in ::fromString");
35  x = m(0, 0);
36  y = m(0, 1);
37  phi = DEG2RAD(m(0, 2));
38 }
40  const mrpt::math::TPose2D& b) const
41 {
42  const double A_cosphi = cos(this->phi), A_sinphi = sin(this->phi);
43  // Use temporary variables for the cases (A==this) or (B==this)
44  const double new_x = this->x + b.x * A_cosphi - b.y * A_sinphi;
45  const double new_y = this->y + b.x * A_sinphi + b.y * A_cosphi;
46  const double new_phi = mrpt::math::wrapToPi(this->phi + b.phi);
47 
48  return mrpt::math::TPose2D(new_x, new_y, new_phi);
49 }
50 
52  const mrpt::math::TPose2D& b) const
53 {
54  const double B_cosphi = cos(b.phi), B_sinphi = sin(b.phi);
55 
56  const double new_x =
57  (this->x - b.x) * B_cosphi + (this->y - b.y) * B_sinphi;
58  const double new_y =
59  -(this->x - b.x) * B_sinphi + (this->y - b.y) * B_cosphi;
60  const double new_phi = mrpt::math::wrapToPi(this->phi - b.phi);
61 
62  return mrpt::math::TPose2D(new_x, new_y, new_phi);
63 }
65 {
66  const double ccos = ::cos(phi), csin = ::sin(phi);
67  return {x + l.x * ccos - l.y * csin, y + l.x * csin + l.y * ccos};
68 }
70 {
71  return this->composePoint(b);
72 }
73 
75 {
76  const double Ax = g.x - x, Ay = g.y - y, ccos = ::cos(phi),
77  csin = ::sin(phi);
78  return {Ax * ccos + Ay * csin, -Ax * csin + Ay * ccos};
79 }
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
void fromString(const std::string &s)
Set the current object value from a string generated by &#39;asString&#39; (eg: "[0.02 1.04 -45...
Definition: TPose2D.cpp:28
This base provides a set of functions for maths stuff.
constexpr double DEG2RAD(const double x)
Degrees to radians.
mrpt::math::TPose2D operator+(const mrpt::math::TPose2D &b) const
Operator "oplus" pose composition: "ret=this \oplus b".
Definition: TPose2D.cpp:39
constexpr TPose2D()=default
Default fast constructor.
#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...
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.
size_type cols() const
Number of columns in the matrix.
constexpr double RAD2DEG(const double x)
Radians to degrees.
mrpt::math::TPoint2D composePoint(const TPoint2D l) const
Definition: TPose2D.cpp:64
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
Lightweight 2D pose.
Definition: TPose2D.h:22
std::string asString() const
Definition: TPose2D.h:108
mrpt::math::TPoint2D inverseComposePoint(const TPoint2D g) const
Definition: TPose2D.cpp:74
mrpt::math::TPose2D operator-(const mrpt::math::TPose2D &b) const
Operator "ominus" pose composition: "ret=this \ominus b".
Definition: TPose2D.cpp:51
double phi
Orientation (rads)
Definition: TPose2D.h:32



Page generated by Doxygen 1.8.14 for MRPT 2.0.5 Git: 40e60e732 Thu Jul 9 08:38:35 2020 +0200 at jue jul 9 08:45:11 CEST 2020