diff --git a/include/core/processor/is_motion.h b/include/core/processor/is_motion.h index 842a8c63ca1a1105e2d44ab01fa4ca68941c45c7..8c99fe72e37698a2210e35a3af0c14bf9168f65e 100644 --- a/include/core/processor/is_motion.h +++ b/include/core/processor/is_motion.h @@ -30,13 +30,6 @@ class IsMotion */ virtual void getCurrentState(Eigen::VectorXd& _x) const = 0; virtual void getCurrentTimeStamp(TimeStamp& _ts) const = 0; - - /** \brief Get the state integrated so far - * \return the state vector - */ - virtual Eigen::VectorXd getCurrentState() const = 0; - virtual TimeStamp getCurrentTimeStamp() const = 0; - /** \brief Fill the state corresponding to the provided time-stamp * \param _ts the time stamp * \param _x the returned state @@ -44,11 +37,11 @@ class IsMotion */ virtual bool getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const = 0; - /** \brief Get the state corresponding to the provided time-stamp - * \param _ts the time stamp - * \return the state vector - */ - virtual Eigen::VectorXd getState(const TimeStamp& _ts) const = 0; + + // Overloaded getters + Eigen::VectorXd getCurrentState() const; + TimeStamp getCurrentTimeStamp() const; + Eigen::VectorXd getState(const TimeStamp& _ts) const; std::string getStateStructure(){return state_structure_;}; void setStateStructure(std::string _state_structure){state_structure_ = _state_structure;}; @@ -61,6 +54,7 @@ class IsMotion } ///// IMPLEMENTATION /////// +#include "core/common/time_stamp.h" namespace wolf{ @@ -68,6 +62,29 @@ inline IsMotion::~IsMotion() { } +inline Eigen::VectorXd IsMotion::getCurrentState() const +{ + Eigen::VectorXd x; + getCurrentState(x); + return x; +} + +inline TimeStamp IsMotion::getCurrentTimeStamp() const +{ + TimeStamp ts; + getCurrentTimeStamp(ts); + return ts; +} + +inline Eigen::VectorXd IsMotion::getState(const TimeStamp& _ts) const +{ + Eigen::VectorXd x; + getState(_ts, x); + return x; +} + + + } /* namespace wolf */ #endif /* PROCESSOR_IS_MOTION_H_ */ diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h index c598b84d0d3aed1a5ee63d914b90afb305a7e615..d883202026e095e3574e5483951d9c185be54c08 100644 --- a/include/core/processor/processor_motion.h +++ b/include/core/processor/processor_motion.h @@ -167,12 +167,9 @@ class ProcessorMotion : public ProcessorBase, public IsMotion */ virtual void getCurrentState(Eigen::VectorXd& _x) const override; virtual void getCurrentTimeStamp(TimeStamp& _ts) const override { _ts = getBuffer().get().back().ts_; } + using IsMotion::getCurrentState; + using IsMotion::getCurrentTimeStamp; - /** \brief Get the state integrated so far - * \return the state vector - */ - virtual Eigen::VectorXd getCurrentState() const override; - virtual TimeStamp getCurrentTimeStamp() const override; /** \brief Fill the state corresponding to the provided time-stamp * \param _ts the time stamp @@ -180,12 +177,7 @@ class ProcessorMotion : public ProcessorBase, public IsMotion * \return if state in the provided time-stamp could be resolved */ virtual bool getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const override; - - /** \brief Get the state corresponding to the provided time-stamp - * \param _ts the time stamp - * \return the state vector - */ - virtual Eigen::VectorXd getState(const TimeStamp& _ts) const override; + using IsMotion::getState; /** \brief Gets the delta preintegrated covariance from all integrations so far * \return the delta preintegrated covariance matrix @@ -507,25 +499,6 @@ inline bool ProcessorMotion::voteForKeyFrame() const return false; } -inline Eigen::VectorXd ProcessorMotion::getState(const TimeStamp& _ts) const -{ - Eigen::VectorXd x(x_size_); - getState(_ts, x); - return x; -} - -inline TimeStamp ProcessorMotion::getCurrentTimeStamp() const -{ - return getBuffer().get().back().ts_; -} - -inline Eigen::VectorXd ProcessorMotion::getCurrentState() const -{ - Eigen::VectorXd x; - getCurrentState(x); - return x; -} - inline void ProcessorMotion::getCurrentState(Eigen::VectorXd& _x) const { // ensure integrity diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index 26910853cd0ec70735077ffad096b43f10d87752..08cd35ab851a5f683c3350fa4045acf94a0ea850 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -373,6 +373,9 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const VectorXd delta_step = motion.jacobian_calib_ * (calib - calib_preint); VectorXd delta = capture_motion->correctDelta( motion.delta_integr_, delta_step); + // ensure proper size of the provided reference + _x.resize( getProblem()->getFrameStructureSize() ); + // Compose on top of origin state using the buffered time stamp, not the query time stamp double dt = motion.ts_ - capture_motion->getBuffer().get().front().ts_; statePlusDelta(state_0, delta, dt, _x);