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
Vector3< T > CoordPositionSub(const Pose3< T > &_pose) const
Subtract one position from another: result = this - pose.
Definition: Pose3.hh:266
void Set(T _w, T _x, T _y, T _z)
Set this quaternion from 4 floating numbers.
Definition: Quaternion.hh:290
const Pose3< T > & operator+=(const Pose3< T > &_pose)
Add-Equals operator.
Definition: Pose3.hh:159
static const Quaternion Identity
math::Quaternion(1, 0, 0, 0)
Definition: Quaternion.hh:40
Quaternion< T > CoordRotationSub(const Quaternion< T > &_rot) const
Subtract one rotation from another: result = this->q - rot.
Definition: Pose3.hh:288
Pose3(T _x, T _y, T _z, T _qw, T _qx, T _qy, T _qz)
Constructor.
Definition: Pose3.hh:71
const Vector3< T > & Pos() const
Get the position.
Definition: Pose3.hh:348
void Correct()
Fix any nan values.
Definition: Pose3.hh:126
friend std::istream & operator>>(std::istream &_in, ignition::math::Pose3< T > &_pose)
Stream extraction operator.
Definition: Pose3.hh:389
Pose3< int > Pose3i
Definition: Pose3.hh:409
A quaternion class.
Definition: Matrix3.hh:33
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
T Y() const
Get the y value.
Definition: Vector3.hh:654
Pose3(const Vector3< T > &_pos, const Quaternion< T > &_rot)
Constructor.
Definition: Pose3.hh:46
bool operator==(const Pose3< T > &_pose) const
Equality operator.
Definition: Pose3.hh:202
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
const Pose3< T > & operator-=(const Pose3< T > &_pose)
Subtraction operator.
Definition: Pose3.hh:191
Encapsulates a position and rotation in three space.
Definition: Pose3.hh:33
const T & Z() const
Get the z component.
Definition: Quaternion.hh:969
void Set(const Vector3< T > &_pos, const Vector3< T > &_rpy)
Set the pose from pos and rpy vectors.
Definition: Pose3.hh:100
T Z() const
Get the z value.
Definition: Vector3.hh:661
Pose3< T > & operator=(const Pose3< T > &_pose)
Equal operator.
Definition: Pose3.hh:225
Pose3< float > Pose3f
Definition: Pose3.hh:411
Pose3< double > Pose3d
Definition: Pose3.hh:410
const T & W() const
Get the w component.
Definition: Quaternion.hh:948
Pose3()
Default constructors.
Definition: Pose3.hh:39
const T & Y() const
Get the y component.
Definition: Quaternion.hh:962
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
bool IsFinite() const
See if a pose is finite (e.g., not nan)
Definition: Pose3.hh:120
void Round(int _precision)
Round all values to _precision decimal places.
Definition: Pose3.hh:340
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:39
void Reset()
Reset the pose.
Definition: Pose3.hh:313
Vector3< T > CoordPositionAdd(const Vector3< T > &_pos) const
Add one point to a vector: result = this + pos.
Definition: Pose3.hh:235
Pose3(const Pose3< T > &_pose)
Copy constructor.
Definition: Pose3.hh:78
const T & X() const
Get the x component.
Definition: Quaternion.hh:955
T X() const
Get the x value.
Definition: Vector3.hh:647
Pose3< T > RotatePositionAboutOrigin(const Quaternion< T > &_q) const
Rotate vector part of a pose about the origin.
Definition: Pose3.hh:323
void Set(const Vector3< T > &_pos, const Quaternion< T > &_rot)
Set the pose from a Vector3 and a Quaternion<T>
Definition: Pose3.hh:91
Pose3(T _x, T _y, T _z, T _roll, T _pitch, T _yaw)
Constructor.
Definition: Pose3.hh:58
bool operator!=(const Pose3< T > &_pose) const
Inequality operator.
Definition: Pose3.hh:210
void Normalize()
Normalize the quaternion.
Definition: Quaternion.hh:223
Vector3< T > CoordPositionAdd(const Pose3< T > &_pose) const
Add one point to another: result = this + pose.
Definition: Pose3.hh:250
Quaternion< T > Inverse() const
Get the inverse of this quaternion.
Definition: Quaternion.hh:131
Definition: Angle.hh:39
Vector3< T > & Pos()
Get a mutable reference to the position.
Definition: Pose3.hh:355
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
Pose3< T > operator *(const Pose3< T > &_pose) const
Multiplication operator.
Definition: Pose3.hh:218
const Quaternion< T > & Rot() const
Get the rotation.
Definition: Pose3.hh:362
friend std::ostream & operator<<(std::ostream &_out, const ignition::math::Pose3< T > &_pose)
Stream insertion operator.
Definition: Pose3.hh:378
Quaternion< T > CoordRotationAdd(const Quaternion< T > &_rot) const
Add one rotation to another: result = this->q + rot.
Definition: Pose3.hh:280
Quaternion< T > & Rot()
Get a mutuable reference to the rotation.
Definition: Pose3.hh:369
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
virtual ~Pose3()
Destructor.
Definition: Pose3.hh:84
static const Pose3< T > Zero
math::Pose3<T>(0, 0, 0, 0, 0, 0)
Definition: Pose3.hh:36
Pose3< T > Inverse() const
Get the inverse of this pose.
Definition: Pose3.hh:134