MRPT  2.0.1
TPose2D.h
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 #pragma once
10 
11 #include <mrpt/core/bits_math.h> // hypot_fast()
12 #include <mrpt/math/TPoseOrPoint.h>
13 #include <mrpt/math/wrap2pi.h>
14 #include <vector>
15 
16 namespace mrpt::math
17 {
18 /**
19  * Lightweight 2D pose. Allows coordinate access using [] operator.
20  * \sa mrpt::poses::CPose2D
21  */
22 struct TPose2D : public TPoseOrPoint,
23  public internal::ProvideStaticResize<TPose2D>
24 {
25  enum
26  {
28  };
29  /** X,Y coordinates */
30  double x{.0}, y{.0};
31  /** Orientation (rads) */
32  double phi{.0};
33 
34  /** Returns the identity transformation */
35  static constexpr TPose2D Identity() { return TPose2D(); }
36 
37  /** Implicit constructor from TPoint2D. Zeroes the phi coordinate.
38  * \sa TPoint2D
39  */
40  TPose2D(const TPoint2D& p);
41  /**
42  * Constructor from TPoint3D, losing information. Zeroes the phi
43  * coordinate.
44  * \sa TPoint3D
45  */
46  explicit TPose2D(const TPoint3D& p);
47  /**
48  * Constructor from TPose3D, losing information. The phi corresponds to the
49  * original pose's yaw.
50  * \sa TPose3D
51  */
52  explicit TPose2D(const TPose3D& p);
53  /**
54  * Constructor from coordinates.
55  */
56  constexpr TPose2D(double xx, double yy, double Phi) : x(xx), y(yy), phi(Phi)
57  {
58  }
59  /**
60  * Default fast constructor. Initializes to zeros.
61  */
62  constexpr TPose2D() = default;
63  /** Coordinate access using operator[]. Order: x,y,phi */
64  double& operator[](size_t i)
65  {
66  switch (i)
67  {
68  case 0:
69  return x;
70  case 1:
71  return y;
72  case 2:
73  return phi;
74  default:
75  throw std::out_of_range("index out of range");
76  }
77  }
78  /** Coordinate access using operator[]. Order: x,y,phi */
79  constexpr double operator[](size_t i) const
80  {
81  switch (i)
82  {
83  case 0:
84  return x;
85  case 1:
86  return y;
87  case 2:
88  return phi;
89  default:
90  throw std::out_of_range("index out of range");
91  }
92  }
93  /**
94  * Transformation into vector.
95  */
96  void asVector(std::vector<double>& v) const
97  {
98  v.resize(3);
99  v[0] = x;
100  v[1] = y;
101  v[2] = phi;
102  }
103  /** Returns a human-readable textual representation of the object (eg: "[x y
104  * yaw]", yaw in degrees)
105  * \sa fromString
106  */
107  void asString(std::string& s) const;
108  std::string asString() const
109  {
110  std::string s;
111  asString(s);
112  return s;
113  }
114 
115  /** Operator "oplus" pose composition: "ret=this \oplus b" \sa CPose2D */
117  /** Operator "ominus" pose composition: "ret=this \ominus b" \sa CPose2D */
119 
121 
123 
125 
126  /** Returns the norm of the (x,y) vector (phi is not used) */
127  double norm() const { return mrpt::hypot_fast(x, y); }
128  /** Forces "phi" to be in the range [-pi,pi] */
130  /** Set the current object value from a string generated by 'asString' (eg:
131  * "[0.02 1.04 -45.0]" )
132  * \sa asString
133  * \exception std::exception On invalid format
134  */
135  void fromString(const std::string& s);
136  static TPose2D FromString(const std::string& s)
137  {
138  TPose2D o;
139  o.fromString(s);
140  return o;
141  }
142 };
143 
144 /** Exact comparison between 2D poses, taking possible cycles into account */
145 inline bool operator==(const TPose2D& p1, const TPose2D& p2)
146 {
147  return (p1.x == p2.x) && (p1.y == p2.y) &&
148  (mrpt::math::wrapTo2Pi(p1.phi) ==
149  mrpt::math::wrapTo2Pi(p2.phi)); //-V550
150 }
151 /** Exact comparison between 2D poses, taking possible cycles into account */
152 inline bool operator!=(const TPose2D& p1, const TPose2D& p2)
153 {
154  return (p1.x != p2.x) || (p1.y != p2.y) ||
155  (mrpt::math::wrapTo2Pi(p1.phi) !=
156  mrpt::math::wrapTo2Pi(p2.phi)); //-V550
157 }
158 
159 } // namespace mrpt::math
160 
161 namespace mrpt::typemeta
162 {
163 // Specialization must occur in the same namespace
165 } // namespace mrpt::typemeta
double norm() const
Returns the norm of the (x,y) vector (phi is not used)
Definition: TPose2D.h:127
double x
X,Y coordinates.
Definition: TPose2D.h:30
void normalizePhi()
Forces "phi" to be in the range [-pi,pi].
Definition: TPose2D.h:129
constexpr double operator[](size_t i) const
Coordinate access using operator[].
Definition: TPose2D.h:79
Base type of all TPoseXX and TPointXX classes in mrpt::math.
Definition: TPoseOrPoint.h:24
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.
void asVector(std::vector< double > &v) const
Transformation into vector.
Definition: TPose2D.h:96
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.
T wrapTo2Pi(T a)
Modifies the given angle to translate it into the [0,2pi[ range.
Definition: wrap2pi.h:38
#define MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(_TYPE, __NS)
Declares a typename to be "type" (without the NS prefix)
Definition: TTypeName.h:128
T hypot_fast(const T x, const T y)
Faster version of std::hypot(), to use when overflow is not an issue and we prefer fast code...
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:50
constexpr bool operator==(const TPoint2D_< T > &p1, const TPoint2D_< T > &p2)
Exact comparison between 2D points.
Definition: TPoint2D.h:223
Base template for TPoint2D and TPoint2Df.
Definition: TPoint2D.h:32
static constexpr TPose2D Identity()
Returns the identity transformation.
Definition: TPose2D.h:35
constexpr bool operator!=(const TPoint2D_< T > &p1, const TPoint2D_< T > &p2)
Exact comparison between 2D points.
Definition: TPoint2D.h:230
Provided for STL and matrices/vectors compatibility.
Definition: TPoseOrPoint.h:63
constexpr TPose2D(double xx, double yy, double Phi)
Constructor from coordinates.
Definition: TPose2D.h:56
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
static TPose2D FromString(const std::string &s)
Definition: TPose2D.h:136
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
double & operator[](size_t i)
Coordinate access using operator[].
Definition: TPose2D.h:64



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020