Ocean
Loading...
Searching...
No Matches
Pose.h
Go to the documentation of this file.
1/*
2 * Copyright (c) Meta Platforms, Inc. and affiliates.
3 *
4 * This source code is licensed under the MIT license found in the
5 * LICENSE file in the root directory of this source tree.
6 */
7
8#ifndef META_OCEAN_MATH_POSE_H
9#define META_OCEAN_MATH_POSE_H
10
11#include "ocean/math/Math.h"
12
13#include "ocean/math/Euler.h"
16#include "ocean/math/Rotation.h"
18#include "ocean/math/Vector3.h"
19
20namespace Ocean
21{
22
23// Forward declaration.
24template <typename T> class PoseT;
25
26/**
27 * Definition of the Pose object, depending on the OCEAN_MATH_USE_SINGLE_PRECISION flag either with single or double precision float data type.
28 * @see PoseT
29 * @ingroup math
30 */
32
33/**
34 * Instantiation of the ExponentialMapT template class using a double precision float data type.
35 * @see ExponentialMapT
36 * @ingroup math
37 */
39
40/**
41 * Instantiation of the ExponentialMapT template class using a single precision float data type.
42 * @see ExponentialMapT
43 * @ingroup math
44 */
46
47/**
48 * Definition of a typename alias for vectors with PoseT objects.
49 * @see PoseT
50 * @ingroup math
51 */
52template <typename T>
53using PosesT = std::vector<PoseT<T>>;
54
55/**
56 * Definition of a vector holding ExponentialMap objects.
57 * @see ExponentialMap
58 * @ingroup math
59 */
60using Poses = std::vector<Pose>;
61
62/**
63 * This class implements a camera pose with six degrees of freedom (6-DOF).
64 * Three degrees for the translation or position and three for the orientation or rotation.<br>
65 * This object stores six elements. The first three elements define the translation.<br>
66 * The last three elements define the orientation as exponential map (rotation axis and angle as axis length).<br>
67 * The element order is defined as: (Tx, Ty, Tz, Rx, Ry, Rz).
68 * @tparam T The scalar floating point data type to be used, either 'float' or 'double'
69 * @ingroup math
70 */
71template <typename T>
72class PoseT
73{
74 public:
75
76 /**
77 * Creates a new pose object with default values (no translation and no rotation).
78 */
80
81 /**
82 * Copies a pose with different element data type than T.
83 * @param pose The pose object to be copied
84 * @tparam U The element data type of the given pose
85 */
86 template <typename U>
87 explicit inline PoseT(const PoseT<U>& pose);
88
89 /**
90 * Creates a new pose object with a translation only.
91 * @param translation 3D vector defining the translation of the pose object
92 */
93 explicit PoseT(const VectorT3<T>& translation);
94
95 /**
96 * Creates a new pose object with a rotation component on.
97 * @param euler The Euler rotation defining the rotation of the pose object
98 */
99 explicit PoseT(const EulerT<T>& euler);
100
101 /**
102 * Creates a new pose object with a rotation component on.
103 * @param quaternion Unit quaternion rotation defining the rotation of the pose object
104 */
105 explicit PoseT(const QuaternionT<T>& quaternion);
106
107 /**
108 * Creates a new pose object with a rotation component on.
109 * @param rotation Angle-axis rotation defining the rotation of the pose object
110 */
111 explicit PoseT(const RotationT<T>& rotation);
112
113 /**
114 * Creates a new pose object by a specified 4x4 homogeneous transformation matrix.
115 * @param transformation Matrix defining the position and rotation of the new pose
116 */
118
119 /**
120 * Creates a new pose by at least six pose values.
121 * The specified pose must have the following order: (Tx, Ty, Tz, Rx, Ry, Rz).<br>
122 * With (Tx, Ty, Tz) as translation vector, (Rx, Ry, Rz) as rotation axis and sqrt(Rx * Rx + Ry * Ry + Rz * Rz) as rotation angle.
123 * @param values The six pose values specifying the new pose, must be valid
124 */
125 explicit PoseT(const T* values);
126
127 /**
128 * Creates a new pose by six pose parameters.
129 * @param tx Translation value for the x-axis
130 * @param ty Translation value for the y-axis
131 * @param tz Translation value for the z-axis
132 * @param rx X parameter of the rotation
133 * @param ry Y parameter of the rotation
134 * @param rz Z parameter of the rotation
135 */
136 PoseT(const T tx, const T ty, const T tz, const T rx, const T ry, const T rz);
137
138 /**
139 * Creates a new pose object.
140 * @param translation 3D vector defining the translation component of the pose object
141 * @param euler The Euler rotation defining the rotation component of the pose object
142 */
143 PoseT(const VectorT3<T>& translation, const EulerT<T>& euler);
144
145 /**
146 * Creates a new pose object.
147 * @param translation 3D vector defining the translation component of the pose object
148 * @param quaternion Unit quaternion rotation defining the rotation component of the pose object
149 */
150 PoseT(const VectorT3<T>& translation, const QuaternionT<T>& quaternion);
151
152 /**
153 * Creates a new pose object.
154 * @param translation 3D vector defining the translation component of the pose object
155 * @param rotation Angle-axis rotation defining the rotation component of the pose object
156 */
157 PoseT(const VectorT3<T>& translation, const RotationT<T>& rotation);
158
159 /**
160 * Returns the translation of this pose.
161 * @return 3D translation vector
162 */
163 inline VectorT3<T> translation() const;
164
165 /**
166 * Returns the orientation of this pose.
167 * @return Unit quaternion defining the pose orientation
168 */
170
171 /**
172 * Returns the 4x4 homogeneous transformation matrix of this pose.
173 * @return Transformation matrix
174 */
176
177 /**
178 * Returns the translation value for the x-axis.
179 * @return Translation value for x-axis
180 */
181 inline T x() const;
182
183 /**
184 * Returns the translation value for the x-axis.
185 * @return Translation value for x-axis
186 */
187 inline T& x();
188
189 /**
190 * Returns the translation value for the y-axis.
191 * @return Translation value for y-axis
192 */
193 inline T y() const;
194
195 /**
196 * Returns the translation value for the y-axis.
197 * @return Translation value for y-axis
198 */
199 inline T& y();
200
201 /**
202 * Returns the translation value for the z-axis.
203 * @return Translation value for z-axis
204 */
205 inline T z() const;
206
207 /**
208 * Returns the translation value for the z-axis.
209 * @return Translation value for z-axis
210 */
211 inline T& z();
212
213 /**
214 * Returns the x parameter of the rotation.
215 * @return Rotation x parameter
216 */
217 inline T rx() const;
218
219 /**
220 * Returns the x parameter of the rotation.
221 * @return Rotation x parameter
222 */
223 inline T& rx();
224
225 /**
226 * Returns the y parameter of the rotation.
227 * @return Rotation y parameter
228 */
229 inline T ry() const;
230
231 /**
232 * Returns the y parameter of the rotation.
233 * @return Rotation y parameter
234 */
235 inline T& ry();
236
237 /**
238 * Returns the z parameter of the rotation.
239 * @return Rotation z parameter
240 */
241 inline T rz() const;
242
243 /**
244 * Returns the z parameter of the rotation.
245 * @return Rotation z parameter
246 */
247 inline T& rz();
248
249 /**
250 * Returns the angle of the pose rotation.
251 * @return Rotation angle in radian
252 */
253 T angle() const;
254
255 /**
256 * Returns whether this pose holds no translation and no rotation.
257 * @return True, if so
258 */
259 bool isNull() const;
260
261 /**
262 * Data access operator.
263 * @return Pointer to the internal elements.
264 */
265 inline const T* data() const;
266
267 /**
268 * Data access operator.
269 * @return Pointer to the internal elements.
270 */
271 inline T* data();
272
273 /**
274 * Returns whether two poses are identical up to a small epsilon.
275 * @param pose Right pose to compare
276 * @return True, if so
277 */
278 inline bool operator==(const PoseT<T>& pose) const;
279
280 /**
281 * Returns whether two poses are not identical up to a small epsilon.
282 * @param pose Right pose to compare
283 * @return True, if so
284 */
285 inline bool operator!=(const PoseT<T>& pose) const;
286
287 /**
288 * Returns a new pose created by the element wise sum of two poses.
289 * @param pose Second pose to add
290 * @return New pose
291 */
292 PoseT<T> operator+(const PoseT<T>& pose) const;
293
294 /**
295 * Adds element wise the six values of a second pose to this pose.
296 * @param pose Second pose to add the six values
297 * @return Reference to this object
298 */
300
301 /**
302 * Returns a new pose created by the element wise subtraction of two poses.
303 * @param pose Second pose to subtract
304 * @return New pose
305 */
306 PoseT<T> operator-(const PoseT<T>& pose) const;
307
308 /**
309 * Subtracts element wise the six values of a second pose from this pose.
310 * @param pose Second pose to subtract the six values
311 * @return Reference to this object
312 */
314
315 /**
316 * Multiplies this pose by a scalar value and returns the new result.
317 * The multiplication is done element wise.
318 * @param value Scalar value for multiplication
319 * @return New resulting pose
320 */
321 PoseT<T> operator*(const T value) const;
322
323 /**
324 * Multiplies this pose by a scalar.
325 * The multiplication is done element wise.
326 * @param value Scalar value for multiplication
327 * @return Reference to this object
328 */
329 PoseT<T>& operator*=(const T value);
330
331 /**
332 * Element access operator.
333 * Beware: No range check will be done!
334 * @param index The index of the element to return, with range [0, 5]
335 * @return Internal element
336 */
337 inline T operator()(unsigned int index) const;
338
339 /**
340 * Element access operator.
341 * Beware: No range check will be done!
342 * @param index The index of the element to return, with range [0, 5]
343 * @return Internal element
344 */
345 inline T& operator()(unsigned int index);
346
347 /**
348 * Element access operator.
349 * Beware: No range check will be done!
350 * @param index The index of the element to return, with range [0, 5]
351 * @return Internal element
352 */
353 inline T operator[](unsigned int index) const;
354
355 /**
356 * Element access operator.
357 * Beware: No range check will be done!
358 * @param index The index of the element to return, with range [0, 5]
359 * @return Internal element
360 */
361 inline T& operator[](unsigned int index);
362
363 /**
364 * Access operator.
365 * @return Pointer to the internal elements.
366 */
367 inline const T* operator()() const;
368
369 /**
370 * Access operator.
371 * @return Pointer to the internal elements.
372 */
373 inline T* operator()();
374
375 /**
376 * Determines a subset of a set of given poses best representing the entire set of poses.
377 * @param poses The entire set of given poses from which a subset is extracted, must be valid
378 * @param subsetSize The number of poses in the resulting subset, with range [1, poses.size()]
379 * @param explicitIndexOffset The explicit offset which is added to each index of the resulting (pose) subset allowing to add an explicit index shift
380 * @return The indices of the poses defining the subset
381 */
382 static Indices32 representativePoses(const PosesT<T>& poses, const size_t subsetSize, const size_t explicitIndexOffset = 0);
383
384 /**
385 * Determines a subset of a set of given poses (defined as matrices) best representing the entire set of poses.
386 * @param poseMatrices The entire set of given pose matrices from which a subset is extracted, must be valid
387 * @param size The number of provided set of pose matrices, with range [1, infinity)
388 * @param subsetSize The number of poses in the resulting subset, with range [1, size]
389 * @param explicitIndexOffset The explicit offset which is added to each index of the resulting (pose) subset allowing to add an explicit index shift
390 * @return The indices of the poses defining the subset
391 */
392 static Indices32 representativePoses(const HomogenousMatrixT4<T>* poseMatrices, const size_t size, const size_t subsetSize, const size_t explicitIndexOffset = 0);
393
394 /**
395 * Interpolates two camera poses by a linear interpolation.
396 * @param pose0 The first camera pose
397 * @param pose1 The second camera pose
398 * @param factor The interpolation factor which is applied as follows: pose0 * (1 - factor) + pose1 * factor, with range [0, 1]
399 * @return The resulting interpolated camera pose
400 */
402
403 protected:
404
405 /// The six values of the pose, with element order: (Tx, Ty, Tz, Rx, Ry, Rz).
407};
408
409template <typename T>
410template <typename U>
411inline PoseT<T>::PoseT(const PoseT<U>& pose)
412{
413 values_[0] = T(pose[0]);
414 values_[1] = T(pose[1]);
415 values_[2] = T(pose[2]);
416 values_[3] = T(pose[3]);
417 values_[4] = T(pose[4]);
418 values_[5] = T(pose[5]);
419}
420
421template <typename T>
423{
424 return VectorT3<T>(values_);
425}
426
427template <typename T>
428inline T PoseT<T>::x() const
429{
430 return values_[0];
431}
432
433template <typename T>
434inline T& PoseT<T>::x()
435{
436 return values_[0];
437}
438
439template <typename T>
440inline T PoseT<T>::y() const
441{
442 return values_[1];
443}
444
445template <typename T>
446inline T& PoseT<T>::y()
447{
448 return values_[1];
449}
450
451template <typename T>
452inline T PoseT<T>::z() const
453{
454 return values_[2];
455}
456
457template <typename T>
458inline T& PoseT<T>::z()
459{
460 return values_[2];
461}
462
463template <typename T>
464inline T PoseT<T>::rx() const
465{
466 return values_[3];
467}
468
469template <typename T>
470inline T& PoseT<T>::rx()
471{
472 return values_[3];
473}
474
475template <typename T>
476inline T PoseT<T>::ry() const
477{
478 return values_[4];
479}
480
481template <typename T>
482inline T& PoseT<T>::ry()
483{
484 return values_[4];
485}
486
487template <typename T>
488inline T PoseT<T>::rz() const
489{
490 return values_[5];
491}
492
493template <typename T>
494inline T& PoseT<T>::rz()
495{
496 return values_[5];
497}
498
499template <typename T>
500inline bool PoseT<T>::operator==(const PoseT<T>& pose) const
501{
502 return NumericT<T>::isEqual(values_[0], pose.values_[0]) && NumericT<T>::isEqual(values_[1], pose.values_[1]) && NumericT<T>::isEqual(values_[2], pose.values_[2]) && orientation() == pose.orientation();
503}
504
505template <typename T>
506inline bool PoseT<T>::operator!=(const PoseT<T>& pose) const
507{
508 return !(*this == pose);
509}
510
511template <typename T>
512inline T PoseT<T>::operator()(unsigned int index) const
513{
514 ocean_assert(index < 6);
515 return values_[index];
516}
517
518template <typename T>
519inline T& PoseT<T>::operator()(unsigned int index)
520{
521 ocean_assert(index < 6);
522 return values_[index];
523}
524
525template <typename T>
526inline T PoseT<T>::operator[](unsigned int index) const
527{
528 ocean_assert(index < 6);
529 return values_[index];
530}
531
532template <typename T>
533inline T& PoseT<T>::operator[](unsigned int index)
534{
535 ocean_assert(index < 6);
536 return values_[index];
537}
538
539template <typename T>
540inline const T* PoseT<T>::operator()() const
541{
542 return values_;
543}
544
545template <typename T>
547{
548 return values_;
549}
550
551template <typename T>
552inline const T* PoseT<T>::data() const
553{
554 return values_;
555}
556
557template <typename T>
558inline T* PoseT<T>::data()
559{
560 return values_;
561}
562
563}
564
565#endif // META_OCEAN_MATH_POSE_H
This class implements an euler rotation with angles: yaw, pitch and roll.
Definition Euler.h:80
This class implements a 4x4 homogeneous transformation matrix using floating point values with the pr...
Definition HomogenousMatrix4.h:110
static bool isEqual(const T first, const T second)
Returns whether two values are equal up to a small epsilon.
Definition Numeric.h:2386
PoseT(const HomogenousMatrixT4< T > &transformation)
Creates a new pose object by a specified 4x4 homogeneous transformation matrix.
T & rx()
Returns the x parameter of the rotation.
Definition Pose.h:470
PoseT(const VectorT3< T > &translation, const QuaternionT< T > &quaternion)
Creates a new pose object.
PoseT< T > & operator-=(const PoseT< T > &pose)
Subtracts element wise the six values of a second pose from this pose.
T operator[](unsigned int index) const
Element access operator.
Definition Pose.h:526
T values_[6]
The six values of the pose, with element order: (Tx, Ty, Tz, Rx, Ry, Rz).
Definition Pose.h:406
VectorT3< T > translation() const
Returns the translation of this pose.
Definition Pose.h:422
T x() const
Returns the translation value for the x-axis.
Definition Pose.h:428
T rx() const
Returns the x parameter of the rotation.
Definition Pose.h:464
bool isNull() const
Returns whether this pose holds no translation and no rotation.
T * operator()()
Access operator.
Definition Pose.h:546
PoseT()
Creates a new pose object with default values (no translation and no rotation).
PoseT(const VectorT3< T > &translation, const RotationT< T > &rotation)
Creates a new pose object.
T operator()(unsigned int index) const
Element access operator.
Definition Pose.h:512
HomogenousMatrixT4< T > transformation() const
Returns the 4x4 homogeneous transformation matrix of this pose.
T & operator()(unsigned int index)
Element access operator.
Definition Pose.h:519
static Indices32 representativePoses(const HomogenousMatrixT4< T > *poseMatrices, const size_t size, const size_t subsetSize, const size_t explicitIndexOffset=0)
Determines a subset of a set of given poses (defined as matrices) best representing the entire set of...
T & operator[](unsigned int index)
Element access operator.
Definition Pose.h:533
T y() const
Returns the translation value for the y-axis.
Definition Pose.h:440
QuaternionT< T > orientation() const
Returns the orientation of this pose.
T & y()
Returns the translation value for the y-axis.
Definition Pose.h:446
T * data()
Data access operator.
Definition Pose.h:558
PoseT(const PoseT< U > &pose)
Copies a pose with different element data type than T.
Definition Pose.h:411
PoseT< T > & operator*=(const T value)
Multiplies this pose by a scalar.
T angle() const
Returns the angle of the pose rotation.
T & z()
Returns the translation value for the z-axis.
Definition Pose.h:458
PoseT(const EulerT< T > &euler)
Creates a new pose object with a rotation component on.
const T * data() const
Data access operator.
Definition Pose.h:552
bool operator!=(const PoseT< T > &pose) const
Returns whether two poses are not identical up to a small epsilon.
Definition Pose.h:506
PoseT(const VectorT3< T > &translation, const EulerT< T > &euler)
Creates a new pose object.
bool operator==(const PoseT< T > &pose) const
Returns whether two poses are identical up to a small epsilon.
Definition Pose.h:500
PoseT(const QuaternionT< T > &quaternion)
Creates a new pose object with a rotation component on.
const T * operator()() const
Access operator.
Definition Pose.h:540
PoseT(const VectorT3< T > &translation)
Creates a new pose object with a translation only.
PoseT< T > & operator+=(const PoseT< T > &pose)
Adds element wise the six values of a second pose to this pose.
PoseT(const RotationT< T > &rotation)
Creates a new pose object with a rotation component on.
PoseT< T > operator*(const T value) const
Multiplies this pose by a scalar value and returns the new result.
T & ry()
Returns the y parameter of the rotation.
Definition Pose.h:482
T & rz()
Returns the z parameter of the rotation.
Definition Pose.h:494
T rz() const
Returns the z parameter of the rotation.
Definition Pose.h:488
static HomogenousMatrixT4< T > linearPoseInterpolation(const HomogenousMatrixT4< T > &pose0, const HomogenousMatrixT4< T > &pose1, const T &factor)
Interpolates two camera poses by a linear interpolation.
T ry() const
Returns the y parameter of the rotation.
Definition Pose.h:476
PoseT(const T *values)
Creates a new pose by at least six pose values.
PoseT< T > operator-(const PoseT< T > &pose) const
Returns a new pose created by the element wise subtraction of two poses.
T z() const
Returns the translation value for the z-axis.
Definition Pose.h:452
PoseT< T > operator+(const PoseT< T > &pose) const
Returns a new pose created by the element wise sum of two poses.
static Indices32 representativePoses(const PosesT< T > &poses, const size_t subsetSize, const size_t explicitIndexOffset=0)
Determines a subset of a set of given poses best representing the entire set of poses.
T & x()
Returns the translation value for the x-axis.
Definition Pose.h:434
PoseT(const T tx, const T ty, const T tz, const T rx, const T ry, const T rz)
Creates a new pose by six pose parameters.
This class implements a unit quaternion rotation.
Definition Quaternion.h:100
This class implements a axis-angle rotation using floating point values.
Definition Rotation.h:79
This class implements a vector with three elements.
Definition Vector3.h:97
std::vector< Index32 > Indices32
Definition of a vector holding 32 bit index values.
Definition Base.h:96
std::vector< PoseT< T > > PosesT
Definition of a typename alias for vectors with PoseT objects.
Definition Pose.h:53
std::vector< Pose > Poses
Definition of a vector holding ExponentialMap objects.
Definition Pose.h:60
The namespace covering the entire Ocean framework.
Definition Accessor.h:15