Main MRPT website > C++ reference for MRPT 1.4.0
Pose6D.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 // $Id: Pose6D.h 371 2012-05-02 15:52:02Z ahornung $
10 
11 /**
12 * OctoMap:
13 * A probabilistic, flexible, and compact 3D mapping library for robotic systems.
14 * @author K. M. Wurm, A. Hornung, University of Freiburg, Copyright (C) 2009.
15 * @see http://octomap.sourceforge.net/
16 * License: New BSD License
17 */
18 
19 /*
20  * Copyright (c) 2009, K. M. Wurm, A. Hornung, University of Freiburg
21  * All rights reserved.
22  *
23  * Redistribution and use in source and binary forms, with or without
24  * modification, are permitted provided that the following conditions are met:
25  *
26  * * Redistributions of source code must retain the above copyright
27  * notice, this list of conditions and the following disclaimer.
28  * * Redistributions in binary form must reproduce the above copyright
29  * notice, this list of conditions and the following disclaimer in the
30  * documentation and/or other materials provided with the distribution.
31  * * Neither the name of the University of Freiburg nor the names of its
32  * contributors may be used to endorse or promote products derived from
33  * this software without specific prior written permission.
34  *
35  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
36  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
37  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
38  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
39  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
40  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
41  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
42  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
43  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
44  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
45  * POSSIBILITY OF SUCH DAMAGE.
46  */
47 
48 #ifndef OCTOMATH_POSE6D_H
49 #define OCTOMATH_POSE6D_H
50 
51 #include "Vector3.h"
52 #include "Quaternion.h"
53 #include <mrpt/maps/link_pragmas.h> // For DLL export within mrpt-maps via the MAPS_IMPEXP macro
54 
55 namespace octomath {
56 
57  /*!
58  * \brief This class represents a tree-dimensional pose of an object
59  *
60  * The tree-dimensional pose is represented by a three-dimensional
61  * translation vector representing the position of the object and
62  * a Quaternion representing the attitude of the object
63  */
65  public:
66 
67  Pose6D();
68  ~Pose6D();
69 
70  /*!
71  * \brief Constructor
72  *
73  * Constructs a pose from given translation and rotation.
74  */
75  Pose6D(const Vector3& trans, const Quaternion& rot);
76 
77  /*!
78  * \brief Constructor
79  *
80  * Constructs a pose from a translation represented by
81  * its x, y, z-values and a rotation represented by its
82  * Tait-Bryan angles roll, pitch, and yaw
83  */
84  Pose6D(float x, float y, float z, double roll, double pitch, double yaw);
85 
86  Pose6D& operator= (const Pose6D& other);
87  bool operator==(const Pose6D& other) const;
88  bool operator!=(const Pose6D& other) const;
89 
90  /*!
91  * \brief Translational component
92  *
93  * @return the translational component of this pose
94  */
95  inline Vector3& trans() { return translation; }
96  /*!
97  * \brief Rotational component
98  *
99  * @return the rotational component of this pose
100  */
101  inline Quaternion& rot() { return rotation; }
102  /*!
103  * \brief Translational component
104  *
105  * @return the translational component of this pose
106  */
107  const Vector3& trans() const { return translation; }
108  /*!
109  * \brief Rotational component
110  * @return the rotational component of this pose
111  */
112  const Quaternion& rot() const { return rotation; }
113 
114 
115  inline float& x() { return translation(0); }
116  inline float& y() { return translation(1); }
117  inline float& z() { return translation(2); }
118  inline const float& x() const { return translation(0); }
119  inline const float& y() const { return translation(1); }
120  inline const float& z() const { return translation(2); }
121 
122  inline double roll() const {return (rotation.toEuler())(0); }
123  inline double pitch() const {return (rotation.toEuler())(1); }
124  inline double yaw() const {return (rotation.toEuler())(2); }
125 
126  /*!
127  * \brief Transformation of a vector
128  *
129  * Transforms the vector v by the transformation which is
130  * specified by this.
131  * @return the vector which is translated by the translation of
132  * this and afterwards rotated by the rotation of this.
133  */
134  Vector3 transform (const Vector3 &v) const;
135 
136  /*!
137  * \brief Inversion
138  *
139  * Inverts the coordinate transformation represented by this pose
140  * @return a copy of this pose inverted
141  */
142  Pose6D inv() const;
143 
144  /*!
145  * \brief Inversion
146  *
147  * Inverts the coordinate transformation represented by this pose
148  * @return a reference to this pose
149  */
150  Pose6D& inv_IP();
151 
152  /*!
153  * \brief Concatenation
154  *
155  * Concatenates the coordinate transformations represented
156  * by this and p.
157  * @return this * p (applies first this, then p)
158  */
159  Pose6D operator* (const Pose6D &p) const;
160  /*!
161  * \brief In place concatenation
162  *
163  * Concatenates p to this Pose6D.
164  * @return this which results from first moving by this and
165  * afterwards by p
166  */
167  const Pose6D& operator*= (const Pose6D &p);
168 
169  /*!
170  * \brief Translational distance
171  *
172  * @return the translational (euclidian) distance to p
173  */
174  double distance(const Pose6D &other) const;
175 
176  /*!
177  * \brief Translational length
178  *
179  * @return the translational (euclidian) length of the translation
180  * vector of this Pose6D
181  */
182  double transLength() const;
183 
184  /*!
185  * \brief Output operator
186  *
187  * Output to stream in a format which can be parsed using read().
188  */
189  std::ostream& write(std::ostream &s) const;
190  /*!
191  * \brief Input operator
192  *
193  * Parsing from stream which was written by write().
194  */
195  std::istream& read(std::istream &s);
196  /*!
197  * \brief Binary output operator
198  *
199  * Output to stream in a binary format which can be parsed using readBinary().
200  */
201  std::ostream& writeBinary(std::ostream &s) const;
202  /*!
203  * \brief Binary input operator
204  *
205  * Parsing from binary stream which was written by writeBinary().
206  */
207  std::istream& readBinary(std::istream &s);
208 
209  protected:
212  };
213 
214  //! user friendly output in format (x y z, u x y z) which is (translation, rotation)
215  std::ostream MAPS_IMPEXP & operator<<(std::ostream& s, const Pose6D& p);
216 
217 }
218 
219 #endif
bool operator!=(const CArray< T, N > &x, const CArray< T, N > &y)
Definition: CArray.h:287
Quaternion rotation
Definition: Pose6D.h:211
This class represents a tree-dimensional pose of an object.
Definition: Pose6D.h:64
OctoMap: A probabilistic, flexible, and compact 3D mapping library for robotic systems.
Definition: Pose6D.h:55
float & x()
Definition: Pose6D.h:115
std::ostream MAPS_IMPEXP & operator<<(std::ostream &s, const Pose6D &p)
user friendly output in format (x y z, u x y z) which is (translation, rotation)
const Vector3 & trans() const
Translational component.
Definition: Pose6D.h:107
Vector3 translation
Definition: Pose6D.h:210
const float & z() const
Definition: Pose6D.h:120
const Quaternion & rot() const
Rotational component.
Definition: Pose6D.h:112
const float & y() const
Definition: Pose6D.h:119
bool operator==(const CArray< T, N > &x, const CArray< T, N > &y)
Definition: CArray.h:279
float & z()
Definition: Pose6D.h:117
EIGEN_STRONG_INLINE PlainObject inv() const
double yaw() const
Definition: Pose6D.h:124
const float & x() const
Definition: Pose6D.h:118
double pitch() const
Definition: Pose6D.h:123
Quaternion & rot()
Rotational component.
Definition: Pose6D.h:101
float & y()
Definition: Pose6D.h:116
This class represents a Quaternion.
Definition: Quaternion.h:71
std::vector< T1 > & operator*=(std::vector< T1 > &a, const std::vector< T2 > &b)
a*=b (element-wise multiplication)
Definition: ops_vectors.h:40
Vector3 & trans()
Translational component.
Definition: Pose6D.h:95
std::vector< T1 > operator*(const std::vector< T1 > &a, const std::vector< T2 > &b)
a*b (element-wise multiplication)
Definition: ops_vectors.h:59
double BASE_IMPEXP distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
double roll() const
Definition: Pose6D.h:122
This class represents a three-dimensional vector.
Definition: Vector3.h:65



Page generated by Doxygen 1.8.11 for MRPT 1.4.0 SVN: at Mon Aug 15 11:50:21 UTC 2016