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


xbot_driver
Author(s): Roc, wangpeng@droid.ac.cn
autogenerated on Sat Oct 10 2020 03:27:37