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 
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;
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>
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>
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:
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 
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  *******************************************/
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_>
415  return LegacyPose2D<Float,Storage>(RotationMath::Product(rot,pose.rot),trans + rotationMatrix()*pose.trans);
416  }
423  // This probably needs checking...could also be (marginally) sped up.
424  *this = (*this)*pose;
425  return (*this);
426  }
441  template <enum Pose2DStorageType Storage_>
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_ */
RotationMatrixStorage.
Angle< Float > RotationType
Rotation storage type (angle).
geometry::Pose2DMath< Float, RotationMatrixStorage > RotationMatrixMath
Rotational math specific to rotational matrices.
Embedded control libraries.
geometry::Pose2DMath< Float, RotationAngleStorage > RotationAngleMath
Rotational math specific to rotational angles.
ecl_traits< LegacyPose2D< Float, Storage, typename enable_if< is_float< Float > >::type > >::RotationType RotationType
The type used for storage of the rotation/angle.
LegacyPose2D< Float, Storage > relative(const LegacyPose2D< Float, Storage_ > &pose) const
Relative pose between this pose and another.
RotationAngleStorage.
geometry::Pose2DMath< Float, Storage > RotationMath
Rotational math specific to this storage type.
LegacyPose2D< Float, Storage > & operator=(const LegacyPose2D< Float, Storage_ > &pose)
Assign from another Pose2D instance.
LegacyPose2D(const LegacyPose2D< Float, Storage_ > &pose)
Copy constructor that works for copies from any pose type.
Translation & translation()
Return a mutable handle to the rotational storage component.
ecl::linear_algebra::Matrix< Float, 2, 2 > RotationMatrixType
void rotationMatrix(const ecl::linear_algebra::MatrixBase< Rot > &rotation_matrix)
Set the rotational component.
LegacyPose2D< Float, Storage > operator*(const LegacyPose2D< Float, Storage_ > &pose) const
Combine this pose with the incoming pose.
Math functions/selectors for the pose2D class with rotation angle storage.
Pose2DStorageType
Used by the traits to select the storage type for Pose2D classes.
static LegacyPose2D< Float, Storage > Identity()
Static function for returning the idenitity pose, eigen style.
static Angle< Float > Heading(const RotationMatrixType &rotation)
Arbitrary heading angle converter.
void translation(const ecl::linear_algebra::MatrixBase< Trans > &T)
Set the translation vector.
LegacyPose2D(const ecl::linear_algebra::MatrixBase< Rot > &R, const ecl::linear_algebra::MatrixBase< Trans > &T)
Construct the pose using a rotation matrix and a translation vector.
Math functions/selectors for the pose2D class with rotation matrix storage.
Float x() const
Return a const handle to the translation vector.
LegacyPose2D< Float > & operator*=(const LegacyPose2D< Float > &pose)
Transform this pose by the specified input pose.
Parent template definition for Pose2D.
static RotationMatrixType Rotation(const RotationMatrixType &rotation)
Arbitrary rotation converter.
void rotation(const RotationMatrix &rotation_matrix)
Set the rotational component with a rotation matrix.
void heading(const Angle< Float > &value)
Sets the heading.
static Angle< Float > Product(const Angle< Float > &angle, RotationMatrixType &rotation)
Overloaded product calculater for poses.
LegacyPose2D(const Float &x, const Float &y, const Angle< Float > &angle)
Construct the pose using scalars for position and rotation angle.
static Angle< Float > Rotation(const RotationMatrixType &rotation)
Arbitrary rotation converter.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef Float Scalar
Eigen style declaration of the element type.
static RotationMatrixType Rotation(const Angle< Float > &angle)
Arbitrary rotation converter.
static RotationMatrixType Product(const RotationMatrixType &rot1, const RotationMatrixType &rot2)
Overloaded product calculater for poses.
ecl::linear_algebra::Matrix< Float, 2, 2 > RotationMatrix
The type used to represent rotation matrices.
static Angle< Float > Heading(const Angle< Float > &angle)
Arbitrary heading angle converter.
void translation(const Float &x, const Float &y)
Set the translation vector from a pair of x,y values.
const RotationType & rotation() const
Return a mutable handle to the translation vector.
static RotationMatrixType Product(const RotationMatrixType &rotation, const Angle< Float > &angle)
Overloaded product calculater for poses.
ecl::linear_algebra::Matrix< Float, 2, 1 > Translation
The type used to represent translations.
LegacyPose2D< Float, Storage > inverse() const
Calculate the inverse pose.
ecl::linear_algebra::Matrix< Float, 2, 2 > RotationMatrixType
static RotationMatrixType Inverse(const RotationMatrixType &rotation)
Pose inverse, rotation matrix format.
static Angle< Float > Product(const Angle< Float > &angle1, const Angle< Float > &angle2)
Overloaded product calculater for poses.
static RotationMatrixType Identity()
Rotation matrix type for pose2D.
void rotation(const Angle< Float > &heading)
Set the rotational component with a heading angle.
Parent template definition for angles.
Definition: angle.hpp:113
Parent template for the pose2D math classes.
C++ interface for angles (degrees/radians).
void setIdentity()
Set this pose to zero rotation and zero translation.
LegacyPose2D(const Angle< Float > &angle, const ecl::linear_algebra::MatrixBase< Trans > &T)
Construct the pose using a rotational angle and a translation vector.
ecl::linear_algebra::Matrix< Float, 2, 2 > RotationType
Rotation storage type (matrix).
const Translation & translation() const
Return a const handle to the rotational storage component.
static Angle< Float > Inverse(const Angle< Float > &angle)
Pose inverse, angle format.
void setPose(const Float &x, const Float &y, const Angle< Float > &heading)
Sets the pose with a triplet.
static Angle< Float > Rotation(const Angle< Float > &radians)
Arbitrary rotation converter.
#define ECL_PUBLIC
static Angle< Float > Identity()
Rotation matrix type for pose2D.


ecl_geometry
Author(s): Daniel Stonier
autogenerated on Mon Jun 10 2019 13:08:37