1
2#include <iostream>
3#include <Eigen/Geometry>
4#include <bench/BenchTimer.h>
5
6using namespace std;
7using namespace Eigen;
8
9#ifndef SCALAR
10#define SCALAR float
11#endif
12
13#ifndef SIZE
14#define SIZE 8
15#endif
16
17typedef SCALAR Scalar;
18typedef NumTraits<Scalar>::Real RealScalar;
19typedef Matrix<RealScalar,Dynamic,Dynamic> A;
20typedef Matrix</*Real*/Scalar,Dynamic,Dynamic> B;
21typedef Matrix<Scalar,Dynamic,Dynamic> C;
22typedef Matrix<RealScalar,Dynamic,Dynamic> M;
23
24template<typename Transformation, typename Data>
25EIGEN_DONT_INLINE void transform(const Transformation& t, Data& data)
26{
27  EIGEN_ASM_COMMENT("begin");
28  data = t * data;
29  EIGEN_ASM_COMMENT("end");
30}
31
32template<typename Scalar, typename Data>
33EIGEN_DONT_INLINE void transform(const Quaternion<Scalar>& t, Data& data)
34{
35  EIGEN_ASM_COMMENT("begin quat");
36  for(int i=0;i<data.cols();++i)
37    data.col(i) = t * data.col(i);
38  EIGEN_ASM_COMMENT("end quat");
39}
40
41template<typename T> struct ToRotationMatrixWrapper
42{
43  enum {Dim = T::Dim};
44  typedef typename T::Scalar Scalar;
45  ToRotationMatrixWrapper(const T& o) : object(o) {}
46  T object;
47};
48
49template<typename QType, typename Data>
50EIGEN_DONT_INLINE void transform(const ToRotationMatrixWrapper<QType>& t, Data& data)
51{
52  EIGEN_ASM_COMMENT("begin quat via mat");
53  data = t.object.toRotationMatrix() * data;
54  EIGEN_ASM_COMMENT("end quat via mat");
55}
56
57template<typename Scalar, int Dim, typename Data>
58EIGEN_DONT_INLINE void transform(const Transform<Scalar,Dim,Projective>& t, Data& data)
59{
60  data = (t * data.colwise().homogeneous()).template block<Dim,Data::ColsAtCompileTime>(0,0);
61}
62
63template<typename T> struct get_dim { enum { Dim = T::Dim }; };
64template<typename S, int R, int C, int O, int MR, int MC>
65struct get_dim<Matrix<S,R,C,O,MR,MC> > { enum { Dim = R }; };
66
67template<typename Transformation, int N>
68struct bench_impl
69{
70  static EIGEN_DONT_INLINE void run(const Transformation& t)
71  {
72    Matrix<typename Transformation::Scalar,get_dim<Transformation>::Dim,N> data;
73    data.setRandom();
74    bench_impl<Transformation,N-1>::run(t);
75    BenchTimer timer;
76    BENCH(timer,10,100000,transform(t,data));
77    cout.width(9);
78    cout << timer.best() << " ";
79  }
80};
81
82
83template<typename Transformation>
84struct bench_impl<Transformation,0>
85{
86  static EIGEN_DONT_INLINE void run(const Transformation&) {}
87};
88
89template<typename Transformation>
90EIGEN_DONT_INLINE void bench(const std::string& msg, const Transformation& t)
91{
92  cout << msg << " ";
93  bench_impl<Transformation,SIZE>::run(t);
94  std::cout << "\n";
95}
96
97int main(int argc, char ** argv)
98{
99  Matrix<Scalar,3,4> mat34; mat34.setRandom();
100  Transform<Scalar,3,Isometry> iso3(mat34);
101  Transform<Scalar,3,Affine> aff3(mat34);
102  Transform<Scalar,3,AffineCompact> caff3(mat34);
103  Transform<Scalar,3,Projective> proj3(mat34);
104  Quaternion<Scalar> quat;quat.setIdentity();
105  ToRotationMatrixWrapper<Quaternion<Scalar> > quatmat(quat);
106  Matrix<Scalar,3,3> mat33; mat33.setRandom();
107
108  cout.precision(4);
109  std::cout
110     << "N          ";
111  for(int i=0;i<SIZE;++i)
112  {
113    cout.width(9);
114    cout << i+1 << " ";
115  }
116  cout << "\n";
117
118  bench("matrix 3x3", mat33);
119  bench("quaternion", quat);
120  bench("quat-mat  ", quatmat);
121  bench("isometry3 ", iso3);
122  bench("affine3   ", aff3);
123  bench("c affine3 ", caff3);
124  bench("proj3     ", proj3);
125}
126
127