State.hpp

Go to the documentation of this file.

#ifndef __ODOMETRY_STATE_HPP__
#define __ODOMETRY_STATE_HPP__

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

namespace odometry
{
    template <class BodyState_>
    class State
    {
    public:
        State() 
            : update_counter(0) {}

    virtual ~State() {};

        virtual void update( const BodyState_& state )
        {
            // make the current configuration the previous configuration
            state_k = state_kp;
            // set the current configuration with the new values
            state_kp = state;
            update_counter++;
        }

        bool isValid()
        {
            return update_counter > 1;
        }

        base::Time getTimeDelta() const
        {
            return state_kp.time - state_k.time;
        }

        const BodyState_& getCurrent() const
        {
            return state_kp;
        }

        const BodyState_& getPrevious() const
        {
            return state_k;
        }

    protected:
        size_t update_counter;

        BodyState_ state_k, state_kp;
    };
}

#endif