GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: tests/tools/test_boost.cpp Lines: 83 83 100.0 %
Date: 2023-03-13 12:09:37 Branches: 92 134 68.7 %

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
}