Kindr
Kinematics and Dynamics for Robotics
RotationMatrix.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 RotationMatrix : public RotationBase<RotationMatrix<PrimType_>>, private Eigen::Matrix<PrimType_, 3, 3> {
55  private:
58  typedef Eigen::Matrix<PrimType_, 3, 3> Base;
59  public:
63  typedef Base Implementation;
67  typedef PrimType_ Scalar;
68 
72  : Base(Base::Identity()) {
73  }
74 
87  RotationMatrix(Scalar r11, Scalar r12, Scalar r13,
88  Scalar r21, Scalar r22, Scalar r23,
89  Scalar r31, Scalar r32, Scalar r33) {
90 
91  *this << r11,r12,r13,r21,r22,r23,r31,r32,r33;
92 
93  KINDR_ASSERT_MATRIX_NEAR_DBG(std::runtime_error, this->toImplementation() * this->toImplementation().transpose(), Base::Identity(), static_cast<Scalar>(1e-4), "Input matrix is not orthogonal.");
94  KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, this->determinant(), static_cast<Scalar>(1), static_cast<Scalar>(1e-4), "Input matrix determinant is not 1.");
95  }
96 
101  explicit RotationMatrix(const Base& other)
102  // : Base(other)
103  {
104 
105  this->toImplementation() = other;
106 
107  KINDR_ASSERT_MATRIX_NEAR_DBG(std::runtime_error, other * other.transpose(), Base::Identity(), static_cast<Scalar>(1e-4), "Input matrix is not orthogonal.");
108  KINDR_ASSERT_SCALAR_NEAR_DBG(std::runtime_error, other.determinant(), static_cast<Scalar>(1), static_cast<Scalar>(1e-4), "Input matrix determinant is not 1.");
109  }
110 
114  template<typename OtherDerived_>
115  inline explicit RotationMatrix(const RotationBase<OtherDerived_>& other)
116  // : Base(internal::ConversionTraits<RotationMatrix, OtherDerived_>::convert(other.derived()).toImplementation())
117  {
118  this->toImplementation() = internal::ConversionTraits<RotationMatrix, OtherDerived_>::convert(other.derived()).toImplementation();
119  }
120 
125  template<typename OtherDerived_>
127  this->toImplementation() = internal::ConversionTraits<RotationMatrix, OtherDerived_>::convert(other.derived()).toImplementation();
128  return *this;
129  }
130 
135  template<typename OtherDerived_>
137  this->toImplementation() = internal::ConversionTraits<RotationMatrix, OtherDerived_>::convert(other.derived()).toImplementation();
138  return *this;
139  }
140 
146  matrix.toImplementation() = this->toImplementation().transpose();
147  return matrix;
148  }
149 
154  *this = this->inverted();
155  return *this;
156  }
157 
163  matrix.toImplementation() = this->toImplementation().transpose();
164  return matrix;
165  }
166 
171  *this = this->transposed();
172  return *this;
173  }
174 
178  Scalar determinant() const {
179  return toImplementation().determinant();
180  }
181 
185  inline Implementation& toImplementation() {
186  return static_cast<Implementation&>(*this);
187  }
188 
192  inline const Implementation& toImplementation() const {
193  return static_cast<const Implementation&>(*this);
194  }
195 
199  inline Implementation matrix() const {
200  return this->toImplementation();
201  }
202 
205  inline void setMatrix(const Implementation & input) {
206  this->toImplementation() = input;
207  }
208 
211  inline void setMatrix(Scalar r11, Scalar r12, Scalar r13,
212  Scalar r21, Scalar r22, Scalar r23,
213  Scalar r31, Scalar r32, Scalar r33) {
214 
215  *this << r11,r12,r13,r21,r22,r23,r31,r32,r33;
216 
217  }
218 
223  this->Implementation::setIdentity();
224  return *this;
225  }
226 
233  return *this;
234  }
235 
241  return *this;
242  }
243 
248  using RotationBase<RotationMatrix<PrimType_>>::operator*; // otherwise ambiguous RotationBase and Eigen
249 
254  using RotationBase<RotationMatrix<PrimType_>>::operator==; // otherwise ambiguous RotationBase and Eigen
255 
259  friend std::ostream& operator << (std::ostream& out, const RotationMatrix& rotationMatrix) {
260  out << rotationMatrix.toImplementation();
261  return out;
262  }
263 };
264 
273 
274 
275 
276 namespace internal {
277 
278 template<typename PrimType_>
279 class get_scalar<RotationMatrix<PrimType_>> {
280  public:
281  typedef PrimType_ Scalar;
282 };
283 
284 template<typename PrimType_>
285 class get_matrix3X<RotationMatrix<PrimType_>>{
286  public:
287  typedef int IndexType;
288 
289  template <IndexType Cols>
290  using Matrix3X = Eigen::Matrix<PrimType_, 3, Cols>;
291 };
292 
293 
294 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
295  * Conversion Traits
296  * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
297 template<typename DestPrimType_, typename SourcePrimType_>
298 class ConversionTraits<RotationMatrix<DestPrimType_>, AngleAxis<SourcePrimType_>> {
299  public:
300  inline static RotationMatrix<DestPrimType_> convert(const AngleAxis<SourcePrimType_>& aa) {
302  matrix.toImplementation() = (aa.toImplementation().template cast<DestPrimType_>()).toRotationMatrix();
303  return matrix;
304  }
305 };
306 
307 template<typename DestPrimType_, typename SourcePrimType_>
308 class ConversionTraits<RotationMatrix<DestPrimType_>, RotationVector<SourcePrimType_>> {
309  public:
310  inline static RotationMatrix<DestPrimType_> convert(const RotationVector<SourcePrimType_>& rotationVector) {
312  }
313 };
314 
315 template<typename DestPrimType_, typename SourcePrimType_>
316 class ConversionTraits<RotationMatrix<DestPrimType_>, RotationQuaternion<SourcePrimType_>> {
317  public:
320  matrix.toImplementation() = (q.toImplementation().template cast<DestPrimType_>()).toRotationMatrix();
321  return matrix;
322  }
323 };
324 
325 template<typename DestPrimType_, typename SourcePrimType_>
326 class ConversionTraits<RotationMatrix<DestPrimType_>, RotationMatrix<SourcePrimType_>> {
327  public:
328  inline static RotationMatrix<DestPrimType_> convert(const RotationMatrix<SourcePrimType_>& R) {
330  matrix.toImplementation() = R.toImplementation().template cast<DestPrimType_>();
331  return matrix;
332  }
333 };
334 
335 template<typename DestPrimType_, typename SourcePrimType_>
336 class ConversionTraits<RotationMatrix<DestPrimType_>, EulerAnglesXyz<SourcePrimType_>> {
337  public:
338  inline static RotationMatrix<DestPrimType_> convert(const EulerAnglesXyz<SourcePrimType_>& xyz) {
340  matrix.toImplementation() = RotationQuaternion<DestPrimType_>(xyz).toImplementation().toRotationMatrix();
341  return matrix;
342  }
343 };
344 
345 template<typename DestPrimType_, typename SourcePrimType_>
346 class ConversionTraits<RotationMatrix<DestPrimType_>, EulerAnglesZyx<SourcePrimType_>> {
347  public:
348  inline static RotationMatrix<DestPrimType_> convert(const EulerAnglesZyx<SourcePrimType_>& zyx) {
350  matrix.toImplementation() = RotationQuaternion<DestPrimType_>(zyx).toImplementation().toRotationMatrix();
351  return matrix;
352  }
353 };
354 
355 
356 
359 template<typename PrimType_>
360 class MultiplicationTraits<RotationBase<RotationMatrix<PrimType_>>, RotationBase<RotationMatrix<PrimType_>>> {
361  public:
362  inline static RotationMatrix<PrimType_> mult(const RotationMatrix<PrimType_>& lhs, const RotationMatrix<PrimType_>& rhs) {
364  result.toImplementation() = lhs.toImplementation() * rhs.toImplementation();
365  return result;
366  }
367 };
368 
369 
370 
371 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
372  * Rotation Traits
373  * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
374 
375 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
376  * Comparison Traits
377  * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
378 
379 
380 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
381  * Box Operations - required?
382  * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
383 
384 
385 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
386  * Fixing Traits
387  * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
388 template<typename PrimType_>
389 class FixingTraits<RotationMatrix<PrimType_>> {
390  public:
391  inline static void fix(RotationMatrix<PrimType_>& R) {
392  const PrimType_ factor = 1/pow(R.determinant(), 1.0/3.0);
393  R.setMatrix(factor*R.matrix()(0,0),
394  factor*R.matrix()(0,1),
395  factor*R.matrix()(0,2),
396  factor*R.matrix()(1,0),
397  factor*R.matrix()(1,1),
398  factor*R.matrix()(1,2),
399  factor*R.matrix()(2,0),
400  factor*R.matrix()(2,1),
401  factor*R.matrix()(2,2));
402  }
403 };
404 
405 
406 } // namespace internal
407 } // namespace kindr
PrimType_ Scalar
The primitive type. Float/Double.
Derived_ & derived()
Gets the derived rotation. (only for advanced users)
friend std::ostream & operator<<(std::ostream &out, const RotationMatrix &rotationMatrix)
Used for printing the object with std::cout.
Implementation matrix() const
Reading access to the rotation matrix.
RotationMatrix(const RotationBase< OtherDerived_ > &other)
Constructor using another rotation.
void setMatrix(const Implementation &input)
Writing access to the rotation matrix.
RotationMatrix< float > RotationMatrixPF
Passive matrix rotation with float primitive type.
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...
#define KINDR_ASSERT_MATRIX_NEAR_DBG(exceptionType, A, B, PERCENT_TOLERANCE, MSG)
RotationMatrix & operator=(const RotationBase< OtherDerived_ > &other)
Assignment operator using another rotation.
RotationMatrix & invert()
Inverts the rotation.
RotationMatrix(const Base &other)
Constructor using Eigen::Matrix. In debug mode, an assertion is thrown if the rotation vector has not...
Eigen::Matrix< PrimType_, 3, 3 > Base
The base type.
RotationMatrix(Scalar r11, Scalar r12, Scalar r13, Scalar r21, Scalar r22, Scalar r23, Scalar r31, Scalar r32, Scalar r33)
Constructor using nine scalars. In debug mode, an assertion is thrown if the matrix is not a rotation...
RotationMatrix & operator()(const RotationBase< OtherDerived_ > &other)
Parenthesis operator to convert from another rotation.
Implementation & toImplementation()
Cast to the implementation type.
void setMatrix(Scalar r11, Scalar r12, Scalar r13, Scalar r21, Scalar r22, Scalar r23, Scalar r31, Scalar r32, Scalar r33)
Writing access to the rotation matrix.
RotationMatrix & transpose()
Transposes the rotation matrix.
Scalar determinant() const
Returns the determinant of the rotation matrix.
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)
RotationMatrix & setUnique()
Modifies the matrix rotation such that it becomes unique. A rotation matrix is always unique...
RotationMatrix()
Default constructor using identity rotation.
Implementation of matrix rotation based on Eigen::Matrix<Scalar, 3, 3>
Definition: Rotation.hpp:49
Implementation of Euler angles (X-Y&#39;-Z&#39;&#39; / roll-pitch-yaw) rotation based on Eigen::Matrix<Scalar,3,1>
RotationMatrix< double > RotationMatrixD
Active matrix rotation with double primitive type.
RotationMatrix & setIdentity()
Sets the rotation to identity.
RotationMatrix transposed() const
Returns the transpose of the rotation matrix.
RotationMatrix< float > RotationMatrixF
Active matrix rotation with float primitive type.
RotationMatrix inverted() const
Returns the inverse of the rotation.
Base Implementation
The implementation type. The implementation type is always an Eigen object.
RotationMatrix< double > RotationMatrixPD
Passive matrix rotation with double primitive type.
Implementation of an angle axis rotation based on Eigen::AngleAxis.
Definition: AngleAxis.hpp:54
Implementation & toImplementation()
Cast to the implementation type.
Implementation of quaternion rotation based on Eigen::Quaternion.
Definition: Rotation.hpp:46
Implementation & toImplementation()
Cast to the implementation type.
Definition: AngleAxis.hpp:177
const Implementation & toImplementation() const
Cast to the implementation type.
RotationMatrix getUnique() const
Returns a unique matrix rotation. A rotation matrix is always unique. This function is used to compar...