Partially implement IsMotion to reduce pure virtuals

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_ */

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

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);