MRPT  1.9.9
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-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 #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 const 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;
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 };
137 
138 /** Exact comparison between 2D poses, taking possible cycles into account */
139 inline bool operator==(const TPose2D& p1, const TPose2D& p2)
140 {
141  return (p1.x == p2.x) && (p1.y == p2.y) &&
142  (mrpt::math::wrapTo2Pi(p1.phi) ==
143  mrpt::math::wrapTo2Pi(p2.phi)); //-V550
144 }
145 /** Exact comparison between 2D poses, taking possible cycles into account */
146 inline bool operator!=(const TPose2D& p1, const TPose2D& p2)
147 {
148  return (p1.x != p2.x) || (p1.y != p2.y) ||
149  (mrpt::math::wrapTo2Pi(p1.phi) !=
150  mrpt::math::wrapTo2Pi(p2.phi)); //-V550
151 }
152 
153 } // namespace mrpt::math
154 
155 namespace mrpt::typemeta
156 {
157 // Specialization must occur in the same namespace
159 } // 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
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...
void normalizePhi()
Forces "phi" to be in the range [-pi,pi].
Definition: TPose2D.h:129
mrpt::math::TPoint2D composePoint(const TPoint2D l) const
Base type of all TPoseXX and TPointXX classes in mrpt::math.
Definition: TPoseOrPoint.h:24
constexpr const double & operator[](size_t i) const
Coordinate access using operator[].
Definition: TPose2D.h:79
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...
GLdouble s
Definition: glext.h:3682
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".
mrpt::math::TPoint2D inverseComposePoint(const TPoint2D g) const
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
GLubyte g
Definition: glext.h:6372
GLubyte GLubyte b
Definition: glext.h:6372
#define MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(_TYPE, __NS)
Declares a typename to be "type" (without the NS prefix)
Definition: TTypeName.h:128
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
static constexpr TPose2D Identity()
Returns the identity transformation.
Definition: TPose2D.h:35
const GLdouble * v
Definition: glext.h:3684
Provided for STL and matrices/vectors compatibility.
Definition: TPoseOrPoint.h:54
constexpr TPose2D(double xx, double yy, double Phi)
Constructor from coordinates.
Definition: TPose2D.h:56
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:23
Lightweight 2D pose.
Definition: TPose2D.h:22
std::string asString() const
Definition: TPose2D.h:108
GLenum GLint GLint y
Definition: glext.h:3542
mrpt::math::TPose2D operator-(const mrpt::math::TPose2D &b) const
Operator "ominus" pose composition: "ret=this \ominus b".
GLenum GLint x
Definition: glext.h:3542
Lightweight 3D point.
Definition: TPoint3D.h:90
constexpr bool operator==(const TPoint2D &p1, const TPoint2D &p2)
Exact comparison between 2D points.
Definition: TPoint2D.h:166
Lightweight 2D point.
Definition: TPoint2D.h:31
GLfloat GLfloat p
Definition: glext.h:6398
constexpr bool operator!=(const TPoint2D &p1, const TPoint2D &p2)
Exact comparison between 2D points.
Definition: TPoint2D.h:171
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 1.9.9 Git: 765b969e7 Sun Sep 22 19:55:28 2019 +0200 at dom sep 22 20:00:14 CEST 2019