diff --git a/include/core/processor/is_motion.h b/include/core/processor/is_motion.h index e749ec2140bad24d93c5cccf0fb25c91f7698346..f3cf0d781f39c9fda6ffb9e64817ceed0acfa162 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,17 +37,18 @@ 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; }; } ///// IMPLEMENTATION /////// +#include "core/common/time_stamp.h" namespace wolf{ @@ -62,6 +56,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 4beca1b195d9f718405629a2c1f6a52c7e30cc61..b08862e676c294dba7e0a82d59b9cc4891a0812c 100644 --- a/include/core/processor/processor_motion.h +++ b/include/core/processor/processor_motion.h @@ -166,12 +166,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 @@ -179,12 +176,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 @@ -506,25 +498,6 @@ inline bool ProcessorMotion::voteForKeyFrame() const return false; } -inline Eigen::VectorXd ProcessorMotion::getState(const TimeStamp& _ts) const -{ - Eigen::VectorXd x(getProblem()->getFrameStructureSize()); - getState(_ts, x); - return x; -} - -inline TimeStamp ProcessorMotion::getCurrentTimeStamp() const -{ - return getBuffer().get().back().ts_; -} - -inline Eigen::VectorXd ProcessorMotion::getCurrentState() const -{ - Eigen::VectorXd x(getProblem()->getFrameStructureSize()); - 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 73e63e5efbe515b6db54f457cd6f2a541e9e0907..39a3d83f425201b6bc148bdd8c3ca93681ac4329 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -369,6 +369,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);