geometry.cpp
Go to the documentation of this file.
00001 
00002 #include <iostream>
00003 #include <Eigen/Geometry>
00004 #include <bench/BenchTimer.h>
00005 
00006 using namespace std;
00007 using namespace Eigen;
00008 
00009 #ifndef SCALAR
00010 #define SCALAR float
00011 #endif
00012 
00013 #ifndef SIZE
00014 #define SIZE 8
00015 #endif
00016 
00017 typedef SCALAR Scalar;
00018 typedef NumTraits<Scalar>::Real RealScalar;
00019 typedef Matrix<RealScalar,Dynamic,Dynamic> A;
00020 typedef Matrix</*Real*/Scalar,Dynamic,Dynamic> B;
00021 typedef Matrix<Scalar,Dynamic,Dynamic> C;
00022 typedef Matrix<RealScalar,Dynamic,Dynamic> M;
00023 
00024 template<typename Transformation, typename Data>
00025 EIGEN_DONT_INLINE void transform(const Transformation& t, Data& data)
00026 {
00027   EIGEN_ASM_COMMENT("begin");
00028   data = t * data;
00029   EIGEN_ASM_COMMENT("end");
00030 }
00031 
00032 template<typename Scalar, typename Data>
00033 EIGEN_DONT_INLINE void transform(const Quaternion<Scalar>& t, Data& data)
00034 {
00035   EIGEN_ASM_COMMENT("begin quat");
00036   for(int i=0;i<data.cols();++i)
00037     data.col(i) = t * data.col(i);
00038   EIGEN_ASM_COMMENT("end quat");
00039 }
00040 
00041 template<typename T> struct ToRotationMatrixWrapper
00042 {
00043   enum {Dim = T::Dim};
00044   typedef typename T::Scalar Scalar;
00045   ToRotationMatrixWrapper(const T& o) : object(o) {}
00046   T object;
00047 };
00048 
00049 template<typename QType, typename Data>
00050 EIGEN_DONT_INLINE void transform(const ToRotationMatrixWrapper<QType>& t, Data& data)
00051 {
00052   EIGEN_ASM_COMMENT("begin quat via mat");
00053   data = t.object.toRotationMatrix() * data;
00054   EIGEN_ASM_COMMENT("end quat via mat");
00055 }
00056 
00057 template<typename Scalar, int Dim, typename Data>
00058 EIGEN_DONT_INLINE void transform(const Transform<Scalar,Dim,Projective>& t, Data& data)
00059 {
00060   data = (t * data.colwise().homogeneous()).template block<Dim,Data::ColsAtCompileTime>(0,0);
00061 }
00062 
00063 template<typename T> struct get_dim { enum { Dim = T::Dim }; };
00064 template<typename S, int R, int C, int O, int MR, int MC>
00065 struct get_dim<Matrix<S,R,C,O,MR,MC> > { enum { Dim = R }; };
00066 
00067 template<typename Transformation, int N>
00068 struct bench_impl
00069 {
00070   static EIGEN_DONT_INLINE void run(const Transformation& t)
00071   {
00072     Matrix<typename Transformation::Scalar,get_dim<Transformation>::Dim,N> data;
00073     data.setRandom();
00074     bench_impl<Transformation,N-1>::run(t);
00075     BenchTimer timer;
00076     BENCH(timer,10,100000,transform(t,data));
00077     cout.width(9);
00078     cout << timer.best() << " ";
00079   }
00080 };
00081 
00082 
00083 template<typename Transformation>
00084 struct bench_impl<Transformation,0>
00085 {
00086   static EIGEN_DONT_INLINE void run(const Transformation&) {}
00087 };
00088 
00089 template<typename Transformation>
00090 EIGEN_DONT_INLINE void bench(const std::string& msg, const Transformation& t)
00091 {
00092   cout << msg << " ";
00093   bench_impl<Transformation,SIZE>::run(t);
00094   std::cout << "\n";
00095 }
00096 
00097 int main(int argc, char ** argv)
00098 {
00099   Matrix<Scalar,3,4> mat34; mat34.setRandom();
00100   Transform<Scalar,3,Isometry> iso3(mat34);
00101   Transform<Scalar,3,Affine> aff3(mat34);
00102   Transform<Scalar,3,AffineCompact> caff3(mat34);
00103   Transform<Scalar,3,Projective> proj3(mat34);
00104   Quaternion<Scalar> quat;quat.setIdentity();
00105   ToRotationMatrixWrapper<Quaternion<Scalar> > quatmat(quat);
00106   Matrix<Scalar,3,3> mat33; mat33.setRandom();
00107   
00108   cout.precision(4);
00109   std::cout
00110      << "N          ";
00111   for(int i=0;i<SIZE;++i)
00112   {
00113     cout.width(9);
00114     cout << i+1 << " ";
00115   }
00116   cout << "\n";
00117   
00118   bench("matrix 3x3", mat33);
00119   bench("quaternion", quat);
00120   bench("quat-mat  ", quatmat);
00121   bench("isometry3 ", iso3);
00122   bench("affine3   ", aff3);
00123   bench("c affine3 ", caff3);
00124   bench("proj3     ", proj3);
00125 }
00126 


libicr
Author(s): Robert Krug
autogenerated on Mon Jan 6 2014 11:32:44