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