Directory: | ./ |
---|---|
File: | src/matrix/matrix-svd.cpp |
Date: | 2024-08-13 12:13:25 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 11 | 27 | 40.7% |
Branches: | 18 | 88 | 20.5% |
Line | Branch | Exec | Source |
---|---|---|---|
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 |
1/2✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
|
400 | SVD_t svd(_inputMatrix, ComputeThinU | ComputeThinV); |
15 |
2/4✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 400 times.
✗ Branch 5 not taken.
|
400 | SVD_t::SingularValuesType m_singularValues = svd.singularValues(); |
16 |
1/2✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
|
400 | SVD_t::SingularValuesType singularValues_inv; |
17 |
1/2✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
|
400 | singularValues_inv.resizeLike(m_singularValues); |
18 |
2/2✓ Branch 1 taken 400 times.
✓ Branch 2 taken 400 times.
|
800 | for (long i = 0; i < m_singularValues.size(); ++i) { |
19 |
2/4✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 400 times.
✗ Branch 4 not taken.
|
400 | if (m_singularValues(i) > threshold) |
20 |
2/4✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 400 times.
✗ Branch 5 not taken.
|
400 | singularValues_inv(i) = 1.0 / m_singularValues(i); |
21 | else | ||
22 | ✗ | singularValues_inv(i) = 0; | |
23 | } | ||
24 |
3/6✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 400 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 400 times.
✗ Branch 8 not taken.
|
400 | _inverseMatrix = (svd.matrixV() * singularValues_inv.asDiagonal() * |
25 |
4/8✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 400 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 400 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 400 times.
✗ Branch 11 not taken.
|
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 | ||
59 |