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