Kindr
Kinematics and Dynamics for Robotics
LinearAlgebra.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 #pragma once
29 
30 #include <Eigen/SVD>
31 
32 namespace kindr {
33 
34 
40 template<typename PrimType_>
41 inline static Eigen::Matrix<PrimType_, 3, 3> getSkewMatrixFromVector(const Eigen::Matrix<PrimType_, 3, 1>& vec) {
42  Eigen::Matrix<PrimType_, 3, 3> mat;
43  mat << 0, -vec(2), vec(1), vec(2), 0, -vec(0), -vec(1), vec(0), 0;
44  return mat;
45 }
46 
52 template<typename PrimType_>
53 inline static Eigen::Matrix<PrimType_, 3, 1> getVectorFromSkewMatrix(const Eigen::Matrix<PrimType_, 3, 3>& matrix) {
54  return Eigen::Matrix<PrimType_, 3, 1> (matrix(2,1), matrix(0,2), matrix(1,0));
55 }
56 
65 template<typename _Matrix_TypeA_, typename _Matrix_TypeB_>
66 bool static pseudoInverse(const _Matrix_TypeA_ &a, _Matrix_TypeB_ &result,
67  double epsilon = std::numeric_limits<typename _Matrix_TypeA_::Scalar>::epsilon())
68 {
69  // Shorthands
70  constexpr auto rowsA = static_cast<int>(_Matrix_TypeA_::RowsAtCompileTime);
71  constexpr auto colsA = static_cast<int>(_Matrix_TypeA_::ColsAtCompileTime);
72  constexpr auto rowsB = static_cast<int>(_Matrix_TypeB_::RowsAtCompileTime);
73  constexpr auto colsB = static_cast<int>(_Matrix_TypeB_::ColsAtCompileTime);
74 
75  // Assert wrong matrix types
76  static_assert(std::is_same<typename _Matrix_TypeA_::Scalar, typename _Matrix_TypeB_::Scalar>::value,
77  "[kindr::pseudoInverse] Matrices must be of the same Scalar type!");
78  static_assert(rowsA == colsB && colsA == rowsB, "[kindr::pseudoInverse] Result type has wrong size!");
79 
80  // If one dimension is dynamic, compute everything as dynamic size
81  constexpr auto m = Eigen::JacobiSVD< _Matrix_TypeA_ >::DiagSizeAtCompileTime;
82 
83  // JacobiSVD needs to be computed on dynamic size if we need ComputeThinU, ComputeThinV, see:
84  // https://eigen.tuxfamily.org/dox/classEigen_1_1JacobiSVD.html
85  Eigen::JacobiSVD< Eigen::Matrix<typename _Matrix_TypeA_::Scalar, Eigen::Dynamic, Eigen::Dynamic> > svd(a, Eigen::ComputeThinU | Eigen::ComputeThinV);
86 
87  typename _Matrix_TypeA_::Scalar tolerance =
88  epsilon * std::max(a.cols(), a.rows()) * svd.singularValues().array().abs().maxCoeff();
89 
90  // Sigma for ThinU and ThinV
91  Eigen::Matrix<typename _Matrix_TypeA_::Scalar, m, m> sigmaThin = Eigen::Matrix<typename _Matrix_TypeA_::Scalar, m, 1>(
92  (svd.singularValues().array().abs() > tolerance).select(svd.singularValues().array().inverse(), 0)).asDiagonal();
93 
94  result = svd.matrixV() * sigmaThin * svd.matrixU().transpose();
95 
96  return true;
97 }
98 
99 
100 } // end namespace kindr
static bool pseudoInverse(const _Matrix_TypeA_ &a, _Matrix_TypeB_ &result, double epsilon=std::numeric_limits< typename _Matrix_TypeA_::Scalar >::epsilon())
Computes the Moore–Penrose pseudoinverse info: http://eigen.tuxfamily.org/bz/show_bug.cgi?id=257.
static Eigen::Matrix< PrimType_, 3, 3 > getSkewMatrixFromVector(const Eigen::Matrix< PrimType_, 3, 1 > &vec)
Gets a skew-symmetric matrix from a (column) vector.
static Eigen::Matrix< PrimType_, 3, 1 > getVectorFromSkewMatrix(const Eigen::Matrix< PrimType_, 3, 3 > &matrix)
Gets a 3x1 vector from a skew-symmetric matrix.