measurements.h

Go to the documentation of this file.

#ifndef TAG_MEASUREMENTS_H
#define TAG_MEASUREMENTS_H

#include <TooN/se3.h>

namespace tag {


template <class State>
class IncrementalPose {
public:
    static const int M_DIMENSION = 6;

    TooN::Matrix<M_DIMENSION> covariance;
    TooN::Matrix<M_DIMENSION,State::STATE_DIMENSION> jacobian;
    TooN::Vector<M_DIMENSION> measurement;

    IncrementalPose(void){
        TooN::Identity(covariance);
        TooN::Zero(jacobian);
        TooN::Identity(jacobian.template slice<0,0,6,6>());
        TooN::Zero(measurement);
    }

    inline TooN::Matrix<M_DIMENSION,State::STATE_DIMENSION> & getMeasurementJacobian( const State & state ){
        return jacobian;
    }

    inline TooN::Matrix<M_DIMENSION> & getMeasurementCovariance( const State & state ){
        return covariance;
    }

    inline TooN::Vector<M_DIMENSION> & getInnovation( const State & state ){
        return measurement;
    }

    inline void setMeasurement( const TooN::SE3 & increment ){
        measurement = increment.ln();
    }
};

template <class State>
class WorldPose {
public:
    static const int M_DIMENSION = 6;

    TooN::Matrix<M_DIMENSION> covariance;
    TooN::Matrix<M_DIMENSION,State::STATE_DIMENSION> jacobian;
    TooN::SE3 measurement;

    WorldPose(void){
        TooN::Identity(covariance);
        TooN::Zero(jacobian);
        TooN::Identity(jacobian.template slice<0,0,6,6>());
    }

    inline TooN::Matrix<M_DIMENSION,State::STATE_DIMENSION> & getMeasurementJacobian( const State & state ){
        return jacobian;
    }

    inline const TooN::Matrix<M_DIMENSION> & getMeasurementCovariance( const State & state ) const {
        TooN::Matrix<M_DIMENSION> localCovariance = covariance;
        state.pose.adjoint(localCovariance);
        return localCovariance;
    }

    inline const TooN::Vector<M_DIMENSION> & getInnovation( const State & state ) const {
        return (measurement * state.pose.inverse()).ln();
    }
};

template <class State>
class WorldPosition {
public:

    static const int M_DIMENSION = 3;
    TooN::Vector<M_DIMENSION> position;
    TooN::Matrix<M_DIMENSION> covariance;
    TooN::Matrix<M_DIMENSION,State::STATE_DIMENSION> jacobian;

    WorldPosition(void){
        TooN::Identity(covariance);
        TooN::Zero(position);
        TooN::Zero(jacobian);
        TooN::Identity(jacobian.template slice<0,0,3,3>());
    }

    inline const TooN::Matrix<M_DIMENSION,State::STATE_DIMENSION> & getMeasurementJacobian( const State & state ) const {
        return jacobian;
    }

    inline const TooN::Matrix<M_DIMENSION> getMeasurementCovariance( const State & state ) const {
        TooN::Matrix<2 * M_DIMENSION> localCovariance;
        TooN::Zero(localCovariance);
        localCovariance.template slice<0,0,3,3>() = covariance;
        state.pose.adjoint(localCovariance);
        return localCovariance.template slice<0,0,3,3>();
    }

    inline const TooN::Vector<M_DIMENSION> getInnovation( const State & state ) const {
        return -(state.pose * position);
    }

    inline void setCovariance( double sigma ){
        TooN::Identity(covariance, sigma);
    }

    inline void setCovariance( const TooN::Vector<M_DIMENSION> & sigma ){
        TooN::Zero(covariance);
        for(int i = 0; i < M_DIMENSION; ++i){
            covariance(i,i) = sigma[i];
        }
    }
};

template <class State>
class AngularVelocity {
public:

    static const int M_DIMENSION = 3;

    TooN::Vector<3> gyro;
    TooN::Matrix<M_DIMENSION> covariance;
    TooN::Matrix<M_DIMENSION,State::STATE_DIMENSION> jacobian;

    AngularVelocity(void){
        TooN::Identity(covariance);
        TooN::Zero(gyro);
        // angularVelocity jacobian
        TooN::Zero(jacobian);
        TooN::Identity(jacobian.template slice<0,9,3,3>());
    }

    inline TooN::Matrix<M_DIMENSION,State::STATE_DIMENSION> & getMeasurementJacobian( const State & state ){
        return jacobian;
    }

    inline TooN::Matrix<M_DIMENSION> & getMeasurementCovariance( const State & state ){
        return covariance;
    }

    inline TooN::Vector<M_DIMENSION> getInnovation( const State & state ){
        // angular velocity
        return  (-gyro - state.angularVelocity);
    }

    inline void setCovariance( double sigma ){
        TooN::Identity(covariance, sigma);
    }
};

template <class State>
class WorldDirection {
public:

    static const int M_DIMENSION = 3;

    TooN::Vector<3> measurement;
    TooN::Vector<3> reference;

    TooN::Matrix<M_DIMENSION> covariance;

    WorldDirection(void){
        TooN::Identity(covariance);
        TooN::Zero(measurement);
        TooN::Zero(reference);
    }

    inline TooN::Matrix<M_DIMENSION,State::STATE_DIMENSION> & getMeasurementJacobian( const State & state ){
        TooN::Matrix<M_DIMENSION,State::STATE_DIMENSION> result;
        TooN::Zero(result);
        // direction jacobian
        TooN::Vector<M_DIMENSION> local = state.pose.get_rotation() * reference;
        result.template slice<0,3,3,1>() = TooN::SO3::generator_field(0, local ).as_col();
        result.template slice<0,4,3,1>() = TooN::SO3::generator_field(1, local ).as_col();
        result.template slice<0,5,3,1>() = TooN::SO3::generator_field(2, local ).as_col();
        return result;
    }

    inline TooN::Matrix<M_DIMENSION> & getMeasurementCovariance( const State & state ){
        return covariance;
    }

    inline TooN::Vector<M_DIMENSION> & getInnovation( const State & state ){
        return (measurement - (state.pose.get_rotation() * reference));
    }

    inline void setCovariance( double sigma ){
        TooN::Identity(covariance, sigma);
    }
};

} // namespace tag

#endif

---