pose3d.hpp

Go to the documentation of this file.

/*****************************************************************************
** Ifdefs
*****************************************************************************/

#ifndef ECL_GEOMETRY_POSE3D_HPP_
#define ECL_GEOMETRY_POSE3D_HPP_

/*****************************************************************************
** Includes
*****************************************************************************/

#include <ecl/linear_algebra.hpp>

#include <ecl/config/macros.hpp>
#include "pose2d.hpp"

/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace ecl {

/*****************************************************************************
** Interface [Pose3D]
*****************************************************************************/

template <class Float, typename Enable = void>
class ECL_LOCAL Pose3D {
private:
        Pose3D() {}; 
};

template<typename Float>
class ECL_PUBLIC Pose3D<Float, typename enable_if<is_float<Float> >::type> {
public:
        /******************************************
        ** Eigen Alignment
        *******************************************/
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW // http://eigen.tuxfamily.org/dox/StructHavingEigenMembers.html

        /******************************************
        ** Typedef
        *******************************************/
        typedef Float Scalar; 
        typedef ecl::linear_algebra::Matrix<Float,3,3> RotationMatrix; 
        typedef ecl::linear_algebra::Matrix<Float,3,1> Translation;  
        /******************************************
        ** Constructors
        *******************************************/
        Pose3D() : rot(RotationMatrix::Identity()), trans(Translation::Zero()) {}

        template<typename Rot, typename Trans>
//      Pose3D(const ecl::linear_algebra::MatrixBase<Rot>& rotation, const ecl::linear_algebra::MatrixBase<Trans>& translation) :
        Pose3D(const ecl::linear_algebra::EigenBase<Rot>& rotation, const ecl::linear_algebra::EigenBase<Trans>& translation) :
                rot(rotation),
                trans(translation)
        {}

        template <enum Pose2DStorageType Storage_>
        Pose3D(const Pose2D<Float,Storage_>& pose) :
            rot(RotationMatrix::Identity()),
            trans(Translation::Zero())
        {
                rot.template block<2,2>(0,0) = pose.rotationMatrix();
                trans.template segment<2>(0) = pose.translation();
        }

        template<typename Trans>
        Pose3D(const ecl::linear_algebra::AngleAxis<Float>& angle_axis, const ecl::linear_algebra::MatrixBase<Trans>& translation) :
            rot(RotationMatrix::Identity()),
            trans(Translation::Zero())
        {
                /* TODO */
        }
        template<typename Trans>
        Pose3D(const ecl::linear_algebra::Quaternion<Float>& quaternion, const ecl::linear_algebra::MatrixBase<Trans>& translation) :
            rot(quaternion.toRotationMatrix()),
            trans(translation)
        {}

        Pose3D(const Pose3D<Float>& pose) :
                rot(pose.rotation()),
                trans(pose.translation())
        {}

        virtual ~Pose3D() {}

        /******************************************
        ** Assignment
        *******************************************/
        template <enum Pose2DStorageType Storage_>
        Pose3D<Float>& operator=(const Pose2D<Float,Storage_>& pose) {
                rot.template block<2,2>(0,0) = pose.rotationMatrix();
                (rot.template block<2,1>(0,2)) << 0.0, 0.0;
                (rot.template block<1,3>(2,0)) << 0.0, 0.0, 1.0;
                trans.template segment<2>(0) = pose.translation();
                trans[2] = 0.0;
                return *this;
        }

        Pose3D<Float>& operator=(const Pose3D<Float>& pose) {
                rot = pose.rotation();
                trans = pose.translation();
                return *this;
        }
        /******************************************
        ** Eigen Style Setters
        *******************************************/
        void rotation(const RotationMatrix &rotation) {
                rot = rotation;
        }
        template <typename Trans>
        void translation(const ecl::linear_algebra::MatrixBase<Trans>& translation) {
                this->trans = translation;
        }
        /******************************************
        ** Convenience Setters
        *******************************************/
        // set from EigenBase (aka affine transforms)
        // set from Quaternions
        // set from AngleAxis

        /******************************************
        ** Eigen Style Accessors
        *******************************************/
        RotationMatrix& rotation() { return rot; } 
        Translation& translation() { return trans; } 
        const RotationMatrix& rotation() const { return rot; } 
        const Translation& translation() const { return trans; }  
        /******************************************
        ** Convenience Accessors
        *******************************************/
        // get a Quaternion for the rotation part
        // get an AngleAxis for the rotation part
        // get an Affine Transform (4x4) for the big lebowski
        RotationMatrix rotationMatrix() const { return rot; }  
        /******************************************
        ** Operators
        *******************************************/
    Pose3D<Float> inverse() const {
        Pose3D<Float> inverse;
        inverse.rotation(rot.transpose());
        inverse.translation(-1*(inverse.rot*trans));
        return inverse;
    }
    template <typename Float_>
    Pose3D<Float> operator*(const Pose3D<Float_> &pose) const {
                return Pose3D<Float>(rotation()*pose.rotation(),translation() + rotation()*pose.translation());
    }

    // Do we need operator* for Pose2D rh values?
    // Probably better if we code like pose_3d*Pose3D(pose_2d) to avoid vague ambiguities.

        template <typename Float_>
    Pose3D<Float>& operator*=(const Pose3D<Float_> &pose) {
        // This probably needs checking for aliasing...could also be (marginally) sped up.
        *this = (*this)*pose;
        return (*this);
    }
        template <typename Float_>
    Pose3D<Float> relative(const Pose3D<Float_> &pose) const {
                return pose.inverse()*(*this);
    }

        // how to do pose2d*pose3d without circular header? -> EigenBase? -> External operators?

    /*********************
        ** Streaming
        **********************/
        template <typename OutputStream, typename Float_>
        friend OutputStream& operator<<(OutputStream &ostream , const Pose3D<Float_> &pose);

private:
        RotationMatrix rot;
        Translation trans;
};

/*****************************************************************************
** Insertion Operators
*****************************************************************************/
template <typename OutputStream, typename Float_>
OutputStream& operator<<(OutputStream &ostream , const Pose3D<Float_> &pose) {
        ecl::Format<Float_> format;
        format.width(6);
        format.precision(3);
        for ( unsigned int i = 0; i < 3; ++i ) {
                ostream << "[ ";
                for ( unsigned int j = 0; j < 3; ++j ) {
                        ostream << format(pose.rot(i,j)) << " ";
                }
                ostream << "] [ ";
                ostream << format(pose.trans(i)) << " ]\n";
        }
        ostream.flush();
  return ostream;
}

} // namespace ecl

// This is more convenient and less bughuntish than always assigning allocators with your vectors
// but currently fails on oneiric with gcc 4.6 (bugfixed in eigen, but not yet out in oneiric).
//EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(ecl::Pose3D<float>)
//EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(ecl::Pose3D<double>)

#endif /* ECL_GEOMETRY_POSE3D_HPP_ */