legacy_pose3d.hpp
Go to the documentation of this file.
1 
10 /*****************************************************************************
11 ** Ifdefs
12 *****************************************************************************/
13 
14 #ifndef ECL_GEOMETRY_LEGACY_POSE3D_HPP_
15 #define ECL_GEOMETRY_LEGACY_POSE3D_HPP_
16 
17 /*****************************************************************************
18 ** Includes
19 *****************************************************************************/
20 
21 #include <ecl/linear_algebra.hpp>
22 
23 #include <ecl/config/macros.hpp>
24 
25 #include "legacy_pose2d.hpp"
26 
27 /*****************************************************************************
28 ** Namespaces
29 *****************************************************************************/
30 
31 namespace ecl {
32 
33 /*****************************************************************************
34 ** Interface [Pose3D]
35 *****************************************************************************/
36 
42 template <class Float, typename Enable = void>
44 private:
45  LegacyPose3D() {};
46 };
47 
59 template<typename Float>
60 class ECL_PUBLIC LegacyPose3D<Float, typename enable_if<is_float<Float> >::type> {
61 public:
62  /******************************************
63  ** Eigen Alignment
64  *******************************************/
65  EIGEN_MAKE_ALIGNED_OPERATOR_NEW // http://eigen.tuxfamily.org/dox/StructHavingEigenMembers.html
66 
67  /******************************************
68  ** Typedef
69  *******************************************/
70  typedef Float Scalar;
71  typedef ecl::linear_algebra::Matrix<Float,3,3> RotationMatrix;
72  typedef ecl::linear_algebra::Matrix<Float,3,1> Translation;
74  /******************************************
75  ** Constructors
76  *******************************************/
80  LegacyPose3D() : rot(RotationMatrix::Identity()), trans(Translation::Zero()) {}
81 
93  template<typename Rot, typename Trans>
94 // LegacyPose3D(const ecl::linear_algebra::MatrixBase<Rot>& rotation, const ecl::linear_algebra::MatrixBase<Trans>& translation) :
95  LegacyPose3D(const ecl::linear_algebra::EigenBase<Rot>& rotation, const ecl::linear_algebra::EigenBase<Trans>& translation) :
96  rot(rotation),
97  trans(translation)
98  {}
99 
106  template <enum Pose2DStorageType Storage_>
108  rot(RotationMatrix::Identity()),
109  trans(Translation::Zero())
110  {
111  rot.template block<2,2>(0,0) = pose.rotationMatrix();
112  trans.template segment<2>(0) = pose.translation();
113  }
114 
122  template<typename Trans>
123  LegacyPose3D(const ecl::linear_algebra::AngleAxis<Float>& angle_axis, const ecl::linear_algebra::MatrixBase<Trans>& translation) :
124  rot(RotationMatrix::Identity()),
125  trans(Translation::Zero())
126  {
127  /* TODO */
128  }
136  template<typename Trans>
137  LegacyPose3D(const ecl::linear_algebra::Quaternion<Float>& quaternion, const ecl::linear_algebra::MatrixBase<Trans>& translation) :
138  rot(quaternion.toRotationMatrix()),
139  trans(translation)
140  {}
141 
147  rot(pose.rotation()),
148  trans(pose.translation())
149  {}
150 
151  virtual ~LegacyPose3D() {}
152 
153  /******************************************
154  ** Assignment
155  *******************************************/
161  template <enum Pose2DStorageType Storage_>
163  rot.template block<2,2>(0,0) = pose.rotationMatrix();
164  (rot.template block<2,1>(0,2)) << 0.0, 0.0;
165  (rot.template block<1,3>(2,0)) << 0.0, 0.0, 1.0;
166  trans.template segment<2>(0) = pose.translation();
167  trans[2] = 0.0;
168  return *this;
169  }
170 
177  rot = pose.rotation();
178  trans = pose.translation();
179  return *this;
180  }
181  /******************************************
182  ** Eigen Style Setters
183  *******************************************/
191  void rotation(const RotationMatrix &rotation) {
192  rot = rotation;
193  }
203  template <typename Trans>
204  void translation(const ecl::linear_algebra::MatrixBase<Trans>& translation) {
205  this->trans = translation;
206  }
207  /******************************************
208  ** Convenience Setters
209  *******************************************/
210  // set from EigenBase (aka affine transforms)
211  // set from Quaternions
212  // set from AngleAxis
213 
214  /******************************************
215  ** Eigen Style Accessors
216  *******************************************/
217  RotationMatrix& rotation() { return rot; }
218  Translation& translation() { return trans; }
219  const RotationMatrix& rotation() const { return rot; }
220  const Translation& translation() const { return trans; }
222  /******************************************
223  ** Convenience Accessors
224  *******************************************/
225  // get a Quaternion for the rotation part
226  // get an AngleAxis for the rotation part
227  // get an Affine Transform (4x4) for the big lebowski
228  RotationMatrix rotationMatrix() const { return rot; }
230  /******************************************
231  ** Operators
232  *******************************************/
241  LegacyPose3D<Float> inverse;
242  inverse.rotation(rot.transpose());
243  inverse.translation(-1*(inverse.rot*trans));
244  return inverse;
245  }
251  template <typename Float_>
253  return LegacyPose3D<Float>(rotation()*pose.rotation(),translation() + rotation()*pose.translation());
254  }
255 
256  // Do we need operator* for Pose2D rh values?
257  // Probably better if we code like pose_3d*Pose3D(pose_2d) to avoid vague ambiguities.
258 
264  template <typename Float_>
266  // This probably needs checking for aliasing...could also be (marginally) sped up.
267  *this = (*this)*pose;
268  return (*this);
269  }
278  template <typename Float_>
280  return pose.inverse()*(*this);
281  }
282 
283  // how to do pose2d*pose3d without circular header? -> EigenBase? -> External operators?
284 
285  /*********************
286  ** Streaming
287  **********************/
288  template <typename OutputStream, typename Float_>
289  friend OutputStream& operator<<(OutputStream &ostream , const LegacyPose3D<Float_> &pose);
290 
291 private:
292  RotationMatrix rot;
293  Translation trans;
294 };
295 
296 /*****************************************************************************
297 ** Insertion Operators
298 *****************************************************************************/
308 template <typename OutputStream, typename Float_>
309 OutputStream& operator<<(OutputStream &ostream , const LegacyPose3D<Float_> &pose) {
310  ecl::Format<Float_> format;
311  format.width(6);
312  format.precision(3);
313  for ( unsigned int i = 0; i < 3; ++i ) {
314  ostream << "[ ";
315  for ( unsigned int j = 0; j < 3; ++j ) {
316  ostream << format(pose.rot(i,j)) << " ";
317  }
318  ostream << "] [ ";
319  ostream << format(pose.trans(i)) << " ]\n";
320  }
321  ostream.flush();
322  return ostream;
323 }
324 
325 } // namespace ecl
326 
327 // This is more convenient and less bughuntish than always assigning allocators with your vectors
328 // but currently fails on oneiric with gcc 4.6 (bugfixed in eigen, but not yet out in oneiric).
329 //EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(ecl::LegacyPose3D<float>)
330 //EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(ecl::LegacyPose3D<double>)
331 
332 #endif /* ECL_GEOMETRY_LEGACY_POSE3D_HPP_ */
LegacyPose3D< Float > operator*(const LegacyPose3D< Float_ > &pose) const
Combine this pose with the incoming pose.
LegacyPose3D(const LegacyPose2D< Float, Storage_ > &pose)
Initialise from a 2D pose.
Embedded control libraries.
void translation(const ecl::linear_algebra::MatrixBase< Trans > &translation)
Set the translation vector.
const Translation & translation() const
Return a const handle to the rotational storage component.
LegacyPose3D(const ecl::linear_algebra::Quaternion< Float > &quaternion, const ecl::linear_algebra::MatrixBase< Trans > &translation)
Initialise from an eigen Quaternion and a translation.
const RotationMatrix & rotation() const
Return a mutable handle to the translation vector.
ecl::linear_algebra::Matrix< Float, 3, 3 > RotationMatrix
The type used to represent rotation matrices.
LegacyPose3D()
Initialise with zero rotation and zero translation.
Translation & translation()
Return a mutable handle to the rotational storage component.
Parent template definition for Pose2D.
LegacyPose3D< Float > relative(const LegacyPose3D< Float_ > &pose) const
Relative pose between this pose and another.
LegacyPose3D(const ecl::linear_algebra::AngleAxis< Float > &angle_axis, const ecl::linear_algebra::MatrixBase< Trans > &translation)
Initialise from an eigen AngleAxis and a translation.
LegacyPose3D< Float > & operator*=(const LegacyPose3D< Float_ > &pose)
Transform this pose by the specified input pose.
LegacyPose3D< Float > inverse() const
Calculate the inverse pose.
LegacyPose3D< Float > & operator=(const LegacyPose3D< Float > &pose)
Assign from another Pose2D instance.
Parent template definition for Pose3D.
void rotation(const RotationMatrix &rotation)
Set the rotational component.
RotationMatrix rotationMatrix() const
Return a const handle to the translation vector.
ecl::linear_algebra::Matrix< Float, 3, 1 > Translation
The type used to represent translations.
LegacyPose3D(const LegacyPose3D< Float > &pose)
Copy constructor for 3d poses.
LegacyPose3D< Float > & operator=(const LegacyPose2D< Float, Storage_ > &pose)
Assign from another Pose2D instance.
LegacyPose3D(const ecl::linear_algebra::EigenBase< Rot > &rotation, const ecl::linear_algebra::EigenBase< Trans > &translation)
Initialise with rotation matrix and translation.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef Float Scalar
Eigen style declaration of the element type.
#define ECL_LOCAL
#define ECL_PUBLIC


ecl_geometry
Author(s): Daniel Stonier
autogenerated on Mon Feb 28 2022 22:18:49