GCC Code Coverage Report

Directory: ./
File: include/hpp/constraints/svd.hh
Date: 2025-05-05 12:19:30
Exec Total Coverage
Lines: 46 46 100.0%
Branches: 79 154 51.3%
Line Branch Exec Source
1 // Copyright (c) 2015 CNRS
2 // Author: Joseph Mirabel
3 //
4 //
5
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are
8 // met:
9 //
10 // 1. Redistributions of source code must retain the above copyright
11 // notice, this list of conditions and the following disclaimer.
12 //
13 // 2. Redistributions in binary form must reproduce the above copyright
14 // notice, this list of conditions and the following disclaimer in the
15 // documentation and/or other materials provided with the distribution.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
18 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
19 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
20 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
21 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
22 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
23 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
24 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
25 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
28 // DAMAGE.
29
30 #ifndef HPP_CONSTRAINTS_SVD_HH
31 #define HPP_CONSTRAINTS_SVD_HH
32
33 #include <Eigen/SVD>
34 #include <hpp/constraints/fwd.hh>
35
36 namespace hpp {
37 namespace constraints {
38 template <typename SVD>
39 16000 static Eigen::Ref<const typename SVD::MatrixUType> getU1(
40 const SVD& svd, const size_type& rank) {
41
1/2
✓ Branch 3 taken 16000 times.
✗ Branch 4 not taken.
 32000 return svd.matrixU().leftCols(rank);
42 }
43
44 template <typename SVD>
45 4000 static Eigen::Ref<const typename SVD::MatrixUType> getU2(
46 const SVD& svd, const size_type& rank) {
47
1/2
✓ Branch 5 taken 4000 times.
✗ Branch 6 not taken.
 8000 return svd.matrixU().rightCols(svd.matrixU().cols() - rank);
48 }
49
50 template <typename SVD>
51 16000 static Eigen::Ref<const typename SVD::MatrixUType> getV1(
52 const SVD& svd, const size_type& rank) {
53
1/2
✓ Branch 3 taken 16000 times.
✗ Branch 4 not taken.
 32000 return svd.matrixV().leftCols(rank);
54 }
55
56 template <typename SVD>
57 4142 static Eigen::Ref<const typename SVD::MatrixUType> getV2(
58 const SVD& svd, const size_type& rank) {
59
1/2
✓ Branch 5 taken 4142 times.
✗ Branch 6 not taken.
 8284 return svd.matrixV().rightCols(svd.matrixV().cols() - rank);
60 }
61
62 template <typename SVD>
63 4000 static void pseudoInverse(const SVD& svd,
64 Eigen::Ref<typename SVD::MatrixType> pinvmat) {
65
2/4
✓ Branch 1 taken 4000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4000 times.
✗ Branch 5 not taken.
 4000 eigen_assert(
66 svd.computeU() && svd.computeV() &&
67 "Eigen::JacobiSVD "
68 "computation flags must be at least: ComputeThinU | ComputeThinV");
69
70
1/2
✓ Branch 1 taken 4000 times.
✗ Branch 2 not taken.
 4000 size_type rank = svd.rank();
71
2/4
✓ Branch 1 taken 4000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4000 times.
✗ Branch 5 not taken.
 4000 typename SVD::SingularValuesType singularValues_inv =
72
1/2
✓ Branch 2 taken 4000 times.
✗ Branch 3 not taken.
 4000 svd.singularValues().segment(0, rank).cwiseInverse();
73
74
6/12
✓ Branch 1 taken 4000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4000 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4000 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4000 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 4000 times.
✗ Branch 17 not taken.
 4000 pinvmat.noalias() = getV1<SVD>(svd, rank) * singularValues_inv.asDiagonal() *
75
2/4
✓ Branch 1 taken 4000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4000 times.
✗ Branch 5 not taken.
 8000 getU1<SVD>(svd, rank).adjoint();
76 4000 }
77
78 template <typename SVD>
79 4000 void projectorOnSpan(const SVD& svd,
80 Eigen::Ref<typename SVD::MatrixType> projector) {
81
2/4
✓ Branch 1 taken 4000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4000 times.
✗ Branch 5 not taken.
 4000 eigen_assert(
82 svd.computeU() && svd.computeV() &&
83 "Eigen::JacobiSVD "
84 "computation flags must be at least: ComputeThinU | ComputeThinV");
85
86
1/2
✓ Branch 1 taken 4000 times.
✗ Branch 2 not taken.
 4000 size_type rank = svd.rank();
87
6/12
✓ Branch 1 taken 4000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4000 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4000 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4000 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 4000 times.
✗ Branch 17 not taken.
 4000 projector.noalias() = getV1<SVD>(svd, rank) * getV1<SVD>(svd, rank).adjoint();
88 4000 }
89
90 template <typename SVD>
91 4000 void projectorOnSpanOfInv(const SVD& svd,
92 Eigen::Ref<typename SVD::MatrixType> projector) {
93
2/4
✓ Branch 1 taken 4000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4000 times.
✗ Branch 5 not taken.
 4000 eigen_assert(
94 svd.computeU() && svd.computeV() &&
95 "Eigen::JacobiSVD "
96 "computation flags must be at least: ComputeThinU | ComputeThinV");
97
98
1/2
✓ Branch 1 taken 4000 times.
✗ Branch 2 not taken.
 4000 size_type rank = svd.rank();
99
6/12
✓ Branch 1 taken 4000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4000 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4000 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4000 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 4000 times.
✗ Branch 17 not taken.
 4000 projector.noalias() = getU1<SVD>(svd, rank) * getU1<SVD>(svd, rank).adjoint();
100 4000 }
101
102 template <typename SVD>
103 4000 void projectorOnKernel(const SVD& svd,
104 Eigen::Ref<typename SVD::MatrixType> projector,
105 const bool& computeFullV = false) {
106
1/2
✓ Branch 1 taken 4000 times.
✗ Branch 2 not taken.
 4000 eigen_assert(svd.computeV() &&
107 "Eigen::JacobiSVD "
108 "computation flags must be at least: ComputeThinV");
109
110
1/2
✓ Branch 1 taken 4000 times.
✗ Branch 2 not taken.
 4000 size_type rank = svd.rank();
111
2/2
✓ Branch 0 taken 2000 times.
✓ Branch 1 taken 2000 times.
 4000 if (computeFullV)
112
2/4
✓ Branch 1 taken 2000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2000 times.
✗ Branch 5 not taken.
 2000 projector.noalias() =
113
4/8
✓ Branch 1 taken 2000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2000 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2000 times.
✗ Branch 11 not taken.
 4000 getV2<SVD>(svd, rank) * getV2<SVD>(svd, rank).adjoint();
114 else {
115
5/10
✓ Branch 1 taken 2000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2000 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2000 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2000 times.
✗ Branch 14 not taken.
 2000 projector.noalias() =
116
2/4
✓ Branch 1 taken 2000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2000 times.
✗ Branch 5 not taken.
 4000 -getV1<SVD>(svd, rank) * getV1<SVD>(svd, rank).adjoint();
117
5/10
✓ Branch 1 taken 2000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2000 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2000 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2000 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 2000 times.
✗ Branch 15 not taken.
 2000 projector.diagonal().noalias() += vector_t::Ones(svd.matrixV().rows());
118 }
119 4000 }
120
121 template <typename SVD>
122 4000 void projectorOnKernelOfInv(const SVD& svd,
123 Eigen::Ref<typename SVD::MatrixType> projector,
124 const bool& computeFullU = false) {
125
1/2
✓ Branch 1 taken 4000 times.
✗ Branch 2 not taken.
 4000 eigen_assert(svd.computeU() &&
126 "Eigen::JacobiSVD "
127 "computation flags must be at least: ComputeThinU");
128
129
1/2
✓ Branch 1 taken 4000 times.
✗ Branch 2 not taken.
 4000 size_type rank = svd.rank();
130
2/2
✓ Branch 0 taken 2000 times.
✓ Branch 1 taken 2000 times.
 4000 if (computeFullU) {
131 // U2 * U2*
132
2/4
✓ Branch 1 taken 2000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2000 times.
✗ Branch 5 not taken.
 2000 projector.noalias() =
133
4/8
✓ Branch 1 taken 2000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2000 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2000 times.
✗ Branch 11 not taken.
 4000 getU2<SVD>(svd, rank) * getU2<SVD>(svd, rank).adjoint();
134 } else {
135 // I - U1 * U1*
136
5/10
✓ Branch 1 taken 2000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2000 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2000 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2000 times.
✗ Branch 14 not taken.
 2000 projector.noalias() =
137
2/4
✓ Branch 1 taken 2000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2000 times.
✗ Branch 5 not taken.
 4000 -getU1<SVD>(svd, rank) * getU1<SVD>(svd, rank).adjoint();
138
4/8
✓ Branch 3 taken 2000 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2000 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2000 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2000 times.
✗ Branch 13 not taken.
 2000 projector.diagonal().noalias() += vector_t::Ones(svd.matrixU().rows());
139 }
140 4000 }
141 } // namespace constraints
142 } // namespace hpp
143
144 #endif // HPP_CONSTRAINTS_SVD_HH
145