| Directory: | ./ |
|---|---|
| File: | src/matrix/matrix-svd.cpp |
| Date: | 2024-12-13 12:22:33 |
| 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 |