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 namespace IGNITION_MATH_VERSION_NAMESPACE
29  {
32  template<typename T>
33  class Pose3
34  {
36  public: static const Pose3<T> Zero;
37 
39  public: Pose3() : p(0, 0, 0), q(1, 0, 0, 0)
40  {
41  }
42 
46  public: Pose3(const Vector3<T> &_pos, const Quaternion<T> &_rot)
47  : p(_pos), q(_rot)
48  {
49  }
50 
58  public: Pose3(T _x, T _y, T _z, T _roll, T _pitch, T _yaw)
59  : p(_x, _y, _z), q(_roll, _pitch, _yaw)
60  {
61  }
62 
71  public: Pose3(T _x, T _y, T _z, T _qw, T _qx, T _qy, T _qz)
72  : p(_x, _y, _z), q(_qw, _qx, _qy, _qz)
73  {
74  }
75 
78  public: Pose3(const Pose3<T> &_pose)
79  : p(_pose.p), q(_pose.q)
80  {
81  }
82 
84  public: virtual ~Pose3()
85  {
86  }
87 
91  public: void Set(const Vector3<T> &_pos, const Quaternion<T> &_rot)
92  {
93  this->p = _pos;
94  this->q = _rot;
95  }
96 
100  public: void Set(const Vector3<T> &_pos, const Vector3<T> &_rpy)
101  {
102  this->p = _pos;
103  this->q.Euler(_rpy);
104  }
105 
113  public: void Set(T _x, T _y, T _z, T _roll, T _pitch, T _yaw)
114  {
115  this->p.Set(_x, _y, _z);
116  this->q.Euler(math::Vector3<T>(_roll, _pitch, _yaw));
117  }
118 
120  public: bool IsFinite() const
121  {
122  return this->p.IsFinite() && this->q.IsFinite();
123  }
124 
126  public: inline void Correct()
127  {
128  this->p.Correct();
129  this->q.Correct();
130  }
131 
134  public: Pose3<T> Inverse() const
135  {
136  Quaternion<T> inv = this->q.Inverse();
137  return Pose3<T>(inv * (this->p*-1), inv);
138  }
139 
146  public: Pose3<T> operator+(const Pose3<T> &_pose) const
147  {
148  Pose3<T> result;
149 
150  result.p = this->CoordPositionAdd(_pose);
151  result.q = this->CoordRotationAdd(_pose.q);
152 
153  return result;
154  }
155 
159  public: const Pose3<T> &operator+=(const Pose3<T> &_pose)
160  {
161  this->p = this->CoordPositionAdd(_pose);
162  this->q = this->CoordRotationAdd(_pose.q);
163 
164  return *this;
165  }
166 
171  public: inline Pose3<T> operator-() const
172  {
173  return Pose3<T>() - *this;
174  }
175 
182  public: inline Pose3<T> operator-(const Pose3<T> &_pose) const
183  {
184  return Pose3<T>(this->CoordPositionSub(_pose),
185  this->CoordRotationSub(_pose.q));
186  }
187 
191  public: const Pose3<T> &operator-=(const Pose3<T> &_pose)
192  {
193  this->p = this->CoordPositionSub(_pose);
194  this->q = this->CoordRotationSub(_pose.q);
195 
196  return *this;
197  }
198 
202  public: bool operator==(const Pose3<T> &_pose) const
203  {
204  return this->p == _pose.p && this->q == _pose.q;
205  }
206 
210  public: bool operator!=(const Pose3<T> &_pose) const
211  {
212  return this->p != _pose.p || this->q != _pose.q;
213  }
214 
218  public: Pose3<T> operator*(const Pose3<T> &_pose) const
219  {
220  return Pose3<T>(this->CoordPositionAdd(_pose), _pose.q * this->q);
221  }
222 
225  public: Pose3<T> &operator=(const Pose3<T> &_pose)
226  {
227  this->p = _pose.p;
228  this->q = _pose.q;
229  return *this;
230  }
231 
235  public: Vector3<T> CoordPositionAdd(const Vector3<T> &_pos) const
236  {
237  Quaternion<T> tmp(0.0, _pos.X(), _pos.Y(), _pos.Z());
238 
239  // result = pose.q + pose.q * this->p * pose.q!
240  tmp = this->q * (tmp * this->q.Inverse());
241 
242  return Vector3<T>(this->p.X() + tmp.X(),
243  this->p.Y() + tmp.Y(),
244  this->p.Z() + tmp.Z());
245  }
246 
250  public: Vector3<T> CoordPositionAdd(const Pose3<T> &_pose) const
251  {
252  Quaternion<T> tmp(static_cast<T>(0),
253  this->p.X(), this->p.Y(), this->p.Z());
254 
255  // result = _pose.q + _pose.q * this->p * _pose.q!
256  tmp = _pose.q * (tmp * _pose.q.Inverse());
257 
258  return Vector3<T>(_pose.p.X() + tmp.X(),
259  _pose.p.Y() + tmp.Y(),
260  _pose.p.Z() + tmp.Z());
261  }
262 
266  public: inline Vector3<T> CoordPositionSub(const Pose3<T> &_pose) const
267  {
268  Quaternion<T> tmp(0,
269  this->p.X() - _pose.p.X(),
270  this->p.Y() - _pose.p.Y(),
271  this->p.Z() - _pose.p.Z());
272 
273  tmp = _pose.q.Inverse() * (tmp * _pose.q);
274  return Vector3<T>(tmp.X(), tmp.Y(), tmp.Z());
275  }
276 
280  public: Quaternion<T> CoordRotationAdd(const Quaternion<T> &_rot) const
281  {
282  return Quaternion<T>(_rot * this->q);
283  }
284 
289  const Quaternion<T> &_rot) const
290  {
291  Quaternion<T> result(_rot.Inverse() * this->q);
292  result.Normalize();
293  return result;
294  }
295 
299  public: Pose3<T> CoordPoseSolve(const Pose3<T> &_b) const
300  {
301  Quaternion<T> qt;
302  Pose3<T> a;
303 
304  a.q = this->q.Inverse() * _b.q;
305  qt = a.q * Quaternion<T>(0, this->p.X(), this->p.Y(), this->p.Z());
306  qt = qt * a.q.Inverse();
307  a.p = _b.p - Vector3<T>(qt.X(), qt.Y(), qt.Z());
308 
309  return a;
310  }
311 
313  public: void Reset()
314  {
315  // set the position to zero
316  this->p.Set();
317  this->q = Quaterniond::Identity;
318  }
319 
324  {
325  Pose3<T> a = *this;
326  a.p.X((1.0 - 2.0*_q.Y()*_q.Y() - 2.0*_q.Z()*_q.Z()) * this->p.X()
327  +(2.0*(_q.X()*_q.Y()+_q.W()*_q.Z())) * this->p.Y()
328  +(2.0*(_q.X()*_q.Z()-_q.W()*_q.Y())) * this->p.Z());
329  a.p.Y((2.0*(_q.X()*_q.Y()-_q.W()*_q.Z())) * this->p.X()
330  +(1.0 - 2.0*_q.X()*_q.X() - 2.0*_q.Z()*_q.Z()) * this->p.Y()
331  +(2.0*(_q.Y()*_q.Z()+_q.W()*_q.X())) * this->p.Z());
332  a.p.Z((2.0*(_q.X()*_q.Z()+_q.W()*_q.Y())) * this->p.X()
333  +(2.0*(_q.Y()*_q.Z()-_q.W()*_q.X())) * this->p.Y()
334  +(1.0 - 2.0*_q.X()*_q.X() - 2.0*_q.Y()*_q.Y()) * this->p.Z());
335  return a;
336  }
337 
340  public: void Round(int _precision)
341  {
342  this->q.Round(_precision);
343  this->p.Round(_precision);
344  }
345 
348  public: inline const Vector3<T> &Pos() const
349  {
350  return this->p;
351  }
352 
355  public: inline Vector3<T> &Pos()
356  {
357  return this->p;
358  }
359 
362  public: inline const Quaternion<T> &Rot() const
363  {
364  return this->q;
365  }
366 
369  public: inline Quaternion<T> &Rot()
370  {
371  return this->q;
372  }
373 
378  public: friend std::ostream &operator<<(
379  std::ostream &_out, const ignition::math::Pose3<T> &_pose)
380  {
381  _out << _pose.Pos() << " " << _pose.Rot();
382  return _out;
383  }
384 
389  public: friend std::istream &operator>>(
390  std::istream &_in, ignition::math::Pose3<T> &_pose)
391  {
392  // Skip white spaces
393  _in.setf(std::ios_base::skipws);
394  Vector3<T> pos;
395  Quaternion<T> rot;
396  _in >> pos >> rot;
397  _pose.Set(pos, rot);
398  return _in;
399  }
400 
402  private: Vector3<T> p;
403 
405  private: Quaternion<T> q;
406  };
407  template<typename T> const Pose3<T> Pose3<T>::Zero(0, 0, 0, 0, 0, 0);
408 
412  }
413  }
414 }
415 #endif
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3::CoordPositionAdd
Vector3< T > CoordPositionAdd(const Pose3< T > &_pose) const
Add one point to another: result = this + pose.
Definition: Pose3.hh:250
ignition
Definition: Angle.hh:39
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3::CoordPositionSub
Vector3< T > CoordPositionSub(const Pose3< T > &_pose) const
Subtract one position from another: result = this - pose.
Definition: Pose3.hh:266
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3::Reset
void Reset()
Reset the pose.
Definition: Pose3.hh:313
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3::operator>>
friend std::istream & operator>>(std::istream &_in, ignition::math::Pose3< T > &_pose)
Stream extraction operator.
Definition: Pose3.hh:389
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3::Round
void Round(int _precision)
Round all values to _precision decimal places.
Definition: Pose3.hh:340
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Quaternion::Y
const T & Y() const
Get the y component.
Definition: Quaternion.hh:962
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3::Rot
Quaternion< T > & Rot()
Get a mutuable reference to the rotation.
Definition: Pose3.hh:369
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3::Pos
const Vector3< T > & Pos() const
Get the position.
Definition: Pose3.hh:348
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3::Zero
static const Pose3< T > Zero
math::Pose3<T>(0, 0, 0, 0, 0, 0)
Definition: Pose3.hh:36
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3::operator*
Pose3< T > operator*(const Pose3< T > &_pose) const
Multiplication operator.
Definition: Pose3.hh:218
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3::operator==
bool operator==(const Pose3< T > &_pose) const
Equality operator.
Definition: Pose3.hh:202
Vector3.hh
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Quaternion< T >
Quaternion.hh
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Quaternion::W
const T & W() const
Get the w component.
Definition: Quaternion.hh:948
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3::operator-
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:171
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3::Set
void Set(const Vector3< T > &_pos, const Vector3< T > &_rpy)
Set the pose from pos and rpy vectors.
Definition: Pose3.hh:100
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3::Pose3
Pose3(T _x, T _y, T _z, T _qw, T _qx, T _qy, T _qz)
Constructor.
Definition: Pose3.hh:71
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3::Set
void Set(const Vector3< T > &_pos, const Quaternion< T > &_rot)
Set the pose from a Vector3 and a Quaternion<T>
Definition: Pose3.hh:91
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3::operator-=
const Pose3< T > & operator-=(const Pose3< T > &_pose)
Subtraction operator.
Definition: Pose3.hh:191
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Quaternion::Correct
void Correct()
Correct any nan values in this quaternion.
Definition: Quaternion.hh:747
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Quaternion::Round
void Round(int _precision)
Round all values to _precision decimal places.
Definition: Quaternion.hh:822
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3::operator+
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:146
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3::RotatePositionAboutOrigin
Pose3< T > RotatePositionAboutOrigin(const Quaternion< T > &_q) const
Rotate vector part of a pose about the origin.
Definition: Pose3.hh:323
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3::CoordPoseSolve
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:299
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector3
The Vector3 class represents the generic vector containing 3 elements. Since it's commonly used to ke...
Definition: Vector3.hh:39
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Quaternion::Inverse
Quaternion< T > Inverse() const
Get the inverse of this quaternion.
Definition: Quaternion.hh:131
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3f
Pose3< float > Pose3f
Definition: Pose3.hh:411
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3::Pose3
Pose3(const Vector3< T > &_pos, const Quaternion< T > &_rot)
Constructor.
Definition: Pose3.hh:46
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3::operator!=
bool operator!=(const Pose3< T > &_pose) const
Inequality operator.
Definition: Pose3.hh:210
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3i
Pose3< int > Pose3i
Definition: Pose3.hh:409
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3::Rot
const Quaternion< T > & Rot() const
Get the rotation.
Definition: Pose3.hh:362
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3::IsFinite
bool IsFinite() const
See if a pose is finite (e.g., not nan)
Definition: Pose3.hh:120
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3::operator=
Pose3< T > & operator=(const Pose3< T > &_pose)
Equal operator.
Definition: Pose3.hh:225
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3::CoordRotationAdd
Quaternion< T > CoordRotationAdd(const Quaternion< T > &_rot) const
Add one rotation to another: result = this->q + rot.
Definition: Pose3.hh:280
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Quaternion::Normalize
void Normalize()
Normalize the quaternion.
Definition: Quaternion.hh:223
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Quaternion::X
const T & X() const
Get the x component.
Definition: Quaternion.hh:955
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3::operator<<
friend std::ostream & operator<<(std::ostream &_out, const ignition::math::Pose3< T > &_pose)
Stream insertion operator.
Definition: Pose3.hh:378
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Quaternion::Identity
static const Quaternion Identity
math::Quaternion(1, 0, 0, 0)
Definition: Quaternion.hh:40
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Quaternion::Euler
void Euler(const Vector3< T > &_vec)
Set the quaternion from Euler angles.
Definition: Quaternion.hh:303
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3::Correct
void Correct()
Fix any nan values.
Definition: Pose3.hh:126
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3::Pose3
Pose3(T _x, T _y, T _z, T _roll, T _pitch, T _yaw)
Constructor.
Definition: Pose3.hh:58
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3
Encapsulates a position and rotation in three space.
Definition: Pose3.hh:33
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3::Pos
Vector3< T > & Pos()
Get a mutable reference to the position.
Definition: Pose3.hh:355
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Quaternion::Z
const T & Z() const
Get the z component.
Definition: Quaternion.hh:969
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3::Pose3
Pose3(const Pose3< T > &_pose)
Copy constructor.
Definition: Pose3.hh:78
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3::Pose3
Pose3()
Default constructors.
Definition: Pose3.hh:39
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3::Set
void Set(T _x, T _y, T _z, T _roll, T _pitch, T _yaw)
Set the pose from a six tuple.
Definition: Pose3.hh:113
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3::operator-
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:182
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Quaternion::IsFinite
bool IsFinite() const
See if a quaternion is finite (e.g., not nan)
Definition: Quaternion.hh:736
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3::~Pose3
virtual ~Pose3()
Destructor.
Definition: Pose3.hh:84
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3::CoordPositionAdd
Vector3< T > CoordPositionAdd(const Vector3< T > &_pos) const
Add one point to a vector: result = this + pos.
Definition: Pose3.hh:235
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3::CoordRotationSub
Quaternion< T > CoordRotationSub(const Quaternion< T > &_rot) const
Subtract one rotation from another: result = this->q - rot.
Definition: Pose3.hh:288
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3d
Pose3< double > Pose3d
Definition: Pose3.hh:410
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3::operator+=
const Pose3< T > & operator+=(const Pose3< T > &_pose)
Add-Equals operator.
Definition: Pose3.hh:159
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector3::X
T X() const
Get the x value.
Definition: Vector3.hh:647
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector3::Z
T Z() const
Get the z value.
Definition: Vector3.hh:661
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3::Inverse
Pose3< T > Inverse() const
Get the inverse of this pose.
Definition: Pose3.hh:134
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector3::Y
T Y() const
Get the y value.
Definition: Vector3.hh:654