eigen.hh 1.79 KB
Newer Older
Joseph Mirabel's avatar
Joseph Mirabel committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
// Copyright (c) 2015 CNRS
// Author: Joseph Mirabel
//
//
// This file is part of hpp-model
// hpp-model is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-model is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-model  If not, see
// <http://www.gnu.org/licenses/>.

#ifndef HPP_MODEL_EIGEN_HH
# define HPP_MODEL_EIGEN_HH

# include <hpp/model/fwd.hh>
# include <Eigen/SVD>

namespace hpp {
  namespace model {
    template < typename SVD >
    void pseudoInverse(const SVD& svd,
        Eigen::Ref <typename SVD::MatrixType> pinvmat,
        const value_type tolerance =
        Eigen::NumTraits<typename SVD::MatrixType::Scalar>::epsilon())
    {
      eigen_assert(svd.computeU() && svd.computeV() && "Eigen::JacobiSVD "
          "computation flags must be at least: ComputeThinU | ComputeThinV");

      const typename SVD::SingularValuesType& singularValues
        = svd.singularValues ();

      typename SVD::SingularValuesType singularValues_inv =
        (singularValues.array () >= tolerance).select (
            singularValues.array ().cwiseInverse (),
            SVD::SingularValuesType::Zero (singularValues.size())
            ).matrix ();

      pinvmat = svd.matrixV () * singularValues_inv.asDiagonal()
        * svd.matrixU().adjoint();
    }
    }
  } // namespace model
} // namespace hpp

#endif // HPP_MODEL_EIGEN_HH