Kindr
Kinematics and Dynamics for Robotics
RotationBase.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 "kindr/common/common.hpp"
34 
35 
36 namespace kindr {
38 
42 namespace internal {
44 
45 
46 
51 template<typename Rotation_>
52 class get_matrix3X {
53  public:
54 // typedef int IndexType; // find type of Cols (e.g. Eigen::Dynamic)
55 // template <IndexType Cols> Matrix3X {
56 // typedef MATRIX type;
57 // }
58 };
59 
60 
65 template<typename Dest_, typename Source_>
66 class ConversionTraits {
67  public:
68  // inline static Dest_ convert(const Source_& );
69 };
70 
71 
72 
77 template<typename Rotation_>
78 class RotationTraits {
79  public:
80 // inline static typename internal::get_matrix3X<Derived_>::type rotate(const Rotation_& r, const typename internal::get_vector3<Derived_>::type& );
81 };
82 
87 template<typename Rotation_>
88 class MapTraits {
89  public:
90 // Derived_& setExponentialMap(const typename internal::get_matrix3X<Derived_>::template Matrix3X<1>& vector);
91 // typename internal::get_matrix3X<Derived_>::template Matrix3X<1> getLogarithmicMap();
92 };
93 
98 template<typename Left_, typename Right_>
99 class BoxOperationTraits {
100  public:
101 // inline static typename internal::get_matrix3X<Left_>::template Matrix3X<1> box_minus(const RotationBase<Left_, Usage_>& lhs, const RotationBase<Right_, Usage_>& rhs);
102 // inline static Left_ box_plus(const RotationBase<Left_, Usage_>& rotation, const typename internal::get_matrix3X<Left_>::template Matrix3X<1>& vector);
103 };
104 
109 template<typename Rotation_>
110 class SetFromVectorsTraits {
111  public:
112 };
113 
118 template<typename Left_, typename Right_>
119 class DisparityAngleTraits {
120  public:
121  // inline static typename internal::get_scalar<Derived_>::Scalar compute(const Left_& left, const Right_& right);
122 };
123 
128 template<typename Rotation_>
129 class FixingTraits {
130  public:
131  inline static void fix(Rotation_& rot) {
132  // do nothing is the standard case
133  }
134 };
135 
136 
141 template<typename Rotation_>
142 class RandomTraits {
143  public:
144  //inline static Rotation_ set_random(const Rotation_& rot);
145 };
146 
147 } // namespace internal
148 
149 
156 template<typename Derived_>
158  public:
159 
163  RotationBase() = default;
164 
168  RotationBase(const Derived_&) = delete; // on purpose!!
169 
173  RotationBase inverted() const;
174 
178  RotationBase& invert();
179 
184  operator Derived_& () {
185  return static_cast<Derived_&>(*this);
186  }
187 
192  operator const Derived_& () const {
193  return static_cast<const Derived_&>(*this);
194  }
195 
200  Derived_& derived() {
201  return static_cast<Derived_&>(*this);
202  }
203 
208  const Derived_& derived() const {
209  return static_cast<const Derived_&>(*this);
210  }
211 
215  Derived_& setIdentity();
216 
220  Derived_& setRandom() {
221  internal::RandomTraits<RotationBase<Derived_>>::set_random(this->derived());
222  return *this;
223  }
224 
228  Derived_ getRandom() const {
229  Derived_ other;
230  internal::RandomTraits<RotationBase<Derived_>>::set_random(other);
231  return other;
232  }
233 
238  Derived_ getUnique() const;
239 
243  Derived_& setUnique();
244 
245 
249  template<typename OtherDerived_>
250  Derived_ operator *(const RotationBase<OtherDerived_>& other) const {
251  return internal::MultiplicationTraits<RotationBase<Derived_>,RotationBase<OtherDerived_>>::mult(this->derived(), other.derived()); // todo: 1. ok? 2. may be optimized
252  }
253 
257  template<typename OtherDerived_>
258  bool operator ==(const RotationBase<OtherDerived_>& other) const { // todo: may be optimized
259  return internal::ComparisonTraits<RotationBase<Derived_>, RotationBase<OtherDerived_>>::isEqual(this->derived().getUnique(), other.derived().getUnique()); // the type conversion must already take place here to ensure the specialised isequal function is called more often
260  }
261 
270  template<typename OtherDerived_>
271  typename internal::get_scalar<Derived_>::Scalar getDisparityAngle(const RotationBase<OtherDerived_>& other) const {
272  return internal::DisparityAngleTraits<RotationBase<Derived_>, RotationBase<OtherDerived_>>::compute(this->derived(), other.derived());
273  }
274 
278  template<typename OtherDerived_>
279  bool isNear(const RotationBase<OtherDerived_>& other, typename internal::get_scalar<Derived_>::Scalar tol) const {
280  const typename internal::get_scalar<Derived_>::Scalar angle = this->getDisparityAngle(other);
281  return (angle <= tol);
282  }
283 
287  template <typename internal::get_matrix3X<Derived_>::IndexType Cols>
288  typename internal::get_matrix3X<Derived_>::template Matrix3X<Cols> rotate(const typename internal::get_matrix3X<Derived_>::template Matrix3X<Cols>& matrix) const {
289  return internal::RotationTraits<RotationBase<Derived_>>::rotate(this->derived(), matrix);
290  }
291 
292 
296  template <typename internal::get_matrix3X<Derived_>::IndexType Cols>
297  typename internal::get_matrix3X<Derived_>::template Matrix3X<Cols> inverseRotate(const typename internal::get_matrix3X<Derived_>::template Matrix3X<Cols>& matrix) const {
298  return internal::RotationTraits<RotationBase<Derived_>>::rotate(this->derived().inverted(), matrix); // todo: may be optimized
299  }
300 
304  template <typename Vector_>
305  Vector_ rotate(const Vector_& vector) const {
306  return internal::RotationTraits<RotationBase<Derived_>>::rotate(this->derived(), vector);
307  }
308 
312  template <typename Vector_>
313  Vector_ inverseRotate(const Vector_& vector) const {
314  return internal::RotationTraits<RotationBase<Derived_>>::rotate(this->derived().inverted(), vector);
315  }
316 
321  Derived_ exponentialMap(const typename internal::get_matrix3X<Derived_>::template Matrix3X<1>& vector) {
322  return internal::MapTraits<RotationBase<Derived_>>::set_exponential_map(vector);
323  }
324 
328  typename internal::get_matrix3X<Derived_>::template Matrix3X<1> logarithmicMap() const {
329  return internal::MapTraits<RotationBase<Derived_>>::get_logarithmic_map(this->derived());
330  }
331 
335  template<typename OtherDerived_>
336  typename internal::get_matrix3X<Derived_>::template Matrix3X<1> boxMinus(const RotationBase<OtherDerived_>& other) const {
337  return internal::BoxOperationTraits<RotationBase<Derived_>, RotationBase<OtherDerived_>>::box_minus(this->derived(), other.derived());
338  }
342  Derived_ boxPlus(const typename internal::get_matrix3X<Derived_>::template Matrix3X<1>& vector) const {
343  return internal::BoxOperationTraits<RotationBase<Derived_>, RotationBase<Derived_>>::box_plus(this->derived(), vector);
344  }
345 
346 
350  template <typename Vector_>
351  Derived_& setFromVectors(const Vector_& B_v, const Vector_& I_v) {
352  internal::SetFromVectorsTraits<RotationBase<Derived_>>::setFromVectors(this->derived(), B_v, I_v);
353  return *this;
354  }
355 
358  void fix() {
359  internal::FixingTraits<Derived_>::fix(this->derived());
360  }
361 };
362 
363 
364 
365 } // namespace kindr
Derived_ & derived()
Gets the derived rotation. (only for advanced users)
Vector_ rotate(const Vector_ &vector) const
Rotates a vector.
bool isNear(const RotationBase< OtherDerived_ > &other, typename internal::get_scalar< Derived_ >::Scalar tol) const
Compares two rotations with a tolerance.
Representation of a generic rotationThis class defines the generic interface for a rotation...
Vector< PhysicalType_, PrimType_, Dimension_ > operator*(PrimTypeFactor_ factor, const Vector< PhysicalType_, PrimType_, Dimension_ > &vector)
Multiplies a vector with a scalar.
Definition: Vector.hpp:578
const Derived_ & derived() const
Gets the derived rotation. (only for advanced users)
internal::get_matrix3X< Derived_ >::template Matrix3X< Cols > inverseRotate(const typename internal::get_matrix3X< Derived_ >::template Matrix3X< Cols > &matrix) const
Rotates a vector or matrix in reverse.
Derived_ & setRandom()
Sets the rotation to a random one.
internal::get_scalar< Derived_ >::Scalar getDisparityAngle(const RotationBase< OtherDerived_ > &other) const
Gets the disparity angle between two rotations for comparison.
internal::get_matrix3X< Derived_ >::template Matrix3X< 1 > boxMinus(const RotationBase< OtherDerived_ > &other) const
Applies the box minus operation.
Derived_ exponentialMap(const typename internal::get_matrix3X< Derived_ >::template Matrix3X< 1 > &vector)
Sets the rotation using an exponential map.
void fix()
Fixes the rotation to get rid of numerical errors (e.g. normalize quaternion).
Derived_ & setFromVectors(const Vector_ &B_v, const Vector_ &I_v)
Sets the rotation C_IB from two vectors such that I_v = C_IB*B_v i.e. I_v = this->rotate(B_v).
Vector_ inverseRotate(const Vector_ &vector) const
Rotates a vector in reverse.
internal::get_matrix3X< Derived_ >::template Matrix3X< Cols > rotate(const typename internal::get_matrix3X< Derived_ >::template Matrix3X< Cols > &matrix) const
Rotates a vector or a matrix column-wise.
Derived_ boxPlus(const typename internal::get_matrix3X< Derived_ >::template Matrix3X< 1 > &vector) const
Applies the box plus operation.
internal::get_matrix3X< Derived_ >::template Matrix3X< 1 > logarithmicMap() const
Gets the logarithmic map from the rotation.
Derived_ getRandom() const
Returns a random rotation.