eigen3_transforms.cpp
Go to the documentation of this file.
1 
9 /*****************************************************************************
10 ** Includes
11 *****************************************************************************/
12 
13 #include <iostream>
14 #include <vector>
15 #include <ecl/config/macros.hpp>
16 #include <ecl/threads/priority.hpp>
17 #include <ecl/time/duration.hpp>
18 #include <ecl/time/cpuwatch.hpp>
19 #include <ecl/time/stopwatch.hpp>
20 #include <ecl/time/time_data.hpp>
21 #include <ecl/linear_algebra.hpp>
22 
23 /*****************************************************************************
24 ** Using
25 *****************************************************************************/
26 
27 using namespace ecl;
28 using namespace Eigen;
29 using std::vector;
30 
31 /*****************************************************************************
32 ** Pose2D
33 *****************************************************************************/
34 
35 class NewPose2D {
36 public:
37  NewPose2D(){}
38  NewPose2D(const double &x, const double &y, const double& theta) :
39  rotation(Rotation2D<double>(theta).toRotationMatrix())
40  {
41  translation << x, y;
42  }
43  NewPose2D operator*(const NewPose2D& pose) const {
44  NewPose2D new_pose;
45  new_pose.translation = translation+rotation*pose.translation;
46  new_pose.rotation = rotation*pose.rotation;
47  return new_pose;
48  }
49  NewPose2D inverse() const {
50  NewPose2D p;
51  p.translation = rotation*translation;
52  p.rotation = -1*rotation;
53  return p;
54  }
55  void print() {
56  Rotation2D<double> angle(0.0);
57  angle.fromRotationMatrix(rotation);
58  std::cout << "[ " << translation[0] << "," << translation[1] << "," << angle.angle() << "]" << std::endl;
59  }
60 
61  Matrix2d rotation;
62  Vector2d translation;
63 };
64 
65 class Pose2D {
66 public:
67  Pose2D(const double &x_=0.0, const double &y_=0.0, const double&theta=0.0) :
68  x(x_), y(y_), rotation(theta)
69  {}
70  Pose2D(const Pose2D& pose) : x(pose.x), y(pose.y), rotation(pose.rotation) {}
71  Pose2D inverse() const {
72  double s = sin(rotation);
73  double c = cos(rotation);
74  return Pose2D(c*x-s*y, s*x+c*y, -rotation);
75  }
76 
77  Pose2D operator*(const Pose2D& pose) const {
78  double s = sin(rotation);
79  double c = cos(rotation);
80  Pose2D new_pose(x+c*pose.x-s*pose.y,
81  y+s*pose.x + c*pose.y,
82  rotation+pose.rotation);
83  while (new_pose.rotation > +3.14) { new_pose.rotation -= 2*3.14; }
84  while (new_pose.rotation < -3.14) { new_pose.rotation += 2*3.14; }
85  return new_pose;
86  }
87 
88  void print() {
89  std::cout << "[ " << x << "," << y << "," << rotation << "]" << std::endl;
90  }
91  double x, y;
92  double rotation;
93 };
94 
95 /*****************************************************************************
96 ** Globals
97 *****************************************************************************/
98 
99 const unsigned int REPEAT = 1000;
100 const double increment = 1.0/static_cast<double>(REPEAT);
101 
102 double random_float() {
103  return static_cast<double>(rand()%99)/100.0;
104 }
105 
106 Matrix3d rotation3D(const double &alpha, const double &beta, const double &gamma) {
107  Matrix3d Ralpha, Rbeta, Rgamma;
108  Ralpha << 1.0, 0.0, 0.0,
109  0.0, cos(alpha), -sin(alpha),
110  0.0, sin(alpha), cos(alpha);
111  Rbeta << cos(beta), 0.0, sin(beta),
112  0.0, 1.0, 0.0,
113  -sin(beta), 0.0, cos(beta);
114  Rgamma << cos(gamma), -sin(gamma), 0.0,
115  sin(gamma), cos(gamma), 0.0,
116  0.0, 0.0, 1.0;
117  return Ralpha*Rbeta*Rgamma;
118 }
119 
120 
121 template <typename MatrixType>
122 ECL_DONT_INLINE Duration product(const MatrixType &a, const MatrixType &b, MatrixType &c) {
123  StopWatch stopwatch;
124  c = a*a;
125  c = a*b;
126  c = b*b;
127  return stopwatch.split();
128 }
129 
130 template <typename MatrixType>
131 ECL_DONT_INLINE Duration inverse(const MatrixType &a, const MatrixType &b, MatrixType &c) {
132  StopWatch stopwatch;
133  c = a.inverse();
134  c = b.inverse();
135  c = a.inverse();
136  return stopwatch.split();
137 }
138 
139 template<typename Pose>
140 ECL_DONT_INLINE TimeData pose2DTest(bool inverse_test = false) {
141  TimeData times;
142  Pose p1, p2, p3;
143  for ( unsigned int i = 0; i < REPEAT; ++i ) {
144  p1 = Pose(i*increment, 2*i*increment, 3.14*i*increment);
145  p2 = Pose(1.2*i*increment, 1.2*2*i*increment, 1.14*i*increment);
146  if ( inverse_test ) {
147  times.push_back(inverse(p1,p2,p3));
148  } else {
149  times.push_back(product(p1,p2,p3));
150  }
151  }
152  return times;
153 }
154 
155 template <typename MatrixType>
156 ECL_DONT_INLINE TimeData transform2DTest(bool inverse_test = false) {
157  TimeData times;
158  MatrixType t1 = MatrixType::Identity(), t2(t1), t3(t1);
159  for ( unsigned int i = 0; i < REPEAT; ++i ) {
160  t1.linear() = Rotation2D<double>(3.14*i*increment).toRotationMatrix();
161  Vector2d v; v << i*increment, 2*i*increment;
162  t1.translation() = v;
163  t2.linear() = Rotation2D<double>(1.14*i*increment).toRotationMatrix();
164  t2.translation() = 1.2*v;
165  if ( inverse_test ) {
166  times.push_back(inverse(t1,t2,t3));
167  } else {
168  times.push_back(product(t1,t2,t3));
169  }
170  }
171  return times;
172 }
173 
174 template <typename MatrixType>
175 ECL_DONT_INLINE TimeData transform3DTest(bool inverse_test = false) {
176  TimeData times;
177  MatrixType t1 = MatrixType::Identity(), t2(t1), t3(t1);
178  for ( unsigned int i = 0; i < REPEAT; ++i ) {
179  t1.linear() = rotation3D(3.14*i*increment,-3.14*i*increment, 1.0+i*increment );
180  Vector3d v; v << i*increment, 2*i*increment, 3*i*increment;
181  t1.translation() = v;
182  t2.linear() = rotation3D(3.14*i*increment,-3.14*i*increment, 1.0+i*increment );
183  t2.translation() = 1.2*v;
184  if ( inverse_test ) {
185  times.push_back(inverse(t1,t2,t3));
186  } else {
187  times.push_back(product(t1,t2,t3));
188  }
189  }
190  return times;
191 }
192 
193 ECL_DONT_INLINE TimeData rotTrans3DTest(bool inverse_test = false) {
194  StopWatch stopwatch;
195  TimeData times;
196  Matrix3d rot1 = Matrix3d::Identity(), rot2(rot1), rot3;
197  Vector3d trans1, trans2, trans3;
198  for ( unsigned int i = 0; i < REPEAT; ++i ) {
199  rot1 = rotation3D(3.14*i*increment,-3.14*i*increment, 1.0+i*increment );
200  rot1 = rotation3D(1.14*i*increment,-1.14*i*increment, 1.2+i*increment );
201  trans1 << i*increment, 2*i*increment, 3*i*increment;
202  trans1 << 2*i*increment, 1*i*increment, 3*i*increment;
203  if ( inverse_test ) {
204  stopwatch.restart();
205  rot3 = rot2*rot1;
206  trans3 = rot2*trans1 + trans2;
207  rot3 = rot1*rot1;
208  trans3 = rot1*trans1 + trans1;
209  rot3 = rot2*rot2;
210  trans3 = rot2*trans2 + trans2;
211  times.push_back(stopwatch.split());
212  } else { // inverse
213  stopwatch.restart();
214  rot3 = rot1.transpose();
215  trans3 = rot3*trans1*-1;
216  rot3 = rot2.transpose();
217  trans3 = rot3*trans2*-1;
218  rot2 = rot3.transpose();
219  trans2 = rot2*trans3*-1;
220  times.push_back(stopwatch.split());
221  }
222  }
223  return times;
224 }
225 
226 ECL_DONT_INLINE TimeData matrix3DTest(bool inverse_test = false) {
227  TimeData times;
228  Matrix4d t1 = Matrix4d::Identity(), t2(t1), t3(t1);
229  for ( unsigned int i = 0; i < REPEAT; ++i ) {
230  t1.block<3,3>(0,0) = rotation3D(3.14*i*increment,-3.14*i*increment, 1.0+i*increment );
231  Vector3d v; v << i*increment, 2*i*increment, 3*i*increment;
232  t1.block<3,1>(0,3) = v;
233  t2.block<3,3>(0,0) = rotation3D(3.14*i*increment,-3.14*i*increment, 1.0+i*increment );
234  t2.block<3,1>(0,3) = 1.2*v;
235  if ( inverse_test ) {
236  times.push_back(inverse(t1,t2,t3));
237  } else {
238  times.push_back(product(t1,t2,t3));
239  }
240  }
241  return times;
242 }
243 
244 ECL_DONT_INLINE vector<Duration> trigTest() {
245  StopWatch stopwatch;
246  vector<Duration> times;
247  double angle1 = 0.0, angle2 = 1.0, d, e;
248  const unsigned long repeats = 1000000000L;
249  double inc = 3.14/static_cast<double>(repeats);
250  stopwatch.restart();
251  for ( unsigned long i = 0; i < repeats; ++i ) {
252  angle1 += inc;
253  angle2 -= inc;
254  d = sin(angle1);
255  e = cos(angle2);
256  }
257  times.push_back(stopwatch.split());
258  stopwatch.restart();
259  for ( unsigned long i = 0; i < repeats; ++i ) {
260  angle1 += inc;
261  angle2 -= inc;
262  d = angle1*angle2;
263  e = 3.315*angle1;
264  }
265  times.push_back(stopwatch.split());
266  return times;
267 }
268 /*****************************************************************************
269 ** Main
270 *****************************************************************************/
271 
272 int main(int argc, char **argv) {
273 
274  try {
275  ecl::set_priority(ecl::RealTimePriority4);
276  } catch ( ecl::StandardException &e ) {
277  // dont worry about it.
278  }
279 
280  TimeData times;
281  const bool inverse_test = true; // Just a flag
282 
283  std::cout << std::endl;
284  std::cout << "***********************************************************" << std::endl;
285  std::cout << " Sizes" << std::endl;
286  std::cout << "***********************************************************" << std::endl;
287  std::cout << std::endl;
288 
289  std::cout << "Matrix4d : " << sizeof(Matrix4d) << std::endl;
290  std::cout << "3x3+3x1 : " << sizeof(Matrix3d) + sizeof(Vector3d) << std::endl;
291  std::cout << "Affine3D : " << sizeof(Transform<double,3,Affine>) << std::endl;
292  std::cout << "AffineCom3D : " << sizeof(Transform<double,3,AffineCompact>) << std::endl;
293  std::cout << "Isometr3D : " << sizeof(Transform<double,3,Isometry>) << std::endl;
294  std::cout << "Rotation2D : " << sizeof(Rotation2D<double>) << std::endl;
295  std::cout << "Translation3D: " << sizeof(Translation<double,3>) << std::endl;
296  std::cout << "Quaternion : " << sizeof(Quaternion<double>) << std::endl;
297 //
298  std::cout << std::endl;
299  std::cout << "***********************************************************" << std::endl;
300  std::cout << " Trig vs *" << std::endl;
301  std::cout << "***********************************************************" << std::endl;
302  std::cout << std::endl;
303 
304  vector<Duration> trig_times = trigTest();
305  std::cout << "Cos+Sin: " << trig_times[0] << std::endl;
306  std::cout << "Two *'s: " << trig_times[1] << std::endl;
307 
308  std::cout << std::endl;
309  std::cout << "***********************************************************" << std::endl;
310  std::cout << " Products 2D" << std::endl;
311  std::cout << "***********************************************************" << std::endl;
312  std::cout << std::endl;
313 
314  times.clear(); times = pose2DTest<Pose2D>();
315  std::cout << "Pose2D : " << times.average() << " " << times.stdDev() << std::endl;
316  times.clear(); times = pose2DTest<NewPose2D>();
317  std::cout << "NewPose2D : " << times.average() << " " << times.stdDev() << std::endl;
318  times.clear(); times = transform2DTest< Transform<double,2,Affine> >();
319  std::cout << "Affine : " << times.average() << " " << times.stdDev() << std::endl;
320  times.clear(); times = transform2DTest< Transform<double,2,AffineCompact> >();
321  std::cout << "CompactAffine: " << times.average() << " " << times.stdDev() << std::endl;
322  times.clear(); times = transform2DTest< Transform<double,2,Isometry> >();
323  std::cout << "Isometry : " << times.average() << " " << times.stdDev() << std::endl;
324  std::cout << "CompactIso : N/A" << std::endl;
325 
326  std::cout << std::endl;
327  std::cout << "***********************************************************" << std::endl;
328  std::cout << " Inverse 2D" << std::endl;
329  std::cout << "***********************************************************" << std::endl;
330  std::cout << std::endl;
331 
332  times.clear(); times = pose2DTest<Pose2D>(inverse_test);
333  std::cout << "Pose2D : " << times.average() << " " << times.stdDev() << std::endl;
334  times.clear(); times = pose2DTest<NewPose2D>(inverse_test);
335  std::cout << "NewPose2D : " << times.average() << " " << times.stdDev() << std::endl;
336  times.clear(); times = transform2DTest< Transform<double,2,Affine> >(inverse_test);
337  std::cout << "Affine : " << times.average() << " " << times.stdDev() << std::endl;
338  times.clear(); times = transform2DTest< Transform<double,2,AffineCompact> >(inverse_test);
339  std::cout << "CompactAffine: " << times.average() << " " << times.stdDev() << std::endl;
340  times.clear(); times = transform2DTest< Transform<double,2,Isometry> >(inverse_test);
341  std::cout << "Isometry : " << times.average() << " " << times.stdDev() << std::endl;
342  std::cout << "CompactIso : N/A" << std::endl;
343 
344  std::cout << std::endl;
345  std::cout << "***********************************************************" << std::endl;
346  std::cout << " Products 3D" << std::endl;
347  std::cout << "***********************************************************" << std::endl;
348  std::cout << std::endl;
349 
350  times.clear(); times = rotTrans3DTest();
351  std::cout << "Rot+Tra : " << times.average() << " " << times.stdDev() << std::endl;
352  times.clear(); times = matrix3DTest();
353  std::cout << "Matrix4d : " << times.average() << " " << times.stdDev() << std::endl;
354  times.clear(); times = transform3DTest< Transform<double,3,Affine> >();
355  std::cout << "Affine : " << times.average() << " " << times.stdDev() << std::endl;
356  times.clear(); times = transform3DTest< Transform<double,3,AffineCompact> >();
357  std::cout << "CompactAffine: " << times.average() << " " << times.stdDev() << std::endl;
358  times.clear(); times = transform3DTest< Transform<double,3,Isometry> >();
359  std::cout << "Isometry : " << times.average() << " " << times.stdDev() << std::endl;
360  std::cout << "CompactIso : N/A" << std::endl;
361 
362  std::cout << std::endl;
363  std::cout << "***********************************************************" << std::endl;
364  std::cout << " Inverse 3D" << std::endl;
365  std::cout << "***********************************************************" << std::endl;
366  std::cout << std::endl;
367 
368  times.clear(); times = rotTrans3DTest(inverse_test);
369  std::cout << "Rot+Tra : " << times.average() << " " << times.stdDev() << std::endl;
370  times.clear(); times = matrix3DTest(inverse_test);
371  std::cout << "Matrix4d : " << times.average() << " " << times.stdDev() << std::endl;
372  times.clear(); times = transform3DTest< Transform<double,3,Affine> >(inverse_test);
373  std::cout << "Affine : " << times.average() << " " << times.stdDev() << std::endl;
374  times.clear(); times = transform3DTest< Transform<double,3,AffineCompact> >(inverse_test);
375  std::cout << "CompactAffine: " << times.average() << " " << times.stdDev() << std::endl;
376  times.clear(); times = transform3DTest< Transform<double,3,Isometry> >(inverse_test);
377  std::cout << "Isometry : " << times.average() << " " << times.stdDev() << std::endl;
378  std::cout << "CompactIso : N/A" << std::endl;
379 
380  std::cout << std::endl;
381 
382 // for ( unsigned int i = 0; i < times.data().size(); ++i ) {
383 // std::cout << times.data()[i] << std::endl;
384 // }
385 
386 // std::cout << std::endl;
387 // std::cout << "***********************************************************" << std::endl;
388 // std::cout << " Pose Unit Tests" << std::endl;
389 // std::cout << "***********************************************************" << std::endl;
390 // std::cout << std::endl;
391 //
392 // Pose2D p1(1.0,0.0,1.57), p2(0.0,1.0,1.57), p3;
393 // NewPose2D o1(1.0,0.0,1.57), o2(0.0,1.0,1.57), o3;
394 // p3 = p2*p1;
395 // o3 = o2*o1;
396 // p3.print();
397 // o3.print();
398 // p3 = p2.inverse();
399 // o3 = o2.inverse();
400 // p3.print();
401 // o3.print();
402  return 0;
403 }
Pose2D::Pose2D
Pose2D(const Pose2D &pose)
Definition: eigen3_transforms.cpp:70
Eigen
NewPose2D
Definition: eigen3_transforms.cpp:35
ECL_DONT_INLINE
#define ECL_DONT_INLINE
rotation3D
Matrix3d rotation3D(const double &alpha, const double &beta, const double &gamma)
Definition: eigen3_transforms.cpp:106
Pose2D::x
double x
Definition: eigen3_transforms.cpp:91
Pose2D::operator*
Pose2D operator*(const Pose2D &pose) const
Definition: eigen3_transforms.cpp:77
Matrix4d
Matrix4< double > Matrix4d
cpuwatch.hpp
ecl::TimeData
linear_algebra.hpp
Matrix2d
Matrix2< double > Matrix2d
Matrix3d
Matrix3< double > Matrix3d
transform3DTest
ECL_DONT_INLINE TimeData transform3DTest(bool inverse_test=false)
Definition: eigen3_transforms.cpp:175
increment
const double increment
Definition: eigen3_transforms.cpp:100
rotTrans3DTest
ECL_DONT_INLINE TimeData rotTrans3DTest(bool inverse_test=false)
Definition: eigen3_transforms.cpp:193
transform2DTest
ECL_DONT_INLINE TimeData transform2DTest(bool inverse_test=false)
Definition: eigen3_transforms.cpp:156
Vector3d
Vector3< double > Vector3d
Pose2D::rotation
double rotation
Definition: eigen3_transforms.cpp:92
Vector2d
Vector2< double > Vector2d
product
ECL_DONT_INLINE Duration product(const MatrixType &a, const MatrixType &b, MatrixType &c)
Definition: eigen3_transforms.cpp:122
time_data.hpp
d
void d()
ecl::StandardException
ecl::TimeData::clear
void clear()
ecl::TimeData::average
ecl::Duration average() const
trigTest
ECL_DONT_INLINE vector< Duration > trigTest()
Definition: eigen3_transforms.cpp:244
Pose2D::print
void print()
Definition: eigen3_transforms.cpp:88
ecl::Duration
TimeStamp Duration
pose2DTest
ECL_DONT_INLINE TimeData pose2DTest(bool inverse_test=false)
Definition: eigen3_transforms.cpp:140
stopwatch.hpp
print
void print(const Container &byte_array)
ecl::TimeData::stdDev
ecl::Duration stdDev() const
Pose2D::inverse
Pose2D inverse() const
Definition: eigen3_transforms.cpp:71
random_float
double random_float()
Definition: eigen3_transforms.cpp:102
REPEAT
const unsigned int REPEAT
Definition: eigen3_transforms.cpp:99
Pose2D::y
double y
Definition: eigen3_transforms.cpp:91
main
int main(int argc, char **argv)
Definition: eigen3_transforms.cpp:272
matrix3DTest
ECL_DONT_INLINE TimeData matrix3DTest(bool inverse_test=false)
Definition: eigen3_transforms.cpp:226
priority.hpp
NewPose2D::translation
Vector2d translation
Definition: eigen3_transforms.cpp:64
macros.hpp
ecl::TimeData::push_back
void push_back(const ecl::Duration &duration)
ecl::RealTimePriority4
RealTimePriority4
inverse
ECL_DONT_INLINE Duration inverse(const MatrixType &a, const MatrixType &b, MatrixType &c)
Definition: eigen3_transforms.cpp:131
ecl
duration.hpp
Pose2D
Definition: eigen3_transforms.cpp:65
NewPose2D::rotation
Matrix2d rotation
Definition: eigen3_transforms.cpp:63
Pose2D::Pose2D
Pose2D(const double &x_=0.0, const double &y_=0.0, const double &theta=0.0)
Definition: eigen3_transforms.cpp:67


ecl_core_apps
Author(s): Daniel Stonier
autogenerated on Wed Mar 2 2022 00:16:52