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