TransformWithCovariance.hpp

Go to the documentation of this file.

#ifndef __BASE_TRANSFORM_WITH_COVARIANCE_HPP__
#define __BASE_TRANSFORM_WITH_COVARIANCE_HPP__

#include <base/Pose.hpp>
#include <base/Eigen.hpp>

namespace base {

    class TransformWithCovariance
    {

    public:
            typedef base::Matrix6d Covariance;

    public:
        base::Position translation;

        base::Quaterniond orientation;

        Covariance cov;

    public:
        TransformWithCovariance()
            : translation(Position::Zero()) , orientation(Quaterniond::Identity())
        {
            this->invalidateCovariance();
        }
      
        explicit TransformWithCovariance( const base::Affine3d& trans)
        {
            this->setTransform(trans);
            this->invalidateCovariance();
        }

        TransformWithCovariance( const base::Affine3d& trans, const Covariance& cov )
        {
            this->setTransform(trans);
            this->cov = cov;
        }

        TransformWithCovariance(const base::Position& translation, const base::Quaterniond& orientation)
            : translation(translation), orientation(orientation)
        {
            this->invalidateCovariance();
        }

        TransformWithCovariance(const base::Position& translation, const base::Quaterniond& orientation, const Covariance& cov )
            : translation(translation), orientation(orientation), cov(cov)
        {

        }

        static TransformWithCovariance Identity()
        {
            return TransformWithCovariance();
        }
        
        
        friend std::ostream & operator<<(std::ostream &out, const TransformWithCovariance& trans);

        TransformWithCovariance composition( const TransformWithCovariance& trans ) const;

        TransformWithCovariance compositionInv( const TransformWithCovariance& trans ) const;

        TransformWithCovariance preCompositionInv( const TransformWithCovariance& trans ) const;

        TransformWithCovariance operator*( const TransformWithCovariance& trans ) const;

        std::pair<Eigen::Vector3d, Eigen::Matrix3d>
        composePointWithCovariance( const Eigen::Vector3d& point, const Eigen::Matrix3d& cov ) const;
        
        TransformWithCovariance inverse() const;

        const Covariance& getCovariance() const
        {
            return this->cov;
        }

        void setCovariance( const Covariance& cov )
        {
            this->cov = cov;
        }

        const base::Matrix3d getTranslationCov() const
        {
            return this->cov.topLeftCorner<3,3>();
        }
        
        void setTranslationCov(const base::Matrix3d& cov)
        {
            this->cov.topLeftCorner<3,3>() = cov;
        }

        const base::Matrix3d getOrientationCov() const
        {
            return this->cov.bottomRightCorner<3,3>();
        }
        
        void setOrientationCov(const base::Matrix3d& cov)
        {
            this->cov.bottomRightCorner<3,3>() = cov;
        }

        const base::Affine3d getTransform() const
        {
            Affine3d trans (this->orientation);
            trans.translation() = this->translation;
            return trans;
        }

        void setTransform( const base::Affine3d& trans )
        {
            this->translation = trans.translation();
            this->orientation = Quaterniond(trans.rotation());
        }

        const base::Orientation &getOrientation() const
        {
            return this->orientation;
        }

        void setOrientation(const base::Orientation & q)
        {
            this->orientation = q;
        }

        bool hasValidTransform() const
        {
            return !translation.hasNaN() && !orientation.coeffs().hasNaN();
        }

        void invalidateTransform()
        {
            translation = Position::Ones() * NaN<double>();
            orientation.coeffs() = Vector4d::Ones() * NaN<double>();
        }

        bool hasValidCovariance() const
        {
            return !cov.hasNaN();
        }
        
        void invalidateCovariance()
        {
            cov = Covariance::Ones() * NaN<double>();
        }

    protected:
        // The uncertainty transformations are implemented according to: 
        // Pennec X, Thirion JP. A framework for uncertainty and validation of 3-D
        // registration methods based on points and frames. International Journal of
        // Computer Vion. 1997;25(3):203–229. Available at:
        // http://www.springerlink.com/index/JJ25N2Q23T402682.pdf.

        static Eigen::Quaterniond r_to_q( const Eigen::Vector3d& r );

        static Eigen::Vector3d q_to_r( const Eigen::Quaterniond& q );

        static inline double sign( double v );

        static Eigen::Matrix<double,3,3> skew_symmetric( const Eigen::Vector3d& r );

        static Eigen::Matrix<double,4,3> dq_by_dr( const Eigen::Quaterniond& q );

        static Eigen::Matrix<double,3,4> dr_by_dq( const Eigen::Quaterniond& q );

        static Eigen::Matrix<double,4,4> dq2q1_by_dq1( const Eigen::Quaterniond& q2 );

        static Eigen::Matrix<double,4,4> dq2q1_by_dq2( const Eigen::Quaterniond& q1 );

        static Eigen::Matrix<double,3,3> dr2r1_by_r1( const Eigen::Quaterniond& q, const Eigen::Quaterniond& q1, const Eigen::Quaterniond& q2 );

        static Eigen::Matrix<double,3,3> dr2r1_by_r2( const Eigen::Quaterniond& q, const Eigen::Quaterniond& q1, const Eigen::Quaterniond& q2 );

        static Eigen::Matrix<double,3,3> drx_by_dr( const Eigen::Quaterniond& q, const Eigen::Vector3d& x );
    };

    std::ostream & operator<<(std::ostream &out, const TransformWithCovariance& trans);
} // namespaces

#endif