Add odometry things to IsMotion

include/core/processor/is_motion.h

@@ -30,12 +30,18 @@ class IsMotion
         virtual VectorComposite getState(const StateStructure& _structure = "") const = 0;
         virtual VectorComposite getState(const TimeStamp& _ts, const StateStructure& _structure = "") const = 0;
 
+        VectorComposite getOdometry ( ) const;
+    private:
+        void setOdometry(const VectorComposite& _zero_odom) {odometry_ = _zero_odom;}
+
+    public:
         const StateStructure& getStateStructure ( ) { return state_structure_; };
         void setStateStructure(std::string _state_structure) { state_structure_ = _state_structure; };
         void addToProblem(ProblemPtr _prb_ptr, IsMotionPtr _motion_ptr);
-   
+
     protected:
         StateStructure state_structure_; ///< The structure of the state vector (to retrieve state blocks from frames)
+        VectorComposite odometry_;
 
 };
 
@@ -45,6 +51,12 @@ inline IsMotion::IsMotion(const StateStructure& _structure) :
     //
 }
 
+
+inline wolf::VectorComposite IsMotion::getOdometry ( ) const
+{
+    return odometry_;
+}
+
 }
 
 /////  IMPLEMENTATION ///////

src/processor/is_motion.cpp

@@ -5,5 +5,6 @@ using namespace wolf;
 
 void IsMotion::addToProblem(ProblemPtr _prb_ptr, IsMotionPtr _motion_ptr)
 {
+    setOdometry(_prb_ptr->stateZero(state_structure_));
     _prb_ptr->addProcessorIsMotion(_motion_ptr);
 }