Merge branch 'devel' of...

Merge branch 'devel' of ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/wolf.git into devel

include/core/processor/is_motion.h

@@ -22,6 +22,7 @@ class IsMotion
 {
     public:
 
+        IsMotion(const StateStructure& _structure);
         virtual ~IsMotion();
 
         // Queries to the processor:
@@ -29,15 +30,33 @@ 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_;
 
 };
 
+inline IsMotion::IsMotion(const StateStructure& _structure) :
+        state_structure_(_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);
 }

src/processor/processor_base.cpp

@@ -118,7 +118,7 @@ void ProcessorBase::setProblem(ProblemPtr _problem)
     // adding processor is motion to the processor is motion vector
     auto is_motion_ptr = std::dynamic_pointer_cast<IsMotion>(shared_from_this());
     if (is_motion_ptr)
-        getProblem()->addProcessorIsMotion(is_motion_ptr);
+        is_motion_ptr->addToProblem(_problem, is_motion_ptr);
 }
 
 /////////////////////////////////////////////////////////////////////////////////////////

src/processor/processor_motion.cpp

@@ -23,6 +23,7 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
                                  SizeEigen _calib_size,
                                  ParamsProcessorMotionPtr _params_motion) :
         ProcessorBase(_type, _dim, _params_motion),
+        IsMotion(_state_structure),
         params_motion_(_params_motion),
         processing_step_(RUNNING_WITHOUT_KF),
         x_size_(_state_size),
@@ -44,7 +45,6 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
         jacobian_delta_(delta_cov_size_, delta_cov_size_),
         jacobian_calib_(delta_cov_size_, calib_size_)
 {   
-    setStateStructure(_state_structure);
     //
 }
 
@@ -725,6 +725,9 @@ void ProcessorMotion::integrateOneStep()
                              jacobian_delta_,
                              jacobian_delta_preint_,
                              jacobian_calib_);
+
+    // integrate odometry
+    statePlusDelta(odometry_, delta_, dt_, odometry_);
 }
 
 void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)

test/gtest_processor_motion.cpp

@@ -423,6 +423,37 @@ TEST_F(ProcessorMotion_test, splitBufferFixPrior)
     C_source->getBuffer().print(1,1,1,0);
 }
 
+TEST_F(ProcessorMotion_test, initOdometry)
+{
+    auto odometry = processor->getOdometry();
+
+    ASSERT_MATRIX_APPROX(odometry.at('P'), Vector2d(0,0), 1e-20);
+    ASSERT_MATRIX_APPROX(odometry.at('O'), Vector1d(0  ), 1e-20);
+}
+
+TEST_F(ProcessorMotion_test, integrateOdometry)
+{
+    auto odometry = processor->getOdometry();
+
+    ASSERT_MATRIX_APPROX(odometry.at('P'), Vector2d(0,0), 1e-20);
+    ASSERT_MATRIX_APPROX(odometry.at('O'), Vector1d(0  ), 1e-20);
+
+    data << 1,0;
+    capture->setData(data);
+
+    capture->setTimeStamp(capture->getTimeStamp() + 1.0);
+    capture->process();
+    odometry = processor->getOdometry();
+    ASSERT_MATRIX_APPROX(odometry.at('P'), Vector2d(1,0), 1e-20);
+    ASSERT_MATRIX_APPROX(odometry.at('O'), Vector1d(0  ), 1e-20);
+
+    capture->setTimeStamp(capture->getTimeStamp() + 1.0);
+    capture->process();
+    odometry = processor->getOdometry();
+    ASSERT_MATRIX_APPROX(odometry.at('P'), Vector2d(2,0), 1e-20);
+    ASSERT_MATRIX_APPROX(odometry.at('O'), Vector1d(0  ), 1e-20);
+}
+
 
 int main(int argc, char **argv)
 {