ground/gcs/src/libs/eigen/bench/geometry.cpp

   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>
  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>
  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;
  45   ToRotationMatrixWrapper(const T& o) : object(o) {}
  46   T object;
  47 };
  48
  49 template<typename QType, typename 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>
  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 {
  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
  83 template<typename Transformation>
  84 struct bench_impl<Transformation,0>
  85 {
  86   static EIGEN_DONT_INLINE void run(const Transformation&) {}
  87 };
  88
  89 template<typename Transformation>
  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
  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