GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
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 |
if (argc > 1) r = atoi(argv[1]); |
134 |
1 |
unsigned int c = 30; |
|
135 |
✗✓ | 1 |
if (argc > 2) c = atoi(argv[2]); |
136 |
static const int BENCH = 100; |
||
137 |
|||
138 |
✓✗ | 2 |
dynamicgraph::Matrix M(r, c); |
139 |
✓✗ | 2 |
dynamicgraph::Matrix M1(r, c); |
140 |
✓✗ | 2 |
dynamicgraph::Matrix Minv(c, r); |
141 |
|||
142 |
✓✗✓✗ ✓✗ |
2 |
dynamicgraph::Matrix U, V, S; |
143 |
|||
144 |
1 |
unsigned int nbzeros = 0; |
|
145 |
✓✓ | 31 |
for (unsigned int j = 0; j < c; ++j) { |
146 |
✓✓ | 30 |
if ((rand() + 1.) / RAND_MAX > .8) { |
147 |
✓✓✓✗ |
12 |
for (unsigned int i = 0; i < r; ++i) M(i, j) = 0.; |
148 |
6 |
nbzeros++; |
|
149 |
} else |
||
150 |
✓✓ | 48 |
for (unsigned int i = 0; i < r; ++i) |
151 |
✓✗ | 24 |
M(i, j) = (rand() + 1.) / RAND_MAX * 2 - 1; |
152 |
} |
||
153 |
✓✓ | 2 |
for (unsigned int i = 0; i < r; ++i) |
154 |
✓✓ | 31 |
for (unsigned int j = 0; j < c; ++j) |
155 |
✓✗✓✗ |
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 |
✓✓✓✗ |
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 |
✓✓✓✗ |
101 |
for (int i = 0; i < BENCH; ++i) dynamicgraph::pseudoInverse(M, Minv); |
170 |
1 |
STOP_CHRONO(inv, "M+standard"); |
|
171 |
✓✗✓✗ |
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 |
2 |
std::list<unsigned int> nonzeros; |
|
196 |
✓✗ | 2 |
dynamicgraph::Matrix Mcreuse; |
197 |
✓✗ | 1 |
dynamicgraph::Matrix Mcreuseinv; |
198 |
✓✓ | 101 |
for (int ib = 0; ib < BENCH; ++ib) { |
199 |
double sumsq; |
||
200 |
100 |
unsigned int parc = 0; |
|
201 |
✓✓ | 100 |
if (!ib) { |
202 |
1 |
nonzeros.clear(); |
|
203 |
✓✓ | 31 |
for (unsigned int j = 0; j < c; ++j) { |
204 |
30 |
sumsq = 0.; |
|
205 |
✓✓✓✗ ✓✗ |
60 |
for (unsigned int i = 0; i < r; ++i) sumsq += M(i, j) * M(i, j); |
206 |
✓✓ | 30 |
if (sumsq > 1e-6) { |
207 |
✓✗ | 24 |
nonzeros.push_back(j); |
208 |
24 |
parc++; |
|
209 |
} |
||
210 |
} |
||
211 |
|||
212 |
✓✗ | 1 |
Mcreuse.resize(r, parc); |
213 |
} |
||
214 |
|||
215 |
// dynamicgraph::Matrix Mcreuse( r,parc ); |
||
216 |
|||
217 |
100 |
parc = 0; |
|
218 |
2500 |
for (std::list<unsigned int>::iterator iter = nonzeros.begin(); |
|
219 |
✓✓ | 2500 |
iter != nonzeros.end(); ++iter) { |
220 |
✓✓ | 4800 |
for (unsigned int i = 0; i < r; ++i) { |
221 |
✓✗✓✗ |
2400 |
Mcreuse(i, parc) = M(i, *iter); |
222 |
} |
||
223 |
2400 |
parc++; |
|
224 |
} |
||
225 |
|||
226 |
// dynamicgraph::Matrix Mcreuseinv( Mcreuse.nbCols(),r ); |
||
227 |
✓✗✓✗ |
100 |
Mcreuseinv.resize(Mcreuse.cols(), r); |
228 |
✓✗ | 100 |
dynamicgraph::pseudoInverse(Mcreuse, Mcreuseinv); |
229 |
100 |
parc = 0; |
|
230 |
✓✗ | 100 |
Minv.fill(0.); |
231 |
2500 |
for (std::list<unsigned int>::iterator iter = nonzeros.begin(); |
|
232 |
✓✓ | 2500 |
iter != nonzeros.end(); ++iter) { |
233 |
✓✓✓✗ ✓✗ |
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 |
✓✓ | 31 |
for (unsigned int j = 0; j < c; ++j) { |
253 |
30 |
sumsq = 0.; |
|
254 |
✓✓✓✗ ✓✗ |
60 |
for (unsigned int i = 0; i < r; ++i) sumsq += M(i, j) * M(i, j); |
255 |
✓✓ | 30 |
if (sumsq > 1e-6) { |
256 |
✓✗ | 24 |
nonzeros.push_back(j); |
257 |
24 |
parc++; |
|
258 |
} |
||
259 |
} |
||
260 |
|||
261 |
✓✗ | 2 |
dynamicgraph::Matrix Mcreuse(r, parc); |
262 |
1 |
parc = 0; |
|
263 |
25 |
for (std::list<unsigned int>::iterator iter = nonzeros.begin(); |
|
264 |
✓✓ | 25 |
iter != nonzeros.end(); ++iter) { |
265 |
✓✓ | 48 |
for (unsigned int i = 0; i < r; ++i) { |
266 |
✓✗✓✗ |
24 |
Mcreuse(i, parc) = M(i, *iter); |
267 |
} |
||
268 |
24 |
parc++; |
|
269 |
} |
||
270 |
|||
271 |
✓✗✓✗ |
2 |
dynamicgraph::Matrix Mcreuseinv(Mcreuse.cols(), r); |
272 |
1 |
START_CHRONO(inv); |
|
273 |
✓✓ | 101 |
for (int ib = 0; ib < BENCH; ++ib) { |
274 |
✓✗ | 100 |
dynamicgraph::pseudoInverse(Mcreuse, Mcreuseinv); |
275 |
} |
||
276 |
1 |
STOP_CHRONO(inv, "M+creuseseule"); |
|
277 |
|||
278 |
1 |
parc = 0; |
|
279 |
✓✗ | 1 |
Minv.fill(0.); |
280 |
25 |
for (std::list<unsigned int>::iterator iter = nonzeros.begin(); |
|
281 |
✓✓ | 25 |
iter != nonzeros.end(); ++iter) { |
282 |
✓✓✓✗ ✓✗ |
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 |
} |
||
294 |
|||
295 |
1 |
return 0; |
|
296 |
} |
Generated by: GCOVR (Version 4.2) |