[136] | 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 |
|
---|