GCC Code Coverage Report

Directory: ./
File: tests/tools/test_boost.cpp
Date: 2025-05-13 12:28:21
Exec Total Coverage
Lines: 85 85 100.0%
Branches: 90 130 69.2%
Line Branch Exec Source
1 /*
2 * Copyright 2010,
3 * François Bleibel,
4 * Olivier Stasse,
5 *
6 * CNRS/AIST
7 *
8 */
9
10 #ifndef WIN32
11 #include <sys/time.h>
12 #endif
13 #include <dynamic-graph/linear-algebra.h>
14
15 #include <iostream>
16 #include <sot/core/debug.hh>
17 #include <sot/core/matrix-geometry.hh>
18 #include <sot/core/matrix-svd.hh>
19 #include <sot/core/memory-task-sot.hh>
20
21 #ifndef WIN32
22 #include <unistd.h>
23 #else
24 #include <sot/core/utils-windows.hh>
25 #endif
26 #include <list>
27
28 using namespace dynamicgraph::sot;
29 using namespace std;
30
31 #define INIT_CHRONO(name) \
32 struct timeval t0##_##name, t1##_##name; \
33 double dt##_##name
34 #define START_CHRONO(name) \
35 gettimeofday(&t0##_##name, NULL); \
36 sotDEBUG(25) << "t0 " << #name << ": " << t0##_##name.tv_sec << " - " \
37 << t0##_##name.tv_usec << std::endl
38 #define STOP_CHRONO(name, commentaire) \
39 gettimeofday(&t1##_##name, NULL); \
40 dt##_##name = ((double)(t1##_##name.tv_sec - t0##_##name.tv_sec) * 1000. + \
41 (double)(t1##_##name.tv_usec - t0##_##name.tv_usec) / 1000.); \
42 sotDEBUG(25) << "t1 " << #name << ": " << t1##_##name.tv_sec << " - " \
43 << t1##_##name.tv_usec << std::endl; \
44 sotDEBUG(1) << "Time spent " << #name " " commentaire << " = " \
45 << dt##_##name << std::endl
46
47 /* ----------------------------------------------------------------------- */
48 /* ----------------------------------------------------------------------- */
49 /* ----------------------------------------------------------------------- */
50 /* ----------------------------------------------------------------------- */
51
52 double timerCounter;
53
54 // static void inverseCounter( ublas::matrix<double>& matrix,
55 // dynamicgraph::Matrix& invMatrix )
56 // {
57 // INIT_CHRONO(inv);
58
59 // ublas::matrix<double,ublas::column_major> I = matrix;
60 // ublas::matrix<double,ublas::column_major>
61 // U(matrix.size1(),matrix.size1());
62 // ublas::matrix<double,ublas::column_major>
63 // VT(matrix.size2(),matrix.size2()); ublas::vector<double>
64 // s(std::min(matrix.size1(),matrix.size2())); char Jobu='A'; /* Compute
65 // complete U Matrix */ char Jobvt='A'; /* Compute complete VT Matrix */
66 // char Lw; Lw='O'; /* Compute the optimal size for the working vector */
67
68 // #ifdef WITH_OPENHRP
69
70 // /* Presupposition: an external function jrlgesvd is defined
71 // * and implemented in the calling library.
72 // */
73
74 // // get workspace size for svd
75 // int lw=-1;
76 // {
77 // const int m = matrix.size1();
78 // const int n = matrix.size2();
79 // double vw;
80 // int linfo;
81 // int lda = std::max(m,n);
82 // ublas::matrix<double> tmp(m,n); // matrix is const!
83 // jrlgesvd_(&Jobu, &Jobvt, &m, &n, traits::matrix_storage(tmp), &lda,
84 // 0, 0, &m, 0, &n, &vw, &lw, &linfo);
85 // lw = int(vw);
86 // }
87 // #else //#ifdef WITH_OPENHRP
88 // int lw;
89 // if( matrix.size1()>matrix.size2() )
90 // {
91 // ublas::matrix<double,ublas::column_major> matrixtranspose;
92 // matrixtranspose = trans(matrix);
93 // lw = lapack::gesvd_work(Lw,Jobu,Jobvt,matrixtranspose);
94 // } else {
95 // lw = lapack::gesvd_work(Lw,Jobu,Jobvt,matrix);
96 // }
97
98 // #endif //#ifdef WITH_OPENHRP
99
100 // ublas::vector<double> w(lw);
101 // gettimeofday(&t0_inv,NULL);
102 // lapack::gesvd(Jobu, Jobvt,I,s,U,VT,w);
103 // gettimeofday(&t1_inv,NULL);
104
105 // const unsigned int nsv = s.size();
106 // ublas::vector<double> sp(nsv);
107 // for( unsigned int i=0;i<nsv;++i )
108 // if( fabs(s(i))>1e-6 ) sp(i)=1/s(i); else sp(i)=0.;
109
110 // invMatrix.matrix.clear();
111 // for( unsigned int i=0;i<VT.size2();++i )
112 // for( unsigned int j=0;j<U.size1();++j )
113 // for( unsigned int k=0;k<nsv;++k )
114 // invMatrix.matrix(i,j)+=VT(k,i)*sp(k)*U(j,k);
115
116 // dt_inv = ( (t1_inv.tv_sec-t0_inv.tv_sec) * 1000.
117 // + (t1_inv.tv_usec-t0_inv.tv_usec+0.) / 1000. );
118 // timerCounter+=dt_inv;
119
120 // return ;
121 // }
122
123 /* ----------------------------------------------------------------------- */
124 /* ----------------------------------------------------------------------- */
125 /* ----------------------------------------------------------------------- */
126
127 1 int main(int argc, char **argv) {
128 if (sotDEBUG_ENABLE(1)) DebugTrace::openFile();
129
130 // const unsigned int r=1;
131 // const unsigned int c=30;
132 1 unsigned int r = 1;
133
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (argc > 1) r = atoi(argv[1]);
134 1 unsigned int c = 30;
135
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (argc > 2) c = atoi(argv[2]);
136 static const int BENCH = 100;
137
138
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 dynamicgraph::Matrix M(r, c);
139
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 dynamicgraph::Matrix M1(r, c);
140
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 dynamicgraph::Matrix Minv(c, r);
141
142
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 dynamicgraph::Matrix U, V, S;
143
144 1 unsigned int nbzeros = 0;
145
2/2
✓ Branch 0 taken 30 times.
✓ Branch 1 taken 1 times.
 31 for (unsigned int j = 0; j < c; ++j) {
146
2/2
✓ Branch 1 taken 6 times.
✓ Branch 2 taken 24 times.
 30 if ((rand() + 1.) / RAND_MAX > .8) {
147
3/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 6 times.
✓ Branch 4 taken 6 times.
 12 for (unsigned int i = 0; i < r; ++i) M(i, j) = 0.;
148 6 nbzeros++;
149 } else
150
2/2
✓ Branch 0 taken 24 times.
✓ Branch 1 taken 24 times.
 48 for (unsigned int i = 0; i < r; ++i)
151
1/2
✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
 24 M(i, j) = (rand() + 1.) / RAND_MAX * 2 - 1;
152 }
153
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1 times.
 2 for (unsigned int i = 0; i < r; ++i)
154
2/2
✓ Branch 0 taken 30 times.
✓ Branch 1 taken 1 times.
 31 for (unsigned int j = 0; j < c; ++j)
155
2/4
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
 30 M1(i, j) = M(i, j); //+ ((rand()+1.) / RAND_MAX*2-1) * 1e-28 ;
156
157 // sotDEBUG(15) << dynamicgraph::MATLAB <<"M = "<< M <<endl;
158 sotDEBUG(15) << "M1 = " << M1 << endl;
159 sotDEBUG(5) << "Nb zeros = " << nbzeros << endl;
160
161 INIT_CHRONO(inv);
162
163 1 START_CHRONO(inv);
164
3/4
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 100 times.
✓ Branch 4 taken 1 times.
 101 for (int i = 0; i < BENCH; ++i) dynamicgraph::pseudoInverse(M, Minv);
165 1 STOP_CHRONO(inv, "init");
166 sotDEBUG(15) << "Minv = " << Minv << endl;
167
168 1 START_CHRONO(inv);
169
3/4
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 100 times.
✓ Branch 4 taken 1 times.
 101 for (int i = 0; i < BENCH; ++i) dynamicgraph::pseudoInverse(M, Minv);
170 1 STOP_CHRONO(inv, "M+standard");
171
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 cout << dt_inv << endl;
172
173 // START_CHRONO(inv);
174 // for( int i=0;i<BENCH;++i ) M.pseudoInverse( Minv,1e-6,&U,&S,&V );
175 // STOP_CHRONO(inv,"M+");
176 // sotDEBUG(15) << dynamicgraph::MATLAB <<"Minv = "<< Minv <<endl;
177
178 // timerCounter=0;
179 // START_CHRONO(inv);
180 // for( int i=0;i<BENCH;++i ) inverseCounter( M1.matrix,Minv );
181 // STOP_CHRONO(inv,"M1+");
182 // sotDEBUG(5) << "Counter: " << timerCounter << endl;
183 // sotDEBUG(15) << dynamicgraph::MATLAB <<"M1inv = "<< Minv <<endl;
184
185 // dynamicgraph::Matrix M1diag = U.transpose()*M1*V;
186 // timerCounter=0;
187 // START_CHRONO(inv);
188 // for( int i=0;i<BENCH;++i ) inverseCounter( M1diag.matrix,Minv );
189 // STOP_CHRONO(inv,"M1diag+");
190 // sotDEBUG(5) << "Counter: " << timerCounter << endl;
191 // sotDEBUG(8) << dynamicgraph::MATLAB <<"M1diag = "<< M1diag <<endl;
192 // sotDEBUG(15) << dynamicgraph::MATLAB <<"M1diaginv = "<< Minv <<endl;
193
194 1 START_CHRONO(inv);
195 1 std::list<unsigned int> nonzeros;
196
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 dynamicgraph::Matrix Mcreuse;
197
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 dynamicgraph::Matrix Mcreuseinv;
198
2/2
✓ Branch 0 taken 100 times.
✓ Branch 1 taken 1 times.
 101 for (int ib = 0; ib < BENCH; ++ib) {
199 double sumsq;
200 100 unsigned int parc = 0;
201
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 99 times.
 100 if (!ib) {
202 1 nonzeros.clear();
203
2/2
✓ Branch 0 taken 30 times.
✓ Branch 1 taken 1 times.
 31 for (unsigned int j = 0; j < c; ++j) {
204 30 sumsq = 0.;
205
4/6
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 30 times.
✓ Branch 7 taken 30 times.
 60 for (unsigned int i = 0; i < r; ++i) sumsq += M(i, j) * M(i, j);
206
2/2
✓ Branch 0 taken 24 times.
✓ Branch 1 taken 6 times.
 30 if (sumsq > 1e-6) {
207
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 24 nonzeros.push_back(j);
208 24 parc++;
209 }
210 }
211
212
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Mcreuse.resize(r, parc);
213 }
214
215 // dynamicgraph::Matrix Mcreuse( r,parc );
216
217 100 parc = 0;
218 100 for (std::list<unsigned int>::iterator iter = nonzeros.begin();
219
2/2
✓ Branch 2 taken 2400 times.
✓ Branch 3 taken 100 times.
 2500 iter != nonzeros.end(); ++iter) {
220
2/2
✓ Branch 0 taken 2400 times.
✓ Branch 1 taken 2400 times.
 4800 for (unsigned int i = 0; i < r; ++i) {
221
2/4
✓ Branch 2 taken 2400 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2400 times.
✗ Branch 6 not taken.
 2400 Mcreuse(i, parc) = M(i, *iter);
222 }
223 2400 parc++;
224 }
225
226 // dynamicgraph::Matrix Mcreuseinv( Mcreuse.nbCols(),r );
227
1/2
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
 100 Mcreuseinv.resize(Mcreuse.cols(), r);
228
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 100 dynamicgraph::pseudoInverse(Mcreuse, Mcreuseinv);
229 100 parc = 0;
230
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 100 Minv.fill(0.);
231 100 for (std::list<unsigned int>::iterator iter = nonzeros.begin();
232
2/2
✓ Branch 2 taken 2400 times.
✓ Branch 3 taken 100 times.
 2500 iter != nonzeros.end(); ++iter) {
233
4/6
✓ Branch 1 taken 2400 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2400 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 2400 times.
✓ Branch 8 taken 2400 times.
 4800 for (unsigned int i = 0; i < r; ++i) Minv(*iter, i) = Mcreuseinv(parc, i);
234 2400 parc++;
235 }
236
237 if (!ib) {
238 // sotDEBUG(15) << dynamicgraph::MATLAB <<"M = "<< M <<endl;
239 // sotDEBUG(15) << dynamicgraph::MATLAB <<"Mcreuse = "<< Mcreuse
240 //<<endl; sotDEBUG(15) << dynamicgraph::MATLAB <<"Minvnc = "<<
241 // Minv
242 //<<endl;
243 }
244 }
245 1 STOP_CHRONO(inv, "M+creuse");
246 // sotDEBUG(15) << dynamicgraph::MATLAB <<"Minv = "<< Minv <<endl;
247
248 {
249 double sumsq;
250 1 nonzeros.clear();
251 1 unsigned int parc = 0;
252
2/2
✓ Branch 0 taken 30 times.
✓ Branch 1 taken 1 times.
 31 for (unsigned int j = 0; j < c; ++j) {
253 30 sumsq = 0.;
254
4/6
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 30 times.
✓ Branch 7 taken 30 times.
 60 for (unsigned int i = 0; i < r; ++i) sumsq += M(i, j) * M(i, j);
255
2/2
✓ Branch 0 taken 24 times.
✓ Branch 1 taken 6 times.
 30 if (sumsq > 1e-6) {
256
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 24 nonzeros.push_back(j);
257 24 parc++;
258 }
259 }
260
261
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 dynamicgraph::Matrix Mcreuse(r, parc);
262 1 parc = 0;
263 1 for (std::list<unsigned int>::iterator iter = nonzeros.begin();
264
2/2
✓ Branch 2 taken 24 times.
✓ Branch 3 taken 1 times.
 25 iter != nonzeros.end(); ++iter) {
265
2/2
✓ Branch 0 taken 24 times.
✓ Branch 1 taken 24 times.
 48 for (unsigned int i = 0; i < r; ++i) {
266
2/4
✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 24 times.
✗ Branch 6 not taken.
 24 Mcreuse(i, parc) = M(i, *iter);
267 }
268 24 parc++;
269 }
270
271
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 dynamicgraph::Matrix Mcreuseinv(Mcreuse.cols(), r);
272 1 START_CHRONO(inv);
273
2/2
✓ Branch 0 taken 100 times.
✓ Branch 1 taken 1 times.
 101 for (int ib = 0; ib < BENCH; ++ib) {
274
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 100 dynamicgraph::pseudoInverse(Mcreuse, Mcreuseinv);
275 }
276 1 STOP_CHRONO(inv, "M+creuseseule");
277
278 1 parc = 0;
279
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Minv.fill(0.);
280 1 for (std::list<unsigned int>::iterator iter = nonzeros.begin();
281
2/2
✓ Branch 2 taken 24 times.
✓ Branch 3 taken 1 times.
 25 iter != nonzeros.end(); ++iter) {
282
4/6
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 24 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 24 times.
✓ Branch 8 taken 24 times.
 48 for (unsigned int i = 0; i < r; ++i) Minv(*iter, i) = Mcreuseinv(parc, i);
283 24 parc++;
284 }
285
286 {
287 // sotDEBUG(15) << dynamicgraph::MATLAB <<"M = "<< M <<endl;
288 // sotDEBUG(15) << dynamicgraph::MATLAB <<"Mcreuse = "<< Mcreuse
289 //<<endl; sotDEBUG(15) << dynamicgraph::MATLAB <<"Minvnc = "<<
290 // Minv
291 //<<endl;
292 }
293 1 }
294
295 1 return 0;
296 1 }
297