From e27109f225c42835d7c364dcb34b9939c186daef Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Wed, 17 Jun 2020 19:14:28 +0200
Subject: [PATCH] Add odometry things to IsMotion
---
include/core/processor/is_motion.h | 14 +++++++++++++-
src/processor/is_motion.cpp | 1 +
2 files changed, 14 insertions(+), 1 deletion(-)
diff --git a/include/core/processor/is_motion.h b/include/core/processor/is_motion.h
index c78994658..dc0487046 100644
--- a/include/core/processor/is_motion.h
+++ b/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 ///////
diff --git a/src/processor/is_motion.cpp b/src/processor/is_motion.cpp
index 1bfd06a5e..a17142e80 100644
--- a/src/processor/is_motion.cpp
+++ b/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);
}
--
GitLab