Directory: | ./ |
---|---|
File: | tests/tools/test_boost.cpp |
Date: | 2024-08-13 12:13:25 |
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 |