constantposition.h

Go to the documentation of this file.

#ifndef TAG_CONSTANTPOSITION_H
#define TAG_CONSTANTPOSITION_H

#include <TooN/se3.h>

namespace tag {

namespace ConstantPosition {


class State {
public:
    State(void){
        reset();
    }

    void reset(void){
        pose = TooN::SE3<>();
        covariance = TooN::Identity;
    }

    static const int STATE_DIMENSION = 6;
    TooN::SE3<> pose;
    TooN::Matrix<STATE_DIMENSION> covariance;
};

template <class O>
O & operator<< (O & os , const State & st){
    os << st.pose.ln() << st.pose.inverse().get_translation();
    return os;
}

class Model {
public:
    TooN::Vector<State::STATE_DIMENSION> sigma;
    TooN::Matrix<State::STATE_DIMENSION> jacobian;
    TooN::Matrix<State::STATE_DIMENSION> noise;

    Model(void){
        sigma = TooN::Zeros;
        noise = TooN::Zeros;
        jacobian = TooN::Identity;
    }

    TooN::Matrix<State::STATE_DIMENSION> & getJacobian(const State & state, double dt){
        return jacobian;
    }

    void updateState( State & state, const double dt ){
    }

    TooN::Matrix<State::STATE_DIMENSION> & getNoiseCovariance( double dt ){
        for(unsigned int i = 0; i < 6; i++){
            noise(i,i) = dt * sigma[i];
        }
        return noise;
    }

    void updateFromMeasurement( State & state, const TooN::Vector<State::STATE_DIMENSION> & innovation ){
        state.pose = TooN::SE3<>::exp(innovation) * state.pose;
    }
};

} // namespace ConstantPosition

} // namespace tag

#endif