1 |
|
|
// Copyright (c) 2018, Joseph Mirabel |
2 |
|
|
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr) |
3 |
|
|
|
4 |
|
|
#include <sot/core/debug.hh> |
5 |
|
|
#include <sot/core/matrix-svd.hh> |
6 |
|
|
|
7 |
|
|
namespace dynamicgraph { |
8 |
|
|
using Eigen::ComputeFullV; |
9 |
|
|
using Eigen::ComputeThinU; |
10 |
|
|
using Eigen::ComputeThinV; |
11 |
|
|
|
12 |
|
400 |
void pseudoInverse(Matrix &_inputMatrix, Matrix &_inverseMatrix, |
13 |
|
|
const double threshold) { |
14 |
✓✗ |
800 |
SVD_t svd(_inputMatrix, ComputeThinU | ComputeThinV); |
15 |
✓✗ |
800 |
SVD_t::SingularValuesType m_singularValues = svd.singularValues(); |
16 |
✓✗ |
400 |
SVD_t::SingularValuesType singularValues_inv; |
17 |
✓✗ |
400 |
singularValues_inv.resizeLike(m_singularValues); |
18 |
✓✗✓✓
|
800 |
for (long i = 0; i < m_singularValues.size(); ++i) { |
19 |
✓✗✓✗
|
400 |
if (m_singularValues(i) > threshold) |
20 |
✓✗✓✗
|
400 |
singularValues_inv(i) = 1.0 / m_singularValues(i); |
21 |
|
|
else |
22 |
|
|
singularValues_inv(i) = 0; |
23 |
|
|
} |
24 |
✓✗✓✗
✓✗ |
400 |
_inverseMatrix = (svd.matrixV() * singularValues_inv.asDiagonal() * |
25 |
✓✗✓✗
✓✗✓✗
|
800 |
svd.matrixU().transpose()); |
26 |
|
400 |
} |
27 |
|
|
|
28 |
|
|
void dampedInverse(const SVD_t &svd, Matrix &_inverseMatrix, |
29 |
|
|
const double threshold) { |
30 |
|
|
typedef SVD_t::SingularValuesType SV_t; |
31 |
|
|
Eigen::ArrayWrapper<const SV_t> sigmas(svd.singularValues()); |
32 |
|
|
|
33 |
|
|
SV_t sv_inv(sigmas / (sigmas.cwiseAbs2() + threshold * threshold)); |
34 |
|
|
const Matrix::Index m = sv_inv.size(); |
35 |
|
|
|
36 |
|
|
_inverseMatrix.noalias() = (svd.matrixV().leftCols(m) * sv_inv.asDiagonal() * |
37 |
|
|
svd.matrixU().leftCols(m).transpose()); |
38 |
|
|
} |
39 |
|
|
|
40 |
|
|
void dampedInverse(const Matrix &_inputMatrix, Matrix &_inverseMatrix, |
41 |
|
|
Matrix &Uref, Vector &Sref, Matrix &Vref, |
42 |
|
|
const double threshold) { |
43 |
|
|
SVD_t svd(_inputMatrix, ComputeThinU | ComputeThinV); |
44 |
|
|
|
45 |
|
|
dampedInverse(svd, _inverseMatrix, threshold); |
46 |
|
|
|
47 |
|
|
Uref = svd.matrixU(); |
48 |
|
|
Vref = svd.matrixV(); |
49 |
|
|
Sref = svd.singularValues(); |
50 |
|
|
} |
51 |
|
|
|
52 |
|
|
void dampedInverse(const Matrix &_inputMatrix, Matrix &_inverseMatrix, |
53 |
|
|
const double threshold) { |
54 |
|
|
SVD_t svd(_inputMatrix, ComputeThinU | ComputeFullV); |
55 |
|
|
dampedInverse(svd, _inverseMatrix, threshold); |
56 |
|
|
} |
57 |
|
|
|
58 |
|
|
} // namespace dynamicgraph |