legacy_pose2d.hpp
Go to the documentation of this file.
1 
10 /*****************************************************************************
11 ** Ifdefs
12 *****************************************************************************/
13 
14 #ifndef ECL_GEOMETRY_LEGACY_POSE2D_HPP_
15 #define ECL_GEOMETRY_LEGACY_POSE2D_HPP_
16 
17 /*****************************************************************************
18 ** Includes
19 *****************************************************************************/
20 
21 #include <ecl/linear_algebra.hpp>
22 
23 #include <ecl/config/macros.hpp>
24 #include <ecl/formatters.hpp>
25 #include <ecl/math/constants.hpp>
27 #include <ecl/mpl/enable_if.hpp>
30 #include "angle.hpp"
31 
32 /*****************************************************************************
33 ** Enums
34 *****************************************************************************/
35 
36 namespace ecl {
37 
41 enum Pose2DStorageType {
44 };
45 
46 } // namespace ecl
47 
48 /*****************************************************************************
49 ** Forward declarations
50 *****************************************************************************/
51 
52 namespace ecl {
53 
54 template <typename Float, enum Pose2DStorageType Storage, typename Enable> class LegacyPose2D;
55 
56 } // namespace ecl
57 
58 /*****************************************************************************
59 ** Traits
60 *****************************************************************************/
61 
62 namespace ecl {
63 
67 template <typename Float, enum Pose2DStorageType Storage, typename Enable>
68 class ecl_traits< LegacyPose2D<Float, Storage, Enable> > {};
69 
73 template <typename Float, typename Enable>
74 class ecl_traits< LegacyPose2D<Float, RotationMatrixStorage, Enable> > {
75 public:
76  typedef Float Scalar;
77  typedef ecl::linear_algebra::Matrix<Float,2,2> RotationType;
78 };
79 
83 template <typename Float, typename Enable>
84 class ecl_traits< LegacyPose2D<Float, RotationAngleStorage, Enable> > {
85 public:
86  typedef Float Scalar;
87  typedef Angle<Float> RotationType;
88 };
89 
90 } // namespace ecl
91 
92 /*****************************************************************************
93 ** Operations
94 *****************************************************************************/
95 
96 namespace ecl {
97 namespace geometry {
98 
102 template <typename Float, enum Pose2DStorageType Storage> class Pose2DMath {};
110 template <typename Float>
111 class ECL_PUBLIC Pose2DMath<Float,RotationMatrixStorage> {
112 public:
113  typedef ecl::linear_algebra::Matrix<Float,2,2> RotationMatrixType;
115  static RotationMatrixType Identity() { return RotationMatrixType::Identity(); }
116  static RotationMatrixType Rotation(const RotationMatrixType &rotation) { return rotation; }
117  static RotationMatrixType Rotation(const Angle<Float>& angle) { return angle.rotationMatrix(); }
118  static Angle<Float> Heading(const RotationMatrixType &rotation) { return Angle<Float>(rotation); }
119  static RotationMatrixType Product(const RotationMatrixType &rot1, const RotationMatrixType &rot2) { return rot1*rot2; }
120  static RotationMatrixType Product(const RotationMatrixType &rotation, const Angle<Float> &angle) { return rotation*angle.rotationMatrix(); }
121  static RotationMatrixType Inverse(const RotationMatrixType &rotation) { return rotation.transpose(); }
122 };
123 
131 template <typename Float>
132 class ECL_PUBLIC Pose2DMath<Float,RotationAngleStorage> {
133 public:
134  typedef ecl::linear_algebra::Matrix<Float,2,2> RotationMatrixType;
135  static Angle<Float> Identity() { return Angle<Float>(0.0); }
136  static Angle<Float> Rotation(const Angle<Float>& radians) { return Angle<Float>(radians); }
137  static Angle<Float> Rotation(const RotationMatrixType &rotation) { return Pose2DMath<Float,RotationMatrixStorage>::Heading(rotation); }
138  static Angle<Float> Heading(const Angle<Float> &angle) { return angle; }
139  static Angle<Float> Product(const Angle<Float> &angle1, const Angle<Float> &angle2) { return angle1+angle2; }
140  static Angle<Float> Product(const Angle<Float> &angle, RotationMatrixType &rotation) { return angle + Angle<Float>(rotation); }
141  static Angle<Float> Inverse(const Angle<Float> &angle) { return Angle<Float>(-1*angle); }
142 };
143 
144 } // namespace geometry
145 } // namespace ecl
146 
147 
148 /*****************************************************************************
149 ** Pose2D
150 *****************************************************************************/
151 
152 namespace ecl {
153 
159 template <class Float, enum Pose2DStorageType Storage = RotationMatrixStorage, typename Enable = void>
160 class ECL_PUBLIC LegacyPose2D {
161  typedef Float Scalar;
162 private:
163  LegacyPose2D() {};
164 };
165 
182 template<typename Float, enum Pose2DStorageType Storage>
183 class ECL_PUBLIC LegacyPose2D<Float, Storage, typename enable_if<is_float<Float> >::type> {
184 public:
185  /******************************************
186  ** Eigen Alignment
187  *******************************************/
188  EIGEN_MAKE_ALIGNED_OPERATOR_NEW // http://eigen.tuxfamily.org/dox/StructHavingEigenMembers.html
189 
190  /******************************************
191  ** Typedef
192  *******************************************/
193  typedef Float Scalar;
198  typedef ecl::linear_algebra::Matrix<Float,2,2> RotationMatrix;
199  typedef ecl::linear_algebra::Matrix<Float,2,1> Translation;
201  /******************************************
202  ** Constructors
203  *******************************************/
212  LegacyPose2D() : rot( RotationMath::Identity()), trans(Translation::Zero()) {}
213 
220  LegacyPose2D(const Float &x, const Float &y, const Angle<Float> &angle) :
221  rot( RotationMath::Rotation(angle)),
222  trans( (Translation() << x,y).finished() )
223  {}
224 
236  template<typename Rot, typename Trans>
237  LegacyPose2D(const ecl::linear_algebra::MatrixBase<Rot>& R, const ecl::linear_algebra::MatrixBase<Trans>& T) :
238  rot( RotationMath::Rotation(R) ),
239  trans(T)
240  {}
251  template<typename Trans>
252  LegacyPose2D(const Angle<Float>& angle, const ecl::linear_algebra::MatrixBase<Trans>& T) :
253  rot( RotationMath::Rotation(angle) ),
254  trans(T)
255  {}
263  template <enum Pose2DStorageType Storage_>
265  rot(RotationMath::Rotation(pose.rotation())),
266  trans(pose.translation())
267  {}
268 
269  virtual ~LegacyPose2D() {}
270 
271  /******************************************
272  ** Assignment
273  *******************************************/
279  template <enum Pose2DStorageType Storage_>
281  trans = pose.translation();
282  rot = RotationMath::Rotation(pose.rotation());
283  return *this;
284  }
285 
286  /******************************************
287  ** Eigen Style Setters
288  *******************************************/
297  void rotation(const Angle<Float> &heading) {
298  rot = RotationMath::Rotation(heading);
299  }
308  void rotation(const RotationMatrix &rotation_matrix) {
309  rot = RotationMath::Rotation(rotation_matrix);
310  }
319  void translation(const Float &x, const Float &y) {
320  this->trans << x, y;
321  }
331  template <typename Trans>
332  void translation(const ecl::linear_algebra::MatrixBase<Trans>& T) {
333  this->trans = T;
334  }
335 
339  void setIdentity() {
340  rot = RotationMath::Identity();
341  trans << 0.0, 0.0;
342  }
343 
349  static LegacyPose2D<Float,Storage> Identity() {
351  }
352 
353  /******************************************
354  ** Convenience Setters
355  *******************************************/
356  void setPose(const Float& x, const Float& y, const Angle<Float>& heading) { trans << x,y; rot = RotationMath::Rotation(heading); }
357  void x(const Float& value) { trans[0] = value; }
358  void y(const Float& value) { trans[1] = value; }
359  void heading(const Angle<Float>& value) { rot = RotationMath::Rotation(value); }
360 
370  template <typename Rot>
371  void rotationMatrix(const ecl::linear_algebra::MatrixBase<Rot>& rotation_matrix) {
372  rot = RotationMath::Rotation(rotation_matrix);
373  }
374 
375  /******************************************
376  ** Eigen Style Accessors
377  *******************************************/
378  RotationType& rotation() { return rot; }
379  Translation& translation() { return trans; }
380  const RotationType& rotation() const { return rot; }
381  const Translation& translation() const { return trans; }
383  /******************************************
384  ** Convenience Accessors
385  *******************************************/
386  Float x() const { return trans[0]; }
387  Float y() const { return trans[1]; }
388  Angle<Float> heading() const { return RotationMath::Heading(rot); }
389  RotationMatrix rotationMatrix() const { return RotationMatrixMath::Rotation(rot); }
391  /******************************************
392  ** Operators
393  *******************************************/
401  LegacyPose2D<Float,Storage> inverse() const {
402  LegacyPose2D<Float,Storage> inverse;
403  inverse.rotation(RotationMath::Inverse(rot));
404  inverse.translation(-1*(RotationMatrixMath::Rotation(inverse.rot)*trans));
405  return inverse;
406  }
407 
413  template <enum Pose2DStorageType Storage_>
414  LegacyPose2D<Float,Storage> operator*(const LegacyPose2D<Float,Storage_> &pose) const {
415  return LegacyPose2D<Float,Storage>(RotationMath::Product(rot,pose.rot),trans + rotationMatrix()*pose.trans);
416  }
422  LegacyPose2D<Float>& operator*=(const LegacyPose2D<Float> &pose) {
423  // This probably needs checking...could also be (marginally) sped up.
424  *this = (*this)*pose;
425  return (*this);
426  }
441  template <enum Pose2DStorageType Storage_>
442  LegacyPose2D<Float,Storage> relative(const LegacyPose2D<Float,Storage_> &pose) const {
443  return pose.inverse()*(*this);
444  }
445 
446  /*********************
447  ** Streaming
448  **********************/
449  template <typename OutputStream, typename Float_, enum Pose2DStorageType Storage_>
450  friend OutputStream& operator<<(OutputStream &ostream , const LegacyPose2D<Float_, Storage_> &pose);
451 
452 private:
453  RotationType rot;
454  Translation trans;
455 };
456 
457 } // namespace ecl
458 
459 /*****************************************************************************
460 ** Insertion Operators
461 *****************************************************************************/
462 
463 namespace ecl {
464 
474 template <typename OutputStream, typename Float_, enum Pose2DStorageType Storage_>
475 OutputStream& operator<<(OutputStream &ostream , const LegacyPose2D<Float_,Storage_> &pose) {
476  ecl::Format<Float_> format;
477  format.width(6);
478  format.precision(3);
479  ostream << "[ ";
480  ostream << format(pose.x()) << " ";
481  ostream << format(pose.y()) << " ";
482  ostream << format(pose.heading());
483  ostream << " ]";
484  ostream.flush();
485  return ostream;
486 }
487 
488 
489 } // namespace ecl
490 
491 // This is more convenient and less bughuntish than always assigning allocators with your vectors
492 // but currently fails on oneiric with gcc 4.6 (bugfixed in eigen, but not yet out in oneiric).
493 // EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(ecl::LegacyPose2D<float,ecl::RotationAngleStorage>)
494 // EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(ecl::LegacyPose2D<float,ecl::RotationMatrixStorage>)
495 // EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(ecl::LegacyPose2D<double,ecl::RotationAngleStorage>)
496 // EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(ecl::LegacyPose2D<double,ecl::RotationMatrixStorage>)
497 
498 #endif /* ECL_GEOMETRY_LEGACY_POSE2D_HPP_ */
angle.hpp
C++ interface for angles (degrees/radians).
ecl::geometry::Pose2DMath
Parent template for the pose2D math classes.
Definition: legacy_pose2d.hpp:106
ecl::geometry::Pose2DMath< Float, RotationAngleStorage >
Math functions/selectors for the pose2D class with rotation angle storage.
Definition: legacy_pose2d.hpp:136
ecl::LegacyPose2D< Float, Storage, enable_if< is_float< Float > >::type >::RotationMatrix
ecl::linear_algebra::Matrix< Float, 2, 2 > RotationMatrix
The type used to represent rotation matrices.
Definition: legacy_pose2d.hpp:200
ecl::operator<<
OutputStream & operator<<(OutputStream &ostream, const Array< ElementType, ArraySize > &array)
formatters.hpp
ecl::geometry::Pose2DMath< Float, RotationMatrixStorage >::RotationMatrixType
ecl::linear_algebra::Matrix< Float, 2, 2 > RotationMatrixType
Definition: legacy_pose2d.hpp:117
linear_algebra.hpp
ecl::LegacyPose2D
Parent template definition for Pose2D.
Definition: legacy_pose2d.hpp:56
ecl::LegacyPose2D< Float, Storage, enable_if< is_float< Float > >::type >::Translation
ecl::linear_algebra::Matrix< Float, 2, 1 > Translation
The type used to represent translations.
Definition: legacy_pose2d.hpp:201
ecl::Pose2DStorageType
Pose2DStorageType
Used by the traits to select the storage type for Pose2D classes.
Definition: legacy_pose2d.hpp:47
ecl::LegacyPose2D< Float, Storage, enable_if< is_float< Float > >::type >::translation
Translation & translation()
Return a mutable handle to the rotational storage component.
Definition: legacy_pose2d.hpp:381
ecl::RotationMatrixStorage
@ RotationMatrixStorage
RotationMatrixStorage.
Definition: legacy_pose2d.hpp:55
ecl::RotationAngleStorage
@ RotationAngleStorage
RotationAngleStorage.
Definition: legacy_pose2d.hpp:54
enable_if.hpp
ecl::LegacyPose2D< Float, Storage, enable_if< is_float< Float > >::type >::rotation
RotationType & rotation()
Definition: legacy_pose2d.hpp:380
ecl::ecl_traits
fundamental_types.hpp
ecl::Format
ecl::ecl_traits< LegacyPose2D< Float, RotationAngleStorage, Enable > >::Scalar
Float Scalar
Element type.
Definition: legacy_pose2d.hpp:88
standard_exception.hpp
traits.hpp
ecl::is_float
ecl::ecl_traits< LegacyPose2D< Float, RotationMatrixStorage, Enable > >::RotationType
ecl::linear_algebra::Matrix< Float, 2, 2 > RotationType
Rotation storage type (matrix).
Definition: legacy_pose2d.hpp:79
ecl::Angle
Parent template definition for angles.
Definition: angle.hpp:119
ecl::LegacyPose2D< Float, Storage, enable_if< is_float< Float > >::type >::RotationType
ecl_traits< LegacyPose2D< Float, Storage, typename enable_if< is_float< Float > >::type > >::RotationType RotationType
The type used for storage of the rotation/angle.
Definition: legacy_pose2d.hpp:199
constants.hpp
ecl::enable_if
macros.hpp
ecl::ecl_traits< LegacyPose2D< Float, RotationMatrixStorage, Enable > >::Scalar
Float Scalar
Element type.
Definition: legacy_pose2d.hpp:78
ecl
Embedded control libraries.
ECL_PUBLIC
#define ECL_PUBLIC
ecl::geometry::Pose2DMath< Float, RotationMatrixStorage >
Math functions/selectors for the pose2D class with rotation matrix storage.
Definition: legacy_pose2d.hpp:115


ecl_geometry
Author(s): Daniel Stonier
autogenerated on Wed Mar 2 2022 00:16:39