test_boost.cpp
Go to the documentation of this file.
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 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  unsigned int r = 1;
133  if (argc > 1) r = atoi(argv[1]);
134  unsigned int c = 30;
135  if (argc > 2) c = atoi(argv[2]);
136  static const int BENCH = 100;
137 
138  dynamicgraph::Matrix M(r, c);
139  dynamicgraph::Matrix M1(r, c);
140  dynamicgraph::Matrix Minv(c, r);
141 
142  dynamicgraph::Matrix U, V, S;
143 
144  unsigned int nbzeros = 0;
145  for (unsigned int j = 0; j < c; ++j) {
146  if ((rand() + 1.) / RAND_MAX > .8) {
147  for (unsigned int i = 0; i < r; ++i) M(i, j) = 0.;
148  nbzeros++;
149  } else
150  for (unsigned int i = 0; i < r; ++i)
151  M(i, j) = (rand() + 1.) / RAND_MAX * 2 - 1;
152  }
153  for (unsigned int i = 0; i < r; ++i)
154  for (unsigned int j = 0; j < c; ++j)
155  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  START_CHRONO(inv);
164  for (int i = 0; i < BENCH; ++i) dynamicgraph::pseudoInverse(M, Minv);
165  STOP_CHRONO(inv, "init");
166  sotDEBUG(15) << "Minv = " << Minv << endl;
167 
168  START_CHRONO(inv);
169  for (int i = 0; i < BENCH; ++i) dynamicgraph::pseudoInverse(M, Minv);
170  STOP_CHRONO(inv, "M+standard");
171  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  START_CHRONO(inv);
195  std::list<unsigned int> nonzeros;
196  dynamicgraph::Matrix Mcreuse;
197  dynamicgraph::Matrix Mcreuseinv;
198  for (int ib = 0; ib < BENCH; ++ib) {
199  double sumsq;
200  unsigned int parc = 0;
201  if (!ib) {
202  nonzeros.clear();
203  for (unsigned int j = 0; j < c; ++j) {
204  sumsq = 0.;
205  for (unsigned int i = 0; i < r; ++i) sumsq += M(i, j) * M(i, j);
206  if (sumsq > 1e-6) {
207  nonzeros.push_back(j);
208  parc++;
209  }
210  }
211 
212  Mcreuse.resize(r, parc);
213  }
214 
215  // dynamicgraph::Matrix Mcreuse( r,parc );
216 
217  parc = 0;
218  for (std::list<unsigned int>::iterator iter = nonzeros.begin();
219  iter != nonzeros.end(); ++iter) {
220  for (unsigned int i = 0; i < r; ++i) {
221  Mcreuse(i, parc) = M(i, *iter);
222  }
223  parc++;
224  }
225 
226  // dynamicgraph::Matrix Mcreuseinv( Mcreuse.nbCols(),r );
227  Mcreuseinv.resize(Mcreuse.cols(), r);
228  dynamicgraph::pseudoInverse(Mcreuse, Mcreuseinv);
229  parc = 0;
230  Minv.fill(0.);
231  for (std::list<unsigned int>::iterator iter = nonzeros.begin();
232  iter != nonzeros.end(); ++iter) {
233  for (unsigned int i = 0; i < r; ++i) Minv(*iter, i) = Mcreuseinv(parc, i);
234  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  STOP_CHRONO(inv, "M+creuse");
246  // sotDEBUG(15) << dynamicgraph::MATLAB <<"Minv = "<< Minv <<endl;
247 
248  {
249  double sumsq;
250  nonzeros.clear();
251  unsigned int parc = 0;
252  for (unsigned int j = 0; j < c; ++j) {
253  sumsq = 0.;
254  for (unsigned int i = 0; i < r; ++i) sumsq += M(i, j) * M(i, j);
255  if (sumsq > 1e-6) {
256  nonzeros.push_back(j);
257  parc++;
258  }
259  }
260 
261  dynamicgraph::Matrix Mcreuse(r, parc);
262  parc = 0;
263  for (std::list<unsigned int>::iterator iter = nonzeros.begin();
264  iter != nonzeros.end(); ++iter) {
265  for (unsigned int i = 0; i < r; ++i) {
266  Mcreuse(i, parc) = M(i, *iter);
267  }
268  parc++;
269  }
270 
271  dynamicgraph::Matrix Mcreuseinv(Mcreuse.cols(), r);
272  START_CHRONO(inv);
273  for (int ib = 0; ib < BENCH; ++ib) {
274  dynamicgraph::pseudoInverse(Mcreuse, Mcreuseinv);
275  }
276  STOP_CHRONO(inv, "M+creuseseule");
277 
278  parc = 0;
279  Minv.fill(0.);
280  for (std::list<unsigned int>::iterator iter = nonzeros.begin();
281  iter != nonzeros.end(); ++iter) {
282  for (unsigned int i = 0; i < r; ++i) Minv(*iter, i) = Mcreuseinv(parc, i);
283  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  return 0;
296 }
utils-windows.hh
START_CHRONO
#define START_CHRONO(name)
Definition: test_boost.cpp:34
V
V
c
Vec3f c
i
int i
dynamicgraph::pseudoInverse
void pseudoInverse(Matrix &_inputMatrix, Matrix &_inverseMatrix, const double threshold=1e-6)
Definition: matrix-svd.cpp:12
dynamicgraph::Matrix
Eigen::MatrixXd Matrix
STOP_CHRONO
#define STOP_CHRONO(name, commentaire)
Definition: test_boost.cpp:38
r
FCL_REAL r
debug.hh
sotDEBUG_ENABLE
#define sotDEBUG_ENABLE(level)
Definition: debug.hh:209
M
M
memory-task-sot.hh
linear-algebra.h
matrix-svd.hh
timerCounter
double timerCounter
Definition: test_boost.cpp:52
matrix-geometry.hh
U
U
dynamicgraph::sot
INIT_CHRONO
#define INIT_CHRONO(name)
Definition: test_boost.cpp:31
dynamicgraph::DebugTrace::openFile
static void openFile(const char *filename=DEBUG_FILENAME_DEFAULT)
rand
def rand(n)
main
int main(int argc, char **argv)
Definition: test_boost.cpp:127
sotDEBUG
#define sotDEBUG(level)
Definition: debug.hh:168

sot-core
Author(s): Olivier Stasse, ostasse@laas.fr
autogenerated on Tue Oct 24 2023 02:26:32