Add many functions to ProcessorMotion

src/CMakeLists.txt

@@ -117,6 +117,7 @@ SET(HDRS
     node_terminus.h
     node_linked.h
     processor_base.h
+    processor_motion.h
     processor_tracker.h
     sensor_base.h
     sensor_camera.h
@@ -165,6 +166,7 @@ SET(SRCS
     map_base.cpp
     node_base.cpp
     processor_base.cpp
+    processor_motion.cpp
     processor_tracker.cpp
     sensor_base.cpp
     sensor_camera.cpp

src/processor_motion.cpp

@@ -8,3 +8,33 @@ ProcessorMotion::~ProcessorMotion()
 {
 }
 
+void ProcessorMotion::composeDeltaState(const TimeStamp& _t_start, TimeStamp& _t_end, Eigen::VectorXs& _Dx)
+{
+    // Get time index
+    i_start_ = (_t_start - ts_origin_) / dt_;
+    i_end_ = (_t_end - ts_origin_) / dt_;
+    if (i_start_ == 0)
+        _Dx = buffer_Dx_[i_end_]; // Trivial delta
+    else
+    {
+        Dx_start_ = buffer_Dx_[i_start_];
+        Dx_end_ = buffer_Dx_[i_end_];
+        minus(Dx_end_, Dx_start_, _Dx);
+    }
+}
+
+void ProcessorMotion::pushBack(TimeStamp& _ts, Eigen::VectorXs& _dx, Eigen::VectorXs& _Dx_integral)
+{
+    // Append to buffers
+    buffer_ts_.push_back(_ts);
+    buffer_dx_.push_back(_dx);
+    buffer_Dx_.push_back(_Dx_integral);
+}
+
+void ProcessorMotion::eraseFront(TimeStamp& _ts)
+{
+    i_end_ = (_ts - ts_origin_) / dt_;
+    buffer_ts_.erase(buffer_ts_.begin(), buffer_ts_.begin() + i_end_ - 1);
+    buffer_dx_.erase(buffer_dx_.begin(), buffer_dx_.begin() + i_end_ - 1);
+    buffer_Dx_.erase(buffer_Dx_.begin(), buffer_Dx_.begin() + i_end_ - 1);
+}

src/processor_motion.h

@@ -8,27 +8,116 @@
 #ifndef SRC_PROCESSOR_MOTION_H_
 #define SRC_PROCESSOR_MOTION_H_
 
+// Wolf
+#include "processor_base.h"
 #include "time_stamp.h"
 #include "wolf.h"
 
+// STL
+#include <deque>
+
 class ProcessorMotion : public ProcessorBase{
     public:
         ProcessorMotion(ProcessorType _tp);
         virtual ~ProcessorMotion();
 
-        // TODO: Implement the pure virtual from base
-        virtual void process(CaptureBase* _capture_ptr){};
+        virtual void process(CaptureBase* _capture_ptr)
+        {
+            extractData(_capture_ptr, ts_, data_);
+            integrate(data_, dt_);
+            pushBack(ts_, dx_, Dx_integral_);
+        }
 
-        // TODO: These three functions will use plus() and minus() below
-        void integrate(TimeStamp& _time_stamp){};
-        void composeDeltaState(const TimeStamp& _t_start, TimeStamp& _t_end, Eigen::VectorXs&){};
-        void composeState(const TimeStamp& _time_stamp, Eigen::VectorXs&){};
+        // Main operations
+        void composeDeltaState(const TimeStamp& _t_start, TimeStamp& _t_end, Eigen::VectorXs&);
+        void composeState(const TimeStamp& _time_stamp, Eigen::VectorXs&);
+        void reset(TimeStamp& _ts);
 
     protected:
-        // TODO: Think about the API of these:
-        virtual void plus(const Eigen::VectorXs& _x, const Eigen::VectorXs& _delta, Eigen::VectorXs& _x_plus_delta) = 0;
+        // Helper functions
+        void integrate(Eigen::VectorXs& _data, WolfScalar _dt);
+        virtual void extractData(CaptureBase* _capture_ptr, TimeStamp& _ts, Eigen::VectorXs& _data) = 0;
+        virtual void data2dx(const Eigen::VectorXs& _data, WolfScalar _dt, Eigen::VectorXs& _dx);
+        void pushBack(TimeStamp& _ts, Eigen::VectorXs& _dx, Eigen::VectorXs& _Dx_integral);
+        void eraseFront(TimeStamp& _ts);
+        virtual void plus(const Eigen::VectorXs& _x, const Eigen::VectorXs& _dx, Eigen::VectorXs& _x_plus_dx) = 0;
         virtual void minus(const Eigen::VectorXs& _x1, const Eigen::VectorXs& _x0, Eigen::VectorXs& _x1_minus_x0) = 0;
 
-        void composeOriginState(Eigen::VectorXs&);
+        WolfScalar computeAverageDt();
+        WolfScalar getDt();
+
+    protected:
+        std::deque<TimeStamp> buffer_ts_;
+        std::deque<Eigen::VectorXs> buffer_dx_;
+        std::deque<Eigen::VectorXs> buffer_Dx_;
+
+    protected:
+        // Helper variables: use them to avoid creating temporaries
+        TimeStamp ts_; // Time stamp of the data being processed
+        TimeStamp ts_origin_; // Time stamp at the origin of buffers
+        Eigen::VectorXs data_; // Last received data
+        Eigen::VectorXs dx_; // A dx value directly resulting from data
+        Eigen::VectorXs Dx_integral_; // The integrated dx's between distant time stamps
+        Eigen::VectorXs x_origin_; // The origin state
+        Eigen::VectorXs x_other_; // Another state, e.g. x_other_ = plus(x_origin_, Dx_)
+        WolfScalar dt_;
+        WolfScalar Dt_start_, Dt_end_;
+        unsigned int i_start_, i_end_;
+        Eigen::VectorXs Dx_start_, Dx_end_;
 };
+
+inline void ProcessorMotion::reset(TimeStamp& _ts)
+{
+    // Pop data from the front of the buffers before _ts
+    // TODO: see if we want to save the popped data somewhere else
+    eraseFront(_ts);
+
+    // Reset origin: time-stamp, state and delta_integral
+    ts_origin_ = _ts;
+    composeState(_ts, x_other_);
+    x_origin_ = x_other_;
+    Dx_integral_.setZero();
+}
+
+inline void ProcessorMotion::integrate(Eigen::VectorXs& _data, WolfScalar _dt)
+{
+    // Make appropriate delta value from data
+    data2dx(_data, _dt, dx_);
+    // integrate on top of Dx_
+    plus(buffer_Dx_.back(), dx_, Dx_integral_);
+}
+
+inline void ProcessorMotion::composeState(const TimeStamp& _time_stamp, Eigen::VectorXs& _x_ts)
+{
+    // Get time index
+    i_end_ = (_time_stamp - ts_origin_) / dt_;
+
+    Dx_end_   = buffer_Dx_[i_end_];
+    plus(x_origin_, Dx_end_, _x_ts);
+}
+
+/** \brief Convert data to increment vector
+ *
+ * Overload this method for non-trivial conversions
+ */
+inline void ProcessorMotion::data2dx(const Eigen::VectorXs& _data, WolfScalar _dt, Eigen::VectorXs& _dx)
+{
+    assert(_data.size() == _dx.size() && "Sizes do not match");
+
+    // Implement the trivial identity converter.
+    _dx = _data * _dt;
+}
+
+inline WolfScalar ProcessorMotion::computeAverageDt()
+{
+    if (buffer_ts_.size() > 0)
+        dt_ = (buffer_ts_.back() - buffer_ts_.front()) / (buffer_ts_.size() - 1);
+    return dt_;
+}
+
+inline WolfScalar ProcessorMotion::getDt()
+{
+    return dt_;
+}
+
 #endif /* SRC_PROCESSOR_MOTION_H_ */