MRPT  2.0.1
TPose3DQuat.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/math/TPoseOrPoint.h>
12 
13 namespace mrpt::math
14 {
15 /** Lightweight 3D pose (three spatial coordinates, plus a quaternion ). Allows
16  * coordinate access using [] operator.
17  * \sa mrpt::poses::CPose3DQuat
18  */
19 struct TPose3DQuat : public TPoseOrPoint,
20  public internal::ProvideStaticResize<TPose3DQuat>
21 {
22  enum
23  {
25  };
26  /** Translation in x,y,z */
27  double x{.0}, y{.0}, z{.0};
28  /** Unit quaternion part, qr,qx,qy,qz */
29  double qr{1.}, qx{.0}, qy{.0}, qz{.0};
30 
31  /** Constructor from coordinates. */
32  constexpr TPose3DQuat(
33  double _x, double _y, double _z, double _qr, double _qx, double _qy,
34  double _qz)
35  : x(_x), y(_y), z(_z), qr(_qr), qx(_qx), qy(_qy), qz(_qz)
36  {
37  }
38  /** Default fast constructor. Initializes to identity transformation. */
39  TPose3DQuat() = default;
40  /** Coordinate access using operator[]. Order: x,y,z,qr,qx,qy,qz */
41  double& operator[](size_t i)
42  {
43  switch (i)
44  {
45  case 0:
46  return x;
47  case 1:
48  return y;
49  case 2:
50  return z;
51  case 3:
52  return qr;
53  case 4:
54  return qx;
55  case 5:
56  return qy;
57  case 6:
58  return qz;
59  default:
60  throw std::out_of_range("index out of range");
61  }
62  }
63  /** Coordinate access using operator[]. Order: x,y,z,qr,qx,qy,qz */
64  constexpr double operator[](size_t i) const
65  {
66  switch (i)
67  {
68  case 0:
69  return x;
70  case 1:
71  return y;
72  case 2:
73  return z;
74  case 3:
75  return qr;
76  case 4:
77  return qx;
78  case 5:
79  return qy;
80  case 6:
81  return qz;
82  default:
83  throw std::out_of_range("index out of range");
84  }
85  }
86  /** Pose's spatial coordinates (x,y,z) norm. */
87  double norm() const;
88  /** Gets the pose as a vector of doubles. */
89  void asVector(std::vector<double>& v) const
90  {
91  v.resize(7);
92  for (size_t i = 0; i < 7; i++) v[i] = (*this)[i];
93  }
94  /** Returns a human-readable textual representation of the object as "[x y z
95  * qr qx qy qz]"
96  * \sa fromString
97  */
98  void asString(std::string& s) const
99  {
100  s = mrpt::format("[%f %f %f %f %f %f %f]", x, y, z, qr, qx, qy, qz);
101  }
102  std::string asString() const
103  {
104  std::string s;
105  asString(s);
106  return s;
107  }
108 
109  /** Set the current object value from a string generated by 'asString' (eg:
110  * "[0.02 1.04 -0.8 1.0 0.0 0.0 0.0]" )
111  * \sa asString
112  * \exception std::exception On invalid format
113  */
114  void fromString(const std::string& s);
115 
116  static TPose3DQuat FromString(const std::string& s)
117  {
118  TPose3DQuat o;
119  o.fromString(s);
120  return o;
121  }
122 };
123 
124 } // namespace mrpt::math
125 
126 namespace mrpt::typemeta
127 {
128 // Specialization must occur in the same namespace
130 } // namespace mrpt::typemeta
constexpr TPose3DQuat(double _x, double _y, double _z, double _qr, double _qx, double _qy, double _qz)
Constructor from coordinates.
Definition: TPose3DQuat.h:32
static TPose3DQuat FromString(const std::string &s)
Definition: TPose3DQuat.h:116
double norm() const
Pose&#39;s spatial coordinates (x,y,z) norm.
Definition: TPose3DQuat.cpp:30
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
TPose3DQuat()=default
Default fast constructor.
Base type of all TPoseXX and TPointXX classes in mrpt::math.
Definition: TPoseOrPoint.h:24
std::string asString() const
Definition: TPose3DQuat.h:102
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: TPose3DQuat.cpp:20
double & operator[](size_t i)
Coordinate access using operator[].
Definition: TPose3DQuat.h:41
This base provides a set of functions for maths stuff.
void asVector(std::vector< double > &v) const
Gets the pose as a vector of doubles.
Definition: TPose3DQuat.h:89
double x
Translation in x,y,z.
Definition: TPose3DQuat.h:27
#define MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(_TYPE, __NS)
Declares a typename to be "type" (without the NS prefix)
Definition: TTypeName.h:128
constexpr double operator[](size_t i) const
Coordinate access using operator[].
Definition: TPose3DQuat.h:64
double qr
Unit quaternion part, qr,qx,qy,qz.
Definition: TPose3DQuat.h:29
Lightweight 3D pose (three spatial coordinates, plus a quaternion ).
Definition: TPose3DQuat.h:19
Provided for STL and matrices/vectors compatibility.
Definition: TPoseOrPoint.h:63
void asString(std::string &s) const
Returns a human-readable textual representation of the object as "[x y z qr qx qy qz]"...
Definition: TPose3DQuat.h:98



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