Kindr
Kinematics and Dynamics for Robotics
AngleAxis.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  * * Redistributions of source code must retain the above copyright
8  * notice, this list of conditions and the following disclaimer.
9  * * Redistributions in binary form must reproduce the above copyright
10  * notice, this list of conditions and the following disclaimer in the
11  * documentation and/or other materials provided with the distribution.
12  * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the
13  * names of its contributors may be used to endorse or promote products
14  * derived from this software without specific prior written permission.
15  *
16  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19  * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale,
20  * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
21  * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
22  * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
23  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
24  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26  *
27 */
28 
29 #pragma once
30 
31 #include <cmath>
32 
33 #include <Eigen/Geometry>
34 
35 #include "kindr/common/common.hpp"
38 
39 namespace kindr {
40 
41 
53 template<typename PrimType_>
54 class AngleAxis : public RotationBase<AngleAxis<PrimType_>> {
55  private:
58  typedef Eigen::AngleAxis<PrimType_> Base;
59 
62  Base angleAxis_;
63 
64  public:
68  typedef Base Implementation;
72  typedef PrimType_ Scalar;
75  typedef Eigen::Matrix<PrimType_, 3, 1> Vector3;
76 
79  typedef Eigen::Matrix<PrimType_, 4, 1> Vector4;
80 
84  : angleAxis_(Base::Identity()) {
85  }
86 
94  AngleAxis(Scalar angle, Scalar v1, Scalar v2, Scalar v3)
95  : angleAxis_(angle,Vector3(v1,v2,v3)) {
96  KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, this->axis().norm(), static_cast<Scalar>(1), static_cast<Scalar>(1e-4), "Input rotation axis has not unit length.");
97  }
98 
104  AngleAxis(Scalar angle, const Vector3& axis)
105  : angleAxis_(angle,axis) {
106  KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, this->axis().norm(), static_cast<Scalar>(1), static_cast<Scalar>(1e-4), "Input rotation axis has not unit length.");
107  }
108 
109 
114  AngleAxis(const Vector4& vector)
115  : angleAxis_(vector(0),vector.template block<3,1>(1,0)) {
116  KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, this->axis().norm(), static_cast<Scalar>(1), static_cast<Scalar>(1e-4), "Input rotation axis has not unit length.");
117  }
118 
123  explicit AngleAxis(const Base& other) // explicit on purpose
124  : angleAxis_(other) {
125  KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, this->axis().norm(), static_cast<Scalar>(1), static_cast<Scalar>(1e-4), "Input rotation axis has not unit length.");
126  }
127 
131  template<typename OtherDerived_>
132  inline explicit AngleAxis(const RotationBase<OtherDerived_>& other)
133  : angleAxis_(internal::ConversionTraits<AngleAxis, OtherDerived_>::convert(other.derived()).toImplementation()) {
134  }
135 
140  template<typename OtherDerived_>
142  this->toImplementation() = internal::ConversionTraits<AngleAxis, OtherDerived_>::convert(other.derived()).toImplementation();
143  return *this;
144  }
145 
150  template<typename OtherDerived_>
152  this->toImplementation() = internal::ConversionTraits<AngleAxis, OtherDerived_>::convert(other.derived()).toImplementation();
153  return *this;
154  }
155 
159  AngleAxis inverted() const {
160  Base inverse = this->toImplementation().inverse();
161  inverse.angle() = -inverse.angle();
162  inverse.axis() = -inverse.axis();
163  return AngleAxis(inverse);
164  }
165 
170  *this = this->inverted();
171  return *this;
172  }
173 
177  inline Implementation& toImplementation() {
178  return static_cast<Implementation&>(angleAxis_);
179  }
180 
184  inline const Implementation& toImplementation() const {
185  return static_cast<const Implementation&>(angleAxis_);
186  }
187 
191  inline Scalar angle() const {
192  return angleAxis_.angle();
193  }
194 
197  inline void setAngle(Scalar angle) {
198  angleAxis_.angle() = angle;
199  }
200 
204  inline const Vector3& axis() const {
205  return angleAxis_.axis();
206  }
207 
210  inline void setAxis(const Vector3& axis) {
211  angleAxis_.axis() = axis;
212  KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, this->axis().norm(), static_cast<Scalar>(1), static_cast<Scalar>(1e-4), "Input rotation axis has not unit length.");
213  }
214 
217  inline void setAxis(Scalar v1, Scalar v2, Scalar v3) {
218  angleAxis_.axis() = Vector3(v1,v2,v3);
219  KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, this->axis().norm(), static_cast<Scalar>(1), static_cast<Scalar>(1e-4), "Input rotation axis has not unit length.");
220  }
221 
224  inline void setVector(const Vector4& vector) {
225  this->setAngle(vector(0));
226  this->setAxis(vector.template block<3,1>(1,0));
227  }
228 
231  inline Vector4 vector() const {
232  Vector4 vector;
233  vector(0) = angle();
234  vector.template block<3,1>(1,0) = axis();
235  return vector;
236  }
237 
238 
243  this->setAngle(static_cast<Scalar>(0));
244  this->setAxis(static_cast<Scalar>(1), static_cast<Scalar>(0), static_cast<Scalar>(0));
245  return *this;
246  }
247 
253  const Scalar anglePosNegPi = kindr::wrapPosNegPI(angle());
254  AngleAxis aa(anglePosNegPi, axis()); // first wraps angle into [-pi,pi)
255  if(aa.angle() > 0.0) {
256  return aa;
257  }
258  else if(aa.angle() < 0.0) {
259  if(aa.angle() != -M_PI) {
260  return AngleAxis(-aa.angle(),-aa.axis());
261  }
262  else { // angle == -pi, so axis must be viewed further, because -pi,axis does the same as -pi,-axis
263  if(aa.axis()[0] < 0.0) {
264  return AngleAxis(-aa.angle(),-aa.axis());
265  }
266  else if(aa.axis()[0] > 0) {
267  return AngleAxis(-aa.angle(),aa.axis());
268  }
269  else { // v1 == 0
270 
271  if(aa.axis()[1] < 0.0) {
272  return AngleAxis(-aa.angle(),-aa.axis());
273  }
274  else if(aa.axis()[1] > 0) {
275  return AngleAxis(-aa.angle(),aa.axis());
276  }
277  else { // v2 == 0
278 
279  if(aa.axis()[2] < 0.0) { // v3 must be -1 or 1
280  return AngleAxis(-aa.angle(),-aa.axis());
281  }
282  else {
283  return AngleAxis(-aa.angle(),aa.axis());
284  }
285  }
286  }
287  }
288  }
289  else { // angle == 0
290  return AngleAxis();
291  }
292  }
293 
298  *this = getUnique();
299  return *this;
300  }
301 
306  using RotationBase<AngleAxis<PrimType_>>::operator*;
307 
311  friend std::ostream& operator << (std::ostream& out, const AngleAxis& a) {
312  out << a.angle() << ", " << a.axis().transpose();
313  return out;
314  }
315 };
316 
325 
326 
327 
328 namespace internal {
329 
330 template<typename PrimType_>
331 class get_scalar<AngleAxis<PrimType_>> {
332  public:
333  typedef PrimType_ Scalar;
334 };
335 
336 template<typename PrimType_>
337 class get_matrix3X<AngleAxis<PrimType_>>{
338  public:
339  typedef int IndexType;
340 
341  template <IndexType Cols>
342  using Matrix3X = Eigen::Matrix<PrimType_, 3, Cols>;
343 };
344 
345 
346 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
347  * Conversion Traits
348  * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
349 template<typename DestPrimType_, typename SourcePrimType_>
350 class ConversionTraits<AngleAxis<DestPrimType_>, AngleAxis<SourcePrimType_>> {
351  public:
352  inline static AngleAxis<DestPrimType_> convert(const AngleAxis<SourcePrimType_>& a) {
353  return AngleAxis<DestPrimType_>(a.toImplementation().template cast<DestPrimType_>());
354  }
355 };
356 
357 template<typename DestPrimType_, typename SourcePrimType_>
358 class ConversionTraits<AngleAxis<DestPrimType_>, RotationVector<SourcePrimType_>> {
359  public:
360  inline static AngleAxis<DestPrimType_> convert(const RotationVector<SourcePrimType_>& rotationVector) {
362 
363  // Cast to destination primitive type
364  const Eigen::Matrix<DestPrimType_, 3, 1> vector = rotationVector.toImplementation().template cast<DestPrimType_>();
365  const DestPrimType_ norm = vector.norm();
366 
367  // Check for identity rotation (we cannot divide by zero norm).
368  if (norm < internal::NumTraits<Scalar>::dummy_precision()) {
369  return AngleAxis<DestPrimType_>();
370  }
371  return AngleAxis<DestPrimType_>(norm, vector.normalized());
372  }
373 };
374 
375 template<typename DestPrimType_, typename SourcePrimType_>
376 class ConversionTraits<AngleAxis<DestPrimType_>, RotationQuaternion<SourcePrimType_>> {
377  public:
378  inline static AngleAxis<DestPrimType_> convert(const RotationQuaternion<SourcePrimType_>& q) {
379  // bad prescion !
380  return AngleAxis<DestPrimType_>(Eigen::AngleAxis<DestPrimType_>(q.toImplementation().template cast<DestPrimType_>()));
381  }
382 };
383 
384 template<typename DestPrimType_, typename SourcePrimType_>
385 class ConversionTraits<AngleAxis<DestPrimType_>, RotationMatrix<SourcePrimType_>> {
386  public:
387  inline static AngleAxis<DestPrimType_> convert(const RotationMatrix<SourcePrimType_>& rotationMatrix) {
388  // Bad precision!
389  return AngleAxis<DestPrimType_>(Eigen::AngleAxis<DestPrimType_>(rotationMatrix.toImplementation().template cast<DestPrimType_>()));
390 
391  }
392 };
393 
394 template<typename DestPrimType_, typename SourcePrimType_>
395 class ConversionTraits<AngleAxis<DestPrimType_>, EulerAnglesXyz<SourcePrimType_>> {
396  public:
397  inline static AngleAxis<DestPrimType_> convert(const EulerAnglesXyz<SourcePrimType_>& xyz) {
398  // bad precision!
399  return AngleAxis<DestPrimType_>(Eigen::AngleAxis<DestPrimType_>(
400  Eigen::AngleAxis<DestPrimType_>((DestPrimType_)xyz.x(), Eigen::Matrix<DestPrimType_, 3, 1>::UnitX()) *
401  Eigen::AngleAxis<DestPrimType_>((DestPrimType_)xyz.y(), Eigen::Matrix<DestPrimType_, 3, 1>::UnitY()) *
402  Eigen::AngleAxis<DestPrimType_>((DestPrimType_)xyz.z(), Eigen::Matrix<DestPrimType_, 3, 1>::UnitZ())));
403  }
404 };
405 
406 template<typename DestPrimType_, typename SourcePrimType_>
407 class ConversionTraits<AngleAxis<DestPrimType_>, EulerAnglesZyx<SourcePrimType_>> {
408  public:
409  inline static AngleAxis<DestPrimType_> convert(const EulerAnglesZyx<SourcePrimType_>& zyx) {
410  // Bad precision!
411  return AngleAxis<DestPrimType_>(Eigen::AngleAxis<DestPrimType_>(
412  Eigen::AngleAxis<DestPrimType_>((DestPrimType_)zyx.z(), Eigen::Matrix<DestPrimType_, 3, 1>::UnitZ()) *
413  Eigen::AngleAxis<DestPrimType_>((DestPrimType_)zyx.y(), Eigen::Matrix<DestPrimType_, 3, 1>::UnitY()) *
414  Eigen::AngleAxis<DestPrimType_>((DestPrimType_)zyx.x(), Eigen::Matrix<DestPrimType_, 3, 1>::UnitX())));
415  }
416 };
417 
418 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
419  * Multiplication Traits
420  * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
421 
422 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
423  * Rotation Traits
424  * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
425 
426 
427 
428 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
429  * Comparison Traits
430  * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
431 template<typename PrimType_>
432 class ComparisonTraits<RotationBase<AngleAxis<PrimType_>>, RotationBase<AngleAxis<PrimType_>>> {
433  public:
434  inline static bool isEqual(const RotationBase<AngleAxis<PrimType_>>& a, const RotationBase<AngleAxis<PrimType_>>& b){
435  const PrimType_ tolPercent = 0.01;
436  return compareRelative(a.derived().angle(), b.derived().angle(), tolPercent) &&
437  compareRelative(a.derived().axis().x(), b.derived().axis().x(), tolPercent) &&
438  compareRelative(a.derived().axis().y(), b.derived().axis().y(), tolPercent) &&
439  compareRelative(a.derived().axis().z(), b.derived().axis().z(), tolPercent);
440  }
441 };
442 
443 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
444  * Fixing Traits
445  * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
446 template<typename PrimType_>
447 class FixingTraits<AngleAxis<PrimType_>> {
448  public:
449  inline static void fix(AngleAxis<PrimType_>& aa) {
450  aa.setAxis(aa.axis().normalized());
451  }
452 };
453 
454 } // namespace internal
455 } // namespace kindr
456 
AngleAxis< double > AngleAxisD
Passive angle axis rotation with double primitive type.
Definition: AngleAxis.hpp:322
Vector4 vector() const
Definition: AngleAxis.hpp:231
PrimType_ Scalar
The primitive type. Float/Double.
Definition: AngleAxis.hpp:72
AngleAxis< PrimType_ > & derived()
Gets the derived rotation. (only for advanced users)
AngleAxis & setIdentity()
Sets the rotation to identity.
Definition: AngleAxis.hpp:242
Scalar angle() const
Returns the rotation angle.
Definition: AngleAxis.hpp:191
Scalar y() const
Reading access to pitch (Y&#39;) angle.
AngleAxis getUnique() const
Returns a unique angle axis rotation with angle in [0,pi]. This function is used to compare different...
Definition: AngleAxis.hpp:252
Implementation of Euler angles (Z-Y&#39;-X&#39;&#39; / yaw-pitch-roll) rotation based on Eigen::Matrix<Scalar, 3, 1>
Representation of a generic rotationThis class defines the generic interface for a rotation...
friend std::ostream & operator<<(std::ostream &out, const AngleAxis &a)
Used for printing the object with std::cout.
Definition: AngleAxis.hpp:311
void setAxis(Scalar v1, Scalar v2, Scalar v3)
Sets the rotation axis.
Definition: AngleAxis.hpp:217
AngleAxis(const RotationBase< OtherDerived_ > &other)
Constructor using another rotation.
Definition: AngleAxis.hpp:132
AngleAxis & operator=(const RotationBase< OtherDerived_ > &other)
Assignment operator using another rotation.
Definition: AngleAxis.hpp:141
Scalar x() const
Reading access to roll (X&#39;&#39;) angle.
AngleAxis< double > AngleAxisPD
Active angle axis rotation with double primitive type.
Definition: AngleAxis.hpp:318
Eigen::Matrix< PrimType_, 3, 1 > Vector3
The axis type is a 3D vector.
Definition: AngleAxis.hpp:75
T wrapPosNegPI(T angle)
wrap angle to [-PI..PI)
Definition: common.hpp:96
void setAngle(Scalar angle)
Sets the rotation angle.
Definition: AngleAxis.hpp:197
Base Implementation
The implementation type. The implementation type is always an Eigen object.
Definition: AngleAxis.hpp:68
AngleAxis< float > AngleAxisF
Passive angle axis rotation with float primitive type.
Definition: AngleAxis.hpp:324
Scalar y() const
Gets pitch (Y&#39;) angle.
AngleAxis(const Vector4 &vector)
Constructor using a 4x1matrix. In debug mode, an assertion is thrown if the rotation vector has not u...
Definition: AngleAxis.hpp:114
Implementation & toImplementation()
Cast to the implementation type.
AngleAxis & operator()(const RotationBase< OtherDerived_ > &other)
Parenthesis operator to convert from another rotation.
Definition: AngleAxis.hpp:151
void fix()
Fixes the rotation to get rid of numerical errors (e.g. normalize quaternion).
Implementation of a rotation vector based on Eigen::Matrix<Scalar, 3, 1>
Definition: Rotation.hpp:43
#define KINDR_ASSERT_SCALAR_NEAR_DBG(exceptionType, A, B, PERCENT_TOLERANCE, MESSAGE)
AngleAxis & invert()
Inverts the rotation.
Definition: AngleAxis.hpp:169
Implementation of matrix rotation based on Eigen::Matrix<Scalar, 3, 3>
Definition: Rotation.hpp:49
Scalar z() const
Gets yaw (Z&#39;&#39;) angle.
PrimType_ Scalar
The primitive type. Float/Double.
void setAxis(const Vector3 &axis)
Sets the rotation axis.
Definition: AngleAxis.hpp:210
AngleAxis< float > AngleAxisPF
Active angle axis rotation with float primitive type.
Definition: AngleAxis.hpp:320
Implementation of Euler angles (X-Y&#39;-Z&#39;&#39; / roll-pitch-yaw) rotation based on Eigen::Matrix<Scalar,3,1>
AngleAxis(Scalar angle, Scalar v1, Scalar v2, Scalar v3)
Constructor using four scalars. In debug mode, an assertion is thrown if the rotation vector has not ...
Definition: AngleAxis.hpp:94
AngleAxis(const Base &other)
Constructor using Eigen::AngleAxis. In debug mode, an assertion is thrown if the rotation vector has ...
Definition: AngleAxis.hpp:123
Eigen::Matrix< PrimType_, 4, 1 > Vector4
All four parameters stored in a vector [angle; axis].
Definition: AngleAxis.hpp:79
const Implementation & toImplementation() const
Cast to the implementation type.
Definition: AngleAxis.hpp:184
const Vector3 & axis() const
Returns the rotation axis.
Definition: AngleAxis.hpp:204
AngleAxis & setUnique()
Modifies the angle axis rotation such that the lies angle in [0,pi).
Definition: AngleAxis.hpp:297
AngleAxis()
Default constructor using identity rotation.
Definition: AngleAxis.hpp:83
void setVector(const Vector4 &vector)
Sets angle-axis from a 4x1-matrix.
Definition: AngleAxis.hpp:224
Eigen::AngleAxis< PrimType_ > Base
The base type.
Definition: AngleAxis.hpp:58
Implementation of an angle axis rotation based on Eigen::AngleAxis.
Definition: AngleAxis.hpp:54
Implementation & toImplementation()
Cast to the implementation type.
Scalar z() const
Reading access to yaw (Z) angle.
Implementation of quaternion rotation based on Eigen::Quaternion.
Definition: Rotation.hpp:46
Implementation & toImplementation()
Cast to the implementation type.
Implementation & toImplementation()
Cast to the implementation type.
Definition: AngleAxis.hpp:177
AngleAxis(Scalar angle, const Vector3 &axis)
Constructor using angle and axis. In debug mode, an assertion is thrown if the rotation vector has no...
Definition: AngleAxis.hpp:104
AngleAxis inverted() const
Returns the inverse of the rotation.
Definition: AngleAxis.hpp:159
Scalar x() const
Gets roll (X) angle.
bool compareRelative(SCALAR a, SCALAR2 b, SCALAR3 percentTolerance, SCALAR *percentError=NULL, SCALAR bothZeroThreshold=SCALAR(1.0e-15))