All Classes Namespaces Files Functions Variables Typedefs Friends Macros Pages
Pose3.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2014 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_POSE_HH_
18 #define _IGNITION_POSE_HH_
19 
21 #include <ignition/math/Vector3.hh>
22 
23 namespace ignition
24 {
25  namespace math
26  {
29  template<typename T>
30  class Pose3
31  {
33  public: static const Pose3<T> Zero;
34 
36  public: Pose3() : p(0, 0, 0), q(1, 0, 0, 0)
37  {
38  }
39 
43  public: Pose3(const Vector3<T> &_pos, const Quaternion<T> &_rot)
44  : p(_pos), q(_rot)
45  {
46  }
47 
55  public: Pose3(T _x, T _y, T _z, T _roll, T _pitch, T _yaw)
56  : p(_x, _y, _z), q(_roll, _pitch, _yaw)
57  {
58  }
59 
68  public: Pose3(T _x, T _y, T _z, T _qw, T _qx, T _qy, T _qz)
69  : p(_x, _y, _z), q(_qw, _qx, _qy, _qz)
70  {
71  }
72 
75  public: Pose3(const Pose3<T> &_pose)
76  : p(_pose.p), q(_pose.q)
77  {
78  }
79 
81  public: virtual ~Pose3()
82  {
83  }
84 
88  public: void Set(const Vector3<T> &_pos, const Quaternion<T> &_rot)
89  {
90  this->p = _pos;
91  this->q = _rot;
92  }
93 
97  public: void Set(const Vector3<T> &_pos, const Vector3<T> &_rpy)
98  {
99  this->p = _pos;
100  this->q.Euler(_rpy);
101  }
102 
110  public: void Set(T _x, T _y, T _z, T _roll, T _pitch, T _yaw)
111  {
112  this->p.Set(_x, _y, _z);
113  this->q.Euler(math::Vector3<T>(_roll, _pitch, _yaw));
114  }
115 
117  public: bool IsFinite() const
118  {
119  return this->p.IsFinite() && this->q.IsFinite();
120  }
121 
123  public: inline void Correct()
124  {
125  this->p.Correct();
126  this->q.Correct();
127  }
128 
131  public: Pose3<T> Inverse() const
132  {
133  Quaternion<T> inv = this->q.Inverse();
134  return Pose3<T>(inv * (this->p*-1), inv);
135  }
136 
143  public: Pose3<T> operator+(const Pose3<T> &_pose) const
144  {
145  Pose3<T> result;
146 
147  result.p = this->CoordPositionAdd(_pose);
148  result.q = this->CoordRotationAdd(_pose.q);
149 
150  return result;
151  }
152 
156  public: const Pose3<T> &operator+=(const Pose3<T> &_pose)
157  {
158  this->p = this->CoordPositionAdd(_pose);
159  this->q = this->CoordRotationAdd(_pose.q);
160 
161  return *this;
162  }
163 
168  public: inline Pose3<T> operator-() const
169  {
170  return Pose3<T>() - *this;
171  }
172 
179  public: inline Pose3<T> operator-(const Pose3<T> &_pose) const
180  {
181  return Pose3<T>(this->CoordPositionSub(_pose),
182  this->CoordRotationSub(_pose.q));
183  }
184 
188  public: const Pose3<T> &operator-=(const Pose3<T> &_pose)
189  {
190  this->p = this->CoordPositionSub(_pose);
191  this->q = this->CoordRotationSub(_pose.q);
192 
193  return *this;
194  }
195 
199  public: bool operator==(const Pose3<T> &_pose) const
200  {
201  return this->p == _pose.p && this->q == _pose.q;
202  }
203 
207  public: bool operator!=(const Pose3<T> &_pose) const
208  {
209  return this->p != _pose.p || this->q != _pose.q;
210  }
211 
215  public: Pose3<T> operator*(const Pose3<T> &_pose)
216  {
217  return Pose3<T>(this->CoordPositionAdd(_pose), _pose.q * this->q);
218  }
219 
222  public: Pose3<T> &operator=(const Pose3<T> &_pose)
223  {
224  this->p = _pose.p;
225  this->q = _pose.q;
226  return *this;
227  }
228 
232  public: Vector3<T> CoordPositionAdd(const Vector3<T> &_pos) const
233  {
234  Quaternion<T> tmp(0.0, _pos.X(), _pos.Y(), _pos.Z());
235 
236  // result = pose.q + pose.q * this->p * pose.q!
237  tmp = this->q * (tmp * this->q.Inverse());
238 
239  return Vector3<T>(this->p.X() + tmp.X(),
240  this->p.Y() + tmp.Y(),
241  this->p.Z() + tmp.Z());
242  }
243 
247  public: Vector3<T> CoordPositionAdd(const Pose3<T> &_pose) const
248  {
249  Quaternion<T> tmp(static_cast<T>(0),
250  this->p.X(), this->p.Y(), this->p.Z());
251 
252  // result = _pose.q + _pose.q * this->p * _pose.q!
253  tmp = _pose.q * (tmp * _pose.q.Inverse());
254 
255  return Vector3<T>(_pose.p.X() + tmp.X(),
256  _pose.p.Y() + tmp.Y(),
257  _pose.p.Z() + tmp.Z());
258  }
259 
263  public: inline Vector3<T> CoordPositionSub(const Pose3<T> &_pose) const
264  {
265  Quaternion<T> tmp(0,
266  this->p.X() - _pose.p.X(),
267  this->p.Y() - _pose.p.Y(),
268  this->p.Z() - _pose.p.Z());
269 
270  tmp = _pose.q.Inverse() * (tmp * _pose.q);
271  return Vector3<T>(tmp.X(), tmp.Y(), tmp.Z());
272  }
273 
277  public: Quaternion<T> CoordRotationAdd(const Quaternion<T> &_rot) const
278  {
279  return Quaternion<T>(_rot * this->q);
280  }
281 
286  const Quaternion<T> &_rot) const
287  {
288  Quaternion<T> result(_rot.Inverse() * this->q);
289  result.Normalize();
290  return result;
291  }
292 
296  public: Pose3<T> CoordPoseSolve(const Pose3<T> &_b) const
297  {
298  Quaternion<T> qt;
299  Pose3<T> a;
300 
301  a.q = this->q.Inverse() * _b.q;
302  qt = a.q * Quaternion<T>(0, this->p.X(), this->p.Y(), this->p.Z());
303  qt = qt * a.q.Inverse();
304  a.p = _b.p - Vector3<T>(qt.X(), qt.Y(), qt.Z());
305 
306  return a;
307  }
308 
310  public: void Reset()
311  {
312  // set the position to zero
313  this->p.Set();
314  this->q = Quaterniond::Identity;
315  }
316 
321  {
322  Pose3<T> a = *this;
323  a.p.X((1.0 - 2.0*_q.Y()*_q.Y() - 2.0*_q.Z()*_q.Z()) * this->p.X()
324  +(2.0*(_q.X()*_q.Y()+_q.W()*_q.Z())) * this->p.Y()
325  +(2.0*(_q.X()*_q.Z()-_q.W()*_q.Y())) * this->p.Z());
326  a.p.Y((2.0*(_q.X()*_q.Y()-_q.W()*_q.Z())) * this->p.X()
327  +(1.0 - 2.0*_q.X()*_q.X() - 2.0*_q.Z()*_q.Z()) * this->p.Y()
328  +(2.0*(_q.Y()*_q.Z()+_q.W()*_q.X())) * this->p.Z());
329  a.p.Z((2.0*(_q.X()*_q.Z()+_q.W()*_q.Y())) * this->p.X()
330  +(2.0*(_q.Y()*_q.Z()-_q.W()*_q.X())) * this->p.Y()
331  +(1.0 - 2.0*_q.X()*_q.X() - 2.0*_q.Y()*_q.Y()) * this->p.Z());
332  return a;
333  }
334 
337  public: void Round(int _precision)
338  {
339  this->q.Round(_precision);
340  this->p.Round(_precision);
341  }
342 
345  public: inline const Vector3<T> &Pos() const
346  {
347  return this->p;
348  }
349 
352  public: inline Vector3<T> &Pos()
353  {
354  return this->p;
355  }
356 
359  public: inline const Quaternion<T> &Rot() const
360  {
361  return this->q;
362  }
363 
366  public: inline Quaternion<T> &Rot()
367  {
368  return this->q;
369  }
370 
375  public: friend std::ostream &operator<<(
376  std::ostream &_out, const ignition::math::Pose3<T> &_pose)
377  {
378  _out << _pose.Pos() << " " << _pose.Rot();
379  return _out;
380  }
381 
386  public: friend std::istream &operator>>(
387  std::istream &_in, ignition::math::Pose3<T> &_pose)
388  {
389  // Skip white spaces
390  _in.setf(std::ios_base::skipws);
391  Vector3<T> pos;
392  Quaternion<T> rot;
393  _in >> pos >> rot;
394  _pose.Set(pos, rot);
395  return _in;
396  }
397 
399  private: Vector3<T> p;
400 
402  private: Quaternion<T> q;
403  };
404  template<typename T> const Pose3<T> Pose3<T>::Zero(0, 0, 0, 0, 0, 0);
405 
409  }
410 }
411 #endif
void Set(const Vector3< T > &_pos, const Vector3< T > &_rpy)
Set the pose from pos and rpy vectors.
Definition: Pose3.hh:97
Pose3(T _x, T _y, T _z, T _qw, T _qx, T _qy, T _qz)
Constructor.
Definition: Pose3.hh:68
Pose3()
Default constructors.
Definition: Pose3.hh:36
bool operator==(const Pose3< T > &_pose) const
Equality operator.
Definition: Pose3.hh:199
bool IsFinite() const
See if a pose is finite (e.g., not nan)
Definition: Pose3.hh:117
Quaternion< T > CoordRotationSub(const Quaternion< T > &_rot) const
Subtract one rotation from another: result = this->q - rot.
Definition: Pose3.hh:285
Quaternion< T > Inverse() const
Get the inverse of this quaternion.
Definition: Quaternion.hh:114
Encapsulates a position and rotation in three space.
Definition: Pose3.hh:30
const Pose3< T > & operator-=(const Pose3< T > &_pose)
Subtraction operator.
Definition: Pose3.hh:188
const Pose3< T > & operator+=(const Pose3< T > &_pose)
Add-Equals operator.
Definition: Pose3.hh:156
const T & Y() const
Get the y component.
Definition: Quaternion.hh:750
void Reset()
Reset the pose.
Definition: Pose3.hh:310
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:179
void Round(int _precision)
Round all values to _precision decimal places.
Definition: Pose3.hh:337
friend std::istream & operator>>(std::istream &_in, ignition::math::Pose3< T > &_pose)
Stream extraction operator.
Definition: Pose3.hh:386
T X() const
Get the x value.
Definition: Vector3.hh:545
const T & Z() const
Get the z component.
Definition: Quaternion.hh:757
friend std::ostream & operator<<(std::ostream &_out, const ignition::math::Pose3< T > &_pose)
Stream insertion operator.
Definition: Pose3.hh:375
Pose3(const Vector3< T > &_pos, const Quaternion< T > &_rot)
Constructor.
Definition: Pose3.hh:43
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:168
void Set(T _x, T _y, T _z, T _roll, T _pitch, T _yaw)
Set the pose from a six tuple.
Definition: Pose3.hh:110
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:143
Pose3< double > Pose3d
Definition: Pose3.hh:407
Vector3< T > CoordPositionSub(const Pose3< T > &_pose) const
Subtract one position from another: result = this - pose.
Definition: Pose3.hh:263
bool operator!=(const Pose3< T > &_pose) const
Inequality operator.
Definition: Pose3.hh:207
T Y() const
Get the y value.
Definition: Vector3.hh:552
virtual ~Pose3()
Destructor.
Definition: Pose3.hh:81
Pose3< int > Pose3i
Definition: Pose3.hh:406
static const Quaternion Identity
math::Quaternion(1, 0, 0, 1)
Definition: Quaternion.hh:34
Vector3< T > & Pos()
Get a mutable reference to the position.
Definition: Pose3.hh:352
Vector3< T > CoordPositionAdd(const Vector3< T > &_pos) const
Add one point to a vector: result = this + pos.
Definition: Pose3.hh:232
Pose3< T > operator*(const Pose3< T > &_pose)
Multiplication operator.
Definition: Pose3.hh:215
T Z() const
Get the z value.
Definition: Vector3.hh:559
Quaternion< T > & Rot()
Get a mutuable reference to the rotation.
Definition: Pose3.hh:366
const Quaternion< T > & Rot() const
Get the rotation.
Definition: Pose3.hh:359
Pose3(T _x, T _y, T _z, T _roll, T _pitch, T _yaw)
Constructor.
Definition: Pose3.hh:55
void Set(const Vector3< T > &_pos, const Quaternion< T > &_rot)
Set the pose from a Vector3 and a Quaternion<T>
Definition: Pose3.hh:88
Pose3(const Pose3< T > &_pose)
Copy constructor.
Definition: Pose3.hh:75
Pose3< T > Inverse() const
Get the inverse of this pose.
Definition: Pose3.hh:131
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:296
Pose3< T > RotatePositionAboutOrigin(const Quaternion< T > &_q) const
Rotate vector part of a pose about the origin.
Definition: Pose3.hh:320
void Correct()
Fix any nan values.
Definition: Pose3.hh:123
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:37
Quaternion< T > CoordRotationAdd(const Quaternion< T > &_rot) const
Add one rotation to another: result = this->q + rot.
Definition: Pose3.hh:277
Vector3< T > CoordPositionAdd(const Pose3< T > &_pose) const
Add one point to another: result = this + pose.
Definition: Pose3.hh:247
static const Pose3< T > Zero
math::Pose3<T>(0, 0, 0, 0, 0, 0)
Definition: Pose3.hh:33
const T & W() const
Get the w component.
Definition: Quaternion.hh:736
Pose3< float > Pose3f
Definition: Pose3.hh:408
void Normalize()
Normalize the quaternion.
Definition: Quaternion.hh:206
A quaternion class.
Definition: Quaternion.hh:31
Pose3< T > & operator=(const Pose3< T > &_pose)
Equal operator.
Definition: Pose3.hh:222
const Vector3< T > & Pos() const
Get the position.
Definition: Pose3.hh:345
const T & X() const
Get the x component.
Definition: Quaternion.hh:743