Ignition Math

API Reference

6.4.0
Pose3.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 #ifndef IGNITION_MATH_POSE_HH_
18 #define IGNITION_MATH_POSE_HH_
19 
21 #include <ignition/math/Vector3.hh>
22 #include <ignition/math/config.hh>
23 
24 namespace ignition
25 {
26  namespace math
27  {
28  // Inline bracket to help doxygen filtering.
29  inline namespace IGNITION_MATH_VERSION_NAMESPACE {
30  //
33  template<typename T>
34  class Pose3
35  {
37  public: static const Pose3<T> Zero;
38 
40  public: Pose3() : p(0, 0, 0), q(1, 0, 0, 0)
41  {
42  }
43 
47  public: Pose3(const Vector3<T> &_pos, const Quaternion<T> &_rot)
48  : p(_pos), q(_rot)
49  {
50  }
51 
59  public: Pose3(T _x, T _y, T _z, T _roll, T _pitch, T _yaw)
60  : p(_x, _y, _z), q(_roll, _pitch, _yaw)
61  {
62  }
63 
72  public: Pose3(T _x, T _y, T _z, T _qw, T _qx, T _qy, T _qz)
73  : p(_x, _y, _z), q(_qw, _qx, _qy, _qz)
74  {
75  }
76 
79  public: Pose3(const Pose3<T> &_pose)
80  : p(_pose.p), q(_pose.q)
81  {
82  }
83 
85  public: virtual ~Pose3()
86  {
87  }
88 
92  public: void Set(const Vector3<T> &_pos, const Quaternion<T> &_rot)
93  {
94  this->p = _pos;
95  this->q = _rot;
96  }
97 
101  public: void Set(const Vector3<T> &_pos, const Vector3<T> &_rpy)
102  {
103  this->p = _pos;
104  this->q.Euler(_rpy);
105  }
106 
114  public: void Set(T _x, T _y, T _z, T _roll, T _pitch, T _yaw)
115  {
116  this->p.Set(_x, _y, _z);
117  this->q.Euler(math::Vector3<T>(_roll, _pitch, _yaw));
118  }
119 
121  public: bool IsFinite() const
122  {
123  return this->p.IsFinite() && this->q.IsFinite();
124  }
125 
127  public: inline void Correct()
128  {
129  this->p.Correct();
130  this->q.Correct();
131  }
132 
135  public: Pose3<T> Inverse() const
136  {
137  Quaternion<T> inv = this->q.Inverse();
138  return Pose3<T>(inv * (this->p*-1), inv);
139  }
140 
147  public: Pose3<T> operator+(const Pose3<T> &_pose) const
148  {
149  Pose3<T> result;
150 
151  result.p = this->CoordPositionAdd(_pose);
152  result.q = this->CoordRotationAdd(_pose.q);
153 
154  return result;
155  }
156 
160  public: const Pose3<T> &operator+=(const Pose3<T> &_pose)
161  {
162  this->p = this->CoordPositionAdd(_pose);
163  this->q = this->CoordRotationAdd(_pose.q);
164 
165  return *this;
166  }
167 
172  public: inline Pose3<T> operator-() const
173  {
174  return Pose3<T>() - *this;
175  }
176 
183  public: inline Pose3<T> operator-(const Pose3<T> &_pose) const
184  {
185  return Pose3<T>(this->CoordPositionSub(_pose),
186  this->CoordRotationSub(_pose.q));
187  }
188 
192  public: const Pose3<T> &operator-=(const Pose3<T> &_pose)
193  {
194  this->p = this->CoordPositionSub(_pose);
195  this->q = this->CoordRotationSub(_pose.q);
196 
197  return *this;
198  }
199 
203  public: bool operator==(const Pose3<T> &_pose) const
204  {
205  return this->p == _pose.p && this->q == _pose.q;
206  }
207 
211  public: bool operator!=(const Pose3<T> &_pose) const
212  {
213  return this->p != _pose.p || this->q != _pose.q;
214  }
215 
219  public: Pose3<T> operator*(const Pose3<T> &_pose) const
220  {
221  return Pose3<T>(_pose.CoordPositionAdd(*this), this->q * _pose.q);
222  }
223 
228  public: const Pose3<T> &operator*=(const Pose3<T> &_pose)
229  {
230  *this = *this * _pose;
231  return *this;
232  }
233 
236  public: Pose3<T> &operator=(const Pose3<T> &_pose)
237  {
238  this->p = _pose.p;
239  this->q = _pose.q;
240  return *this;
241  }
242 
246  public: Vector3<T> CoordPositionAdd(const Vector3<T> &_pos) const
247  {
248  Quaternion<T> tmp(0.0, _pos.X(), _pos.Y(), _pos.Z());
249 
250  // result = pose.q + pose.q * this->p * pose.q!
251  tmp = this->q * (tmp * this->q.Inverse());
252 
253  return Vector3<T>(this->p.X() + tmp.X(),
254  this->p.Y() + tmp.Y(),
255  this->p.Z() + tmp.Z());
256  }
257 
261  public: Vector3<T> CoordPositionAdd(const Pose3<T> &_pose) const
262  {
263  Quaternion<T> tmp(static_cast<T>(0),
264  this->p.X(), this->p.Y(), this->p.Z());
265 
266  // result = _pose.q + _pose.q * this->p * _pose.q!
267  tmp = _pose.q * (tmp * _pose.q.Inverse());
268 
269  return Vector3<T>(_pose.p.X() + tmp.X(),
270  _pose.p.Y() + tmp.Y(),
271  _pose.p.Z() + tmp.Z());
272  }
273 
277  public: inline Vector3<T> CoordPositionSub(const Pose3<T> &_pose) const
278  {
279  Quaternion<T> tmp(0,
280  this->p.X() - _pose.p.X(),
281  this->p.Y() - _pose.p.Y(),
282  this->p.Z() - _pose.p.Z());
283 
284  tmp = _pose.q.Inverse() * (tmp * _pose.q);
285  return Vector3<T>(tmp.X(), tmp.Y(), tmp.Z());
286  }
287 
291  public: Quaternion<T> CoordRotationAdd(const Quaternion<T> &_rot) const
292  {
293  return Quaternion<T>(_rot * this->q);
294  }
295 
300  const Quaternion<T> &_rot) const
301  {
302  Quaternion<T> result(_rot.Inverse() * this->q);
303  result.Normalize();
304  return result;
305  }
306 
310  public: Pose3<T> CoordPoseSolve(const Pose3<T> &_b) const
311  {
312  Quaternion<T> qt;
313  Pose3<T> a;
314 
315  a.q = this->q.Inverse() * _b.q;
316  qt = a.q * Quaternion<T>(0, this->p.X(), this->p.Y(), this->p.Z());
317  qt = qt * a.q.Inverse();
318  a.p = _b.p - Vector3<T>(qt.X(), qt.Y(), qt.Z());
319 
320  return a;
321  }
322 
324  public: void Reset()
325  {
326  // set the position to zero
327  this->p.Set();
328  this->q = Quaterniond::Identity;
329  }
330 
335  {
336  Pose3<T> a = *this;
337  a.p.X((1.0 - 2.0*_q.Y()*_q.Y() - 2.0*_q.Z()*_q.Z()) * this->p.X()
338  +(2.0*(_q.X()*_q.Y()+_q.W()*_q.Z())) * this->p.Y()
339  +(2.0*(_q.X()*_q.Z()-_q.W()*_q.Y())) * this->p.Z());
340  a.p.Y((2.0*(_q.X()*_q.Y()-_q.W()*_q.Z())) * this->p.X()
341  +(1.0 - 2.0*_q.X()*_q.X() - 2.0*_q.Z()*_q.Z()) * this->p.Y()
342  +(2.0*(_q.Y()*_q.Z()+_q.W()*_q.X())) * this->p.Z());
343  a.p.Z((2.0*(_q.X()*_q.Z()+_q.W()*_q.Y())) * this->p.X()
344  +(2.0*(_q.Y()*_q.Z()-_q.W()*_q.X())) * this->p.Y()
345  +(1.0 - 2.0*_q.X()*_q.X() - 2.0*_q.Y()*_q.Y()) * this->p.Z());
346  return a;
347  }
348 
351  public: void Round(int _precision)
352  {
353  this->q.Round(_precision);
354  this->p.Round(_precision);
355  }
356 
359  public: inline const Vector3<T> &Pos() const
360  {
361  return this->p;
362  }
363 
366  public: inline Vector3<T> &Pos()
367  {
368  return this->p;
369  }
370 
375  public: inline const T X() const
376  {
377  return this->p.X();
378  }
379 
381  public: inline void SetX(T x)
382  {
383  this->p.X() = x;
384  }
385 
390  public: inline const T Y() const
391  {
392  return this->p.Y();
393  }
394 
396  public: inline void SetY(T y)
397  {
398  this->p.Y() = y;
399  }
400 
405  public: inline const T Z() const
406  {
407  return this->p.Z();
408  }
409 
411  public: inline void SetZ(T z)
412  {
413  this->p.Z() = z;
414  }
415 
418  public: inline const Quaternion<T> &Rot() const
419  {
420  return this->q;
421  }
422 
425  public: inline Quaternion<T> &Rot()
426  {
427  return this->q;
428  }
429 
434  public: inline const T Roll() const
435  {
436  return this->q.Roll();
437  }
438 
443  public: inline const T Pitch() const
444  {
445  return this->q.Pitch();
446  }
447 
452  public: inline const T Yaw() const
453  {
454  return this->q.Yaw();
455  }
456 
461  public: friend std::ostream &operator<<(
462  std::ostream &_out, const ignition::math::Pose3<T> &_pose)
463  {
464  _out << _pose.Pos() << " " << _pose.Rot();
465  return _out;
466  }
467 
472  public: friend std::istream &operator>>(
474  {
475  // Skip white spaces
476  _in.setf(std::ios_base::skipws);
477  Vector3<T> pos;
478  Quaternion<T> rot;
479  _in >> pos >> rot;
480  _pose.Set(pos, rot);
481  return _in;
482  }
483 
485  private: Vector3<T> p;
486 
488  private: Quaternion<T> q;
489  };
490  template<typename T> const Pose3<T> Pose3<T>::Zero(0, 0, 0, 0, 0, 0);
491 
495  }
496  }
497 }
498 #endif
const T Roll() const
Get the Roll value of the rotation.
Definition: Pose3.hh:434
friend std::istream & operator>>(std::istream &_in, Pose3< T > &_pose)
Stream extraction operator.
Definition: Pose3.hh:472
Pose3< T > CoordPoseSolve(const Pose3< T > &_b) const
Find the inverse of a pose; i.e., if b = this + a, given b and this, find a.
Definition: Pose3.hh:310
T setf(T... args)
Pose3< T > & operator=(const Pose3< T > &_pose)
Equal operator.
Definition: Pose3.hh:236
const T & W() const
Get the w component.
Definition: Quaternion.hh:949
Quaternion< T > & Rot()
Get a mutuable reference to the rotation.
Definition: Pose3.hh:425
bool IsFinite() const
See if a pose is finite (e.g., not nan)
Definition: Pose3.hh:121
const T & Z() const
Get the z component.
Definition: Quaternion.hh:970
void Reset()
Reset the pose.
Definition: Pose3.hh:324
Pose3< T > operator-() const
Negation operator A is the transform from O to P in frame O then -A is transform from P to O specifie...
Definition: Pose3.hh:172
bool operator!=(const Pose3< T > &_pose) const
Inequality operator.
Definition: Pose3.hh:211
void Normalize()
Normalize the quaternion.
Definition: Quaternion.hh:224
const T Yaw() const
Get the Yaw value of the rotation.
Definition: Pose3.hh:452
void SetZ(T z)
Set the Z value of the position.
Definition: Pose3.hh:411
Pose3(T _x, T _y, T _z, T _roll, T _pitch, T _yaw)
Constructor.
Definition: Pose3.hh:59
Quaternion< T > CoordRotationSub(const Quaternion< T > &_rot) const
Subtract one rotation from another: result = this->q - rot.
Definition: Pose3.hh:299
void Round(int _precision)
Round all values to _precision decimal places.
Definition: Pose3.hh:351
Encapsulates a position and rotation in three space.
Definition: Pose3.hh:34
void Correct()
Fix any nan values.
Definition: Pose3.hh:127
const Pose3< T > & operator+=(const Pose3< T > &_pose)
Add-Equals operator.
Definition: Pose3.hh:160
const T & X() const
Get the x component.
Definition: Quaternion.hh:956
Pose3< T > operator-(const Pose3< T > &_pose) const
Subtraction operator A is the transform from O to P in frame O B is the transform from O to Q in fram...
Definition: Pose3.hh:183
static const Pose3< T > Zero
math::Pose3<T>(0, 0, 0, 0, 0, 0)
Definition: Pose3.hh:37
Pose3(const Vector3< T > &_pos, const Quaternion< T > &_rot)
Constructor.
Definition: Pose3.hh:47
const T & Y() const
Get the y component.
Definition: Quaternion.hh:963
void SetX(T x)
Set X value of the position.
Definition: Pose3.hh:381
STL class.
T X() const
Get the x value.
Definition: Vector3.hh:648
const T Y() const
Get the Y value of the position.
Definition: Pose3.hh:390
const Pose3< T > & operator*=(const Pose3< T > &_pose)
Multiplication assignment operator. This pose will become equal to this * _pose.
Definition: Pose3.hh:228
T Z() const
Get the z value.
Definition: Vector3.hh:662
const T Z() const
Get the Z value of the position.
Definition: Pose3.hh:405
Pose3< T > Inverse() const
Get the inverse of this pose.
Definition: Pose3.hh:135
Pose3< T > operator+(const Pose3< T > &_pose) const
Addition operator A is the transform from O to P specified in frame O B is the transform from P to Q ...
Definition: Pose3.hh:147
T Y() const
Get the y value.
Definition: Vector3.hh:655
void Set(const Vector3< T > &_pos, const Quaternion< T > &_rot)
Set the pose from a Vector3 and a Quaternion<T>
Definition: Pose3.hh:92
void SetY(T y)
Set the Y value of the position.
Definition: Pose3.hh:396
Vector3< T > CoordPositionAdd(const Vector3< T > &_pos) const
Add one point to a vector: result = this + pos.
Definition: Pose3.hh:246
void Set(T _x, T _y, T _z, T _roll, T _pitch, T _yaw)
Set the pose from a six tuple.
Definition: Pose3.hh:114
const Pose3< T > & operator-=(const Pose3< T > &_pose)
Subtraction operator.
Definition: Pose3.hh:192
Pose3()
Default constructors.
Definition: Pose3.hh:40
Quaternion< T > CoordRotationAdd(const Quaternion< T > &_rot) const
Add one rotation to another: result = this->q + rot.
Definition: Pose3.hh:291
Pose3< double > Pose3d
Definition: Pose3.hh:493
Pose3(T _x, T _y, T _z, T _qw, T _qx, T _qy, T _qz)
Constructor.
Definition: Pose3.hh:72
Pose3< int > Pose3i
Definition: Pose3.hh:492
static const Quaternion Identity
math::Quaternion(1, 0, 0, 0)
Definition: Quaternion.hh:41
const T Pitch() const
Get the Pitch value of the rotation.
Definition: Pose3.hh:443
Vector3< T > & Pos()
Get a mutable reference to the position.
Definition: Pose3.hh:366
const Quaternion< T > & Rot() const
Get the rotation.
Definition: Pose3.hh:418
friend std::ostream & operator<<(std::ostream &_out, const Pose3< T > &_pose)
Stream insertion operator.
Definition: Pose3.hh:461
Pose3< float > Pose3f
Definition: Pose3.hh:494
Pose3< T > operator*(const Pose3< T > &_pose) const
Multiplication operator.
Definition: Pose3.hh:219
const T X() const
Get the X value of the position.
Definition: Pose3.hh:375
The Vector3 class represents the generic vector containing 3 elements. Since it&#39;s commonly used to ke...
Definition: Vector3.hh:40
Vector3< T > CoordPositionAdd(const Pose3< T > &_pose) const
Add one point to another: result = this + pose.
Definition: Pose3.hh:261
const Vector3< T > & Pos() const
Get the position.
Definition: Pose3.hh:359
Pose3(const Pose3< T > &_pose)
Copy constructor.
Definition: Pose3.hh:79
Vector3< T > CoordPositionSub(const Pose3< T > &_pose) const
Subtract one position from another: result = this - pose.
Definition: Pose3.hh:277
bool operator==(const Pose3< T > &_pose) const
Equality operator.
Definition: Pose3.hh:203
virtual ~Pose3()
Destructor.
Definition: Pose3.hh:85
void Set(const Vector3< T > &_pos, const Vector3< T > &_rpy)
Set the pose from pos and rpy vectors.
Definition: Pose3.hh:101
Definition: Angle.hh:42
A quaternion class.
Definition: Matrix3.hh:35
Quaternion< T > Inverse() const
Get the inverse of this quaternion.
Definition: Quaternion.hh:132
STL class.
Pose3< T > RotatePositionAboutOrigin(const Quaternion< T > &_q) const
Rotate vector part of a pose about the origin.
Definition: Pose3.hh:334