1
2 #include <iostream>
3 #include <Eigen/Geometry>
4 #include <bench/BenchTimer.h>
5
6 using namespace std;
7 using namespace Eigen;
8
9 #ifndef SCALAR
10 #define SCALAR float
11 #endif
12
13 #ifndef SIZE
14 #define SIZE 8
15 #endif
16
17 typedef SCALAR Scalar;
18 typedef NumTraits<Scalar>::Real RealScalar;
19 typedef Matrix<RealScalar,Dynamic,Dynamic> A;
20 typedef Matrix</*Real*/Scalar,Dynamic,Dynamic> B;
21 typedef Matrix<Scalar,Dynamic,Dynamic> C;
22 typedef Matrix<RealScalar,Dynamic,Dynamic> M;
23
24 template<typename Transformation, typename Data>
transform(const Transformation & t,Data & data)25 EIGEN_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
32 template<typename Scalar, typename Data>
transform(const Quaternion<Scalar> & t,Data & data)33 EIGEN_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
41 template<typename T> struct ToRotationMatrixWrapper
42 {
43 enum {Dim = T::Dim};
44 typedef typename T::Scalar Scalar;
ToRotationMatrixWrapperToRotationMatrixWrapper45 ToRotationMatrixWrapper(const T& o) : object(o) {}
46 T object;
47 };
48
49 template<typename QType, typename Data>
transform(const ToRotationMatrixWrapper<QType> & t,Data & data)50 EIGEN_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
57 template<typename Scalar, int Dim, typename Data>
transform(const Transform<Scalar,Dim,Projective> & t,Data & data)58 EIGEN_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
63 template<typename T> struct get_dim { enum { Dim = T::Dim }; };
64 template<typename S, int R, int C, int O, int MR, int MC>
65 struct get_dim<Matrix<S,R,C,O,MR,MC> > { enum { Dim = R }; };
66
67 template<typename Transformation, int N>
68 struct bench_impl
69 {
runbench_impl70 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
83 template<typename Transformation>
84 struct bench_impl<Transformation,0>
85 {
runbench_impl86 static EIGEN_DONT_INLINE void run(const Transformation&) {}
87 };
88
89 template<typename Transformation>
bench(const std::string & msg,const Transformation & t)90 EIGEN_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
main(int argc,char ** argv)97 int 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