geometry.cpp
Go to the documentation of this file.
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;
20 typedef Matrix</*Real*/Scalar,Dynamic,Dynamic> B;
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>
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) {}
47 };
48 
49 template<typename QType, typename 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>
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  {
73  data.setRandom();
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 << " ";
94  std::cout << "\n";
95 }
96 
97 int main(int argc, char ** argv)
98 {
99  Matrix<Scalar,3,4> mat34; mat34.setRandom();
101  Transform<Scalar,3,Affine> aff3(mat34);
103  Transform<Scalar,3,Projective> proj3(mat34);
104  Quaternion<Scalar> quat;quat.setIdentity();
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 
Matrix< SCALARB, Dynamic, Dynamic, opt_B > B
Definition: bench_gemm.cpp:49
static noiseModel::Isotropic::shared_ptr iso3
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: geometry.cpp:22
Rot2 R(Rot2::fromAngle(0.1))
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
#define EIGEN_ASM_COMMENT(X)
Definition: Macros.h:1082
Definition: BFloat16.h:88
Holds information about the various numeric (i.e. scalar) types allowed by Eigen. ...
Definition: NumTraits.h:232
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
#define N
Definition: gksort.c:12
DiscreteKey S(1, 2)
#define EIGEN_DONT_INLINE
Definition: Macros.h:940
int main(int argc, char **argv)
Definition: geometry.cpp:97
Key O(std::uint64_t j)
double best(int TIMER=CPU_TIMER) const
Definition: BenchTimer.h:111
int data[]
#define SCALAR
Definition: geometry.cpp:10
EIGEN_DONT_INLINE void transform(const Transformation &t, Data &data)
Definition: geometry.cpp:25
static EIGEN_DONT_INLINE void run(const Transformation &)
Definition: geometry.cpp:86
EIGEN_DONT_INLINE void bench(const std::string &msg, const Transformation &t)
Definition: geometry.cpp:90
ToRotationMatrixWrapper(const T &o)
Definition: geometry.cpp:45
static EIGEN_DONT_INLINE void run(const Transformation &t)
Definition: geometry.cpp:70
The quaternion class used to represent 3D orientations and rotations.
NumTraits< Scalar >::Real RealScalar
Definition: geometry.cpp:18
const int Dynamic
Definition: Constants.h:22
#define BENCH(TIMER, TRIES, REP, CODE)
Definition: BenchTimer.h:174
SCALAR Scalar
Definition: geometry.cpp:17
The matrix class, also used for vectors and row-vectors.
#define SIZE
Definition: geometry.cpp:14
Matrix< _Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols > & setRandom(Index size)
Definition: Random.h:151
static BenchTimer timer
Represents an homogeneous transformation in a N dimensional space.
Point2 t(10, 10)
Matrix< Scalar, Dynamic, Dynamic > C
Definition: geometry.cpp:21


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:18