timings-eigen.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2015-2016 CNRS
3 //
4 
5 #include <iostream>
6 #include "pinocchio/utils/timer.hpp"
7 #include <Eigen/Dense>
8 #include <Eigen/Geometry>
9 #include <Eigen/StdVector>
10 
11 using namespace Eigen;
12 
13 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Quaternion<double>)
14 
15 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Matrix<double,1,1,RowMajor>)
16 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Matrix<double,2,2,RowMajor>)
17 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Matrix<double,3,3,RowMajor>)
18 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Matrix<double,4,4,RowMajor>)
19 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Matrix<double,5,5,RowMajor>)
20 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Matrix<double,6,6,RowMajor>)
21 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Matrix<double,7,7,RowMajor>)
22 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Matrix<double,8,8,RowMajor>)
23 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Matrix<double,9,9,RowMajor>)
24 
25 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Matrix<double,1,1,ColMajor>)
26 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Matrix<double,2,2,ColMajor>)
27 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Matrix<double,3,3,ColMajor>)
28 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Matrix<double,4,4,ColMajor>)
29 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Matrix<double,5,5,ColMajor>)
30 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Matrix<double,6,6,ColMajor>)
31 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Matrix<double,7,7,ColMajor>)
32 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Matrix<double,8,8,ColMajor>)
33 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Matrix<double,9,9,ColMajor>)
34 
35 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Matrix<double,2,1>)
36 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Matrix<double,3,1>)
37 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Matrix<double,4,1>)
38 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Matrix<double,5,1>)
39 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Matrix<double,6,1>)
40 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Matrix<double,7,1>)
41 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Matrix<double,8,1>)
42 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Matrix<double,9,1>)
43 
44 template<int NBT>
45 void checkQuaternionToMatrix(std::string label)
46 {
47 
48  using namespace Eigen;
49 
50  PinocchioTicToc timer(PinocchioTicToc::NS);
51 
52  /* Random pre-initialization of all matrices in the stack. */
53  std::vector< Quaterniond > q1s (NBT);
54  std::vector< Matrix3d > R3s (NBT);
55  for(size_t i=0;i<NBT;++i)
56  {
57  q1s[i] = Quaterniond(Vector4d::Random()).normalized();
58  R3s[i] = Matrix3d::Random();
59  }
60 
61  /* Timed product. */
62  timer.tic();
63  SMOOTH(NBT)
64  {
65  R3s[_smooth] = q1s[_smooth].toRotationMatrix();
66  }
67 
68  std::cout << label << " = \t\t" ; timer.toc(std::cout,NBT);
69 }
70 
71 template<int NBT>
72 void checkQuaternion(std::string label)
73 {
74 
75  using namespace Eigen;
76 
77  PinocchioTicToc timer(PinocchioTicToc::NS);
78 
79  /* Random pre-initialization of all matrices in the stack. */
80  std::vector< Quaterniond > q1s (NBT);
81  std::vector< Vector3d > v2s (NBT);
82  std::vector< Vector3d > v3s (NBT);
83  for(size_t i=0;i<NBT;++i)
84  {
85  q1s[i] = Quaterniond(Vector4d::Random()).normalized();
86  v2s[i] = Vector3d::Random();
87  v3s[i] = Vector3d::Random();
88  }
89 
90  /* Timed product. */
91  timer.tic();
92  SMOOTH(NBT)
93  {
94  v3s[_smooth] = q1s[_smooth]*v2s[_smooth];
95  }
96 
97  std::cout << label << " = \t\t" ; timer.toc(std::cout,NBT);
98 }
99 
100 template<int NBT>
101 void checkQuaternionQuaternion(std::string label)
102 {
103 
104  using namespace Eigen;
105 
106  PinocchioTicToc timer(PinocchioTicToc::NS);
107 
108  /* Random pre-initialization of all matrices in the stack. */
109  std::vector< Quaterniond > q1s (NBT);
110  std::vector< Quaterniond > q2s (NBT);
111  std::vector< Quaterniond > q3s (NBT);
112  for(size_t i=0;i<NBT;++i)
113  {
114  q1s[i] = Quaterniond(Vector4d::Random()).normalized();
115  q2s[i] = Quaterniond(Vector4d::Random()).normalized();
116  q3s[i] = Quaterniond(Vector4d::Random()).normalized();
117  }
118 
119  /* Timed product. */
120  timer.tic();
121  SMOOTH(NBT)
122  {
123  q3s[_smooth] = q1s[_smooth]*q2s[_smooth];
124  }
125 
126  std::cout << label << " = \t\t" ; timer.toc(std::cout,NBT);
127 }
128 
129 template<int NBT>
130 void checkQuaternionD(std::string label)
131 {
132 
133  using namespace Eigen;
134 
135  PinocchioTicToc timer(PinocchioTicToc::NS);
136 
137  /* Random pre-initialization of all matrices in the stack. */
138  std::vector< Quaterniond > q1s (NBT);
139  std::vector< VectorXd > v2s (NBT);
140  std::vector< VectorXd > v3s (NBT);
141  for(size_t i=0;i<NBT;++i)
142  {
143  q1s[i] = Quaterniond(Vector4d::Random()).normalized();
144  v2s[i] = VectorXd::Random(3);
145  v3s[i] = VectorXd::Random(3);
146  }
147 
148  /* Timed product. */
149  timer.tic();
150  SMOOTH(NBT)
151  {
152  v3s[_smooth] = q1s[_smooth]*v2s[_smooth];
153  }
154 
155  std::cout << label << " = \t\t" ; timer.toc(std::cout,NBT);
156 }
157 
158 template<int MSIZE,int NBT>
159 void checkMatrix(std::string label)
160 {
161  /* Row/Col organization of the three matrices M3=M1*M2 */
162  #define RC1 RowMajor
163  #define RC2 RowMajor
164  #define RC3 RowMajor
165 
166  using namespace Eigen;
167 
168  PinocchioTicToc timer(PinocchioTicToc::NS);
169 
170  /* Random pre-initialization of all matrices in the stack. */
171  std::vector< Matrix<double,MSIZE,MSIZE,RC1> > R1s (NBT);
172  std::vector< Matrix<double,MSIZE,MSIZE,RC2> > R2s (NBT);
173  std::vector< Matrix<double,MSIZE,MSIZE,RC3> > R3s (NBT);
174  for(size_t i=0;i<NBT;++i)
175  {
176  R1s[i] = Matrix<double,MSIZE,MSIZE,RC1>::Random();
177  R2s[i] = Matrix<double,MSIZE,MSIZE,RC2>::Random();
178  R3s[i] = Matrix<double,MSIZE,MSIZE,RC3>::Random();
179  }
180 
181  /* Timed product. */
182  timer.tic();
183  SMOOTH(NBT)
184  {
185  R3s[_smooth] = R1s[_smooth]*R2s[_smooth];
186  }
187 
188  std::cout << label << " = \t\t" ; timer.toc(std::cout,NBT);
189 }
190 
191 template<int MSIZE,int NBT>
192 void checkVector(std::string label)
193 {
194  using namespace Eigen;
195 
196  PinocchioTicToc timer(PinocchioTicToc::NS);
197  std::vector< Matrix<double,MSIZE,MSIZE> > R1s (NBT);
198  std::vector< Matrix<double,MSIZE,1> > v2s (NBT);
199  std::vector< Matrix<double,MSIZE,1> > v3s (NBT);
200  for(size_t i=0;i<NBT;++i)
201  {
202  R1s[i] = Matrix<double,MSIZE,MSIZE>::Random();
203  v2s[i] = Matrix<double,MSIZE,1>::Random();
204  v3s[i] = Matrix<double,MSIZE,1>::Random();
205  }
206 
207  timer.tic();
208  SMOOTH(NBT)
209  {
210  v3s[_smooth] = R1s[_smooth]*v2s[_smooth];
211  }
212 
213  std::cout << label << " = \t\t"; timer.toc(std::cout,NBT);
214 }
215 
216 template<int NBT>
217 void checkDMatrix(int MSIZE,std::string label)
218 {
219  using namespace Eigen;
220 
221  PinocchioTicToc timer(PinocchioTicToc::NS);
222  std::vector< MatrixXd > R1s (NBT);
223  std::vector< MatrixXd > R2s (NBT);
224  std::vector< MatrixXd > R3s (NBT);
225  for(size_t i=0;i<NBT;++i)
226  {
227  R1s[i] = MatrixXd::Random(MSIZE,MSIZE);
228  R2s[i] = MatrixXd::Random(MSIZE,MSIZE);
229  R3s[i] = MatrixXd::Random(MSIZE,MSIZE);
230  }
231 
232  timer.tic();
233  SMOOTH(NBT)
234  {
235  R3s[_smooth] = R1s[_smooth]*R2s[_smooth];
236  }
237 
238  std::cout << label << " = \t\t"; timer.toc(std::cout,NBT);
239 }
240 
241 template<int NBT>
242 void checkDVector(int MSIZE,std::string label)
243 {
244  using namespace Eigen;
245 
246  PinocchioTicToc timer(PinocchioTicToc::NS);
247  std::vector< MatrixXd > R1s (NBT);
248  std::vector< MatrixXd > v2s (NBT);
249  std::vector< MatrixXd > v3s (NBT);
250  for(size_t i=0;i<NBT;++i)
251  {
252  R1s[i] = MatrixXd::Random(MSIZE,MSIZE);
253  v2s[i] = MatrixXd::Random(MSIZE,1);
254  v3s[i] = MatrixXd::Random(MSIZE,1);
255  }
256 
257  timer.tic();
258  SMOOTH(NBT)
259  {
260  v3s[_smooth] = R1s[_smooth]*v2s[_smooth];
261  }
262 
263  std::cout << label << " = \t\t"; timer.toc(std::cout,NBT);
264 }
265 
266 
267 int main()
268 {
269 
270  #ifdef NDEBUG
271  const int NBT = 1000*1000;
272  #else
273  const int NBT = 1;
274  std::cout << "(the time score in debug mode is not relevant) " << std::endl;
275  #endif
276  checkQuaternion<NBT>( "quaternion-vector static ");
277  checkQuaternionD<NBT>( "quaternion-vector dynamic");
278  checkQuaternionQuaternion<NBT>( "quaternion-quaternion ");
279  checkQuaternionToMatrix<NBT>( "quaternion->matrix static");
280  std::cout << std::endl;
281 
282  checkVector<3,NBT> ( "matrix-vector static 3x3");
283  checkDVector<NBT> (3,"matrix-vector dynamic 3x3");
284  checkMatrix<3,NBT> ( "matrix-matrix static 3x3");
285  checkDMatrix<NBT> (3,"matrix-matrix dynamic 3x3");
286  std::cout << std::endl;
287 
288  checkVector<4,NBT> ( "matrix-vector static 4x4");
289  checkDVector<NBT> (4,"matrix-vector dynamic 4x4");
290  checkMatrix<4,NBT> ( "matrix-matrix static 4x4");
291  checkDMatrix<NBT> (4,"matrix-matrix dynamic 4x4");
292  std::cout << std::endl;
293 
294  std::cout << "--" << std::endl;
295  return 0;
296 }
PinocchioTicToc::toc
double toc()
Definition: timer.hpp:68
checkMatrix
void checkMatrix(std::string label)
Definition: timings-eigen.cpp:159
checkVector
void checkVector(std::string label)
Definition: timings-eigen.cpp:192
PinocchioTicToc::tic
void tic()
Definition: timer.hpp:63
checkQuaternionQuaternion
void checkQuaternionQuaternion(std::string label)
Definition: timings-eigen.cpp:101
checkDMatrix
void checkDMatrix(int MSIZE, std::string label)
Definition: timings-eigen.cpp:217
checkQuaternionToMatrix
void checkQuaternionToMatrix(std::string label)
Definition: timings-eigen.cpp:45
SMOOTH
#define SMOOTH(s)
Definition: timer.hpp:38
checkQuaternionD
void checkQuaternionD(std::string label)
Definition: timings-eigen.cpp:130
main
int main()
Definition: timings-eigen.cpp:267
checkQuaternion
void checkQuaternion(std::string label)
Definition: timings-eigen.cpp:72
checkDVector
void checkDVector(int MSIZE, std::string label)
Definition: timings-eigen.cpp:242

pinocchio
Author(s):
autogenerated on Tue Jun 1 2021 02:45:04