pose3d_eigen2.hpp
Go to the documentation of this file.
00001 
00008 /*****************************************************************************
00009 ** Ifdefs
00010 *****************************************************************************/
00011 
00012 #ifndef ECL_GEOMETRY_POSE3D_EIGEN2_HPP_
00013 #define ECL_GEOMETRY_POSE3D_EIGEN2_HPP_
00014 
00015 /*****************************************************************************
00016 ** Includes
00017 *****************************************************************************/
00018 
00019 #include <ecl/config/macros.hpp>
00020 #include "pose2d.hpp"
00021 
00022 /*****************************************************************************
00023 ** Namespaces
00024 *****************************************************************************/
00025 
00026 namespace ecl {
00027 
00028 /*****************************************************************************
00029 ** Interface [Pose3D]
00030 *****************************************************************************/
00031 
00037 template <class Float, typename Enable = void>
00038 class ECL_LOCAL Pose3D {
00039 private:
00040         Pose3D() {}; 
00041 };
00042 
00054 template<typename Float>
00055 class ECL_PUBLIC Pose3D<Float, typename enable_if<is_float<Float> >::type> {
00056 public:
00057         /******************************************
00058         ** Eigen Alignment
00059         *******************************************/
00060         EIGEN_MAKE_ALIGNED_OPERATOR_NEW // http://eigen.tuxfamily.org/dox/StructHavingEigenMembers.html
00061 
00062         /******************************************
00063         ** Typedef
00064         *******************************************/
00065         typedef Float Scalar; 
00066         typedef ecl::linear_algebra::Matrix<Float,3,3> RotationMatrix; 
00067         typedef ecl::linear_algebra::Matrix<Float,3,1> Translation;  
00069         /******************************************
00070         ** Constructors
00071         *******************************************/
00075         Pose3D() : rot(RotationMatrix::Identity()), trans(Translation::Zero()) {}
00076 
00088         template<typename Rot, typename Trans>
00089         Pose3D(const ecl::linear_algebra::MatrixBase<Rot>& rotation, const ecl::linear_algebra::MatrixBase<Trans>& translation) :
00090 //      Pose3D(const ecl::linear_algebra::EigenBase<Rot>& rotation, const ecl::linear_algebra::EigenBase<Trans>& translation) :
00091                 rot(rotation),
00092                 trans(translation)
00093         {}
00094 
00101         template <enum Pose2DStorageType Storage_>
00102         Pose3D(const Pose2D<Float,Storage_>& pose) :
00103             rot(RotationMatrix::Identity()),
00104             trans(Translation::Zero())
00105         {
00106                 rot.template block<2,2>(0,0) = pose.rotationMatrix();
00107                 trans.template segment<2>(0) = pose.translation();
00108         }
00109 
00117         template<typename Trans>
00118         Pose3D(const ecl::linear_algebra::AngleAxis<Float>& angle_axis, const ecl::linear_algebra::MatrixBase<Trans>& translation) :
00119             rot(RotationMatrix::Identity()),
00120             trans(Translation::Zero())
00121         {
00122                 /* TODO */
00123         }
00131         template<typename Trans>
00132         Pose3D(const ecl::linear_algebra::Quaternion<Float>& quaternion, const ecl::linear_algebra::MatrixBase<Trans>& translation) :
00133             rot(quaternion.toRotationMatrix()),
00134             trans(translation)
00135         {}
00136 
00141         Pose3D(const Pose3D<Float>& pose) :
00142                 rot(pose.rotation()),
00143                 trans(pose.translation())
00144         {}
00145 
00146         virtual ~Pose3D() {}
00147 
00148         /******************************************
00149         ** Assignment
00150         *******************************************/
00156         template <enum Pose2DStorageType Storage_>
00157         Pose3D<Float>& operator=(const Pose2D<Float,Storage_>& pose) {
00158                 rot.template block<2,2>(0,0) = pose.rotationMatrix();
00159                 (rot.template block<2,1>(0,2)) << 0.0, 0.0;
00160                 (rot.template block<1,3>(2,0)) << 0.0, 0.0, 1.0;
00161                 trans.template segment<2>(0) = pose.translation();
00162                 trans[2] = 0.0;
00163                 return *this;
00164         }
00165 
00171         Pose3D<Float>& operator=(const Pose3D<Float>& pose) {
00172                 rot = pose.rotation();
00173                 trans = pose.translation();
00174                 return *this;
00175         }
00176         /******************************************
00177         ** Eigen Style Setters
00178         *******************************************/
00186         void rotation(const RotationMatrix &rotation) {
00187                 rot = rotation;
00188         }
00198         template <typename Trans>
00199         void translation(const ecl::linear_algebra::MatrixBase<Trans>& translation) {
00200                 this->trans = translation;
00201         }
00202         /******************************************
00203         ** Convenience Setters
00204         *******************************************/
00205         // set from EigenBase (aka affine transforms)
00206         // set from Quaternions
00207         // set from AngleAxis
00208 
00209         /******************************************
00210         ** Eigen Style Accessors
00211         *******************************************/
00212         RotationMatrix& rotation() { return rot; } 
00213         Translation& translation() { return trans; } 
00214         const RotationMatrix& rotation() const { return rot; } 
00215         const Translation& translation() const { return trans; }  
00217         /******************************************
00218         ** Convenience Accessors
00219         *******************************************/
00220         // get a Quaternion for the rotation part
00221         // get an AngleAxis for the rotation part
00222         // get an Affine Transform (4x4) for the big lebowski
00223         RotationMatrix rotationMatrix() const { return rot; }  
00225         /******************************************
00226         ** Operators
00227         *******************************************/
00235     Pose3D<Float> inverse() const {
00236         Pose3D<Float> inverse;
00237         inverse.rotation(rot.transpose());
00238         inverse.translation(-1*(inverse.rot*trans));
00239         return inverse;
00240     }
00246     template <typename Float_>
00247     Pose3D<Float> operator*(const Pose3D<Float_> &pose) const {
00248                 return Pose3D<Float>(rotation()*pose.rotation(),translation() + rotation()*pose.translation());
00249     }
00250 
00251     // Do we need operator* for Pose2D rh values?
00252     // Probably better if we code like pose_3d*Pose3D(pose_2d) to avoid vague ambiguities.
00253 
00259         template <typename Float_>
00260     Pose3D<Float>& operator*=(const Pose3D<Float_> &pose) {
00261         // This probably needs checking for aliasing...could also be (marginally) sped up.
00262         *this = (*this)*pose;
00263         return (*this);
00264     }
00273         template <typename Float_>
00274     Pose3D<Float> relative(const Pose3D<Float_> &pose) const {
00275                 return pose.inverse()*(*this);
00276     }
00277 
00278     /*********************
00279         ** Streaming
00280         **********************/
00281         template <typename OutputStream, typename Float_>
00282         friend OutputStream& operator<<(OutputStream &ostream , const Pose3D<Float_> &pose);
00283 
00284 private:
00285         RotationMatrix rot;
00286         Translation trans;
00287 };
00288 
00289 /*****************************************************************************
00290 ** Insertion Operators
00291 *****************************************************************************/
00301 template <typename OutputStream, typename Float_>
00302 OutputStream& operator<<(OutputStream &ostream , const Pose3D<Float_> &pose) {
00303         ecl::Format<Float_> format;
00304         format.width(6);
00305         format.precision(3);
00306         for ( unsigned int i = 0; i < 3; ++i ) {
00307                 ostream << "[ ";
00308                 for ( unsigned int j = 0; j < 3; ++j ) {
00309                         ostream << format(pose.rot(i,j)) << " ";
00310                 }
00311                 ostream << "] [ ";
00312                 ostream << format(pose.trans(i)) << " ]\n";
00313         }
00314         ostream.flush();
00315   return ostream;
00316 }
00317 
00318 } // namespace ecl
00319 
00320 #endif /* ECL_GEOMETRY_POSE3D_EIGEN2_HPP_ */


ecl_geometry
Author(s): Daniel Stonier
autogenerated on Sun Oct 5 2014 23:36:08