From 607dae491c2322ce2aaa34ee389abc642d3ddf0a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Wed, 25 Mar 2020 23:50:10 +0100
Subject: [PATCH] WIP Use StateStructure instead of std::string for state
 structures

---
 include/core/capture/capture_base.h         |  2 +-
 include/core/common/wolf.h                  |  5 ++
 include/core/frame/frame_base.h             |  2 +-
 include/core/processor/is_motion.h          |  7 +-
 include/core/state_block/has_state_blocks.h | 87 ++++++++++++---------
 src/capture/capture_base.cpp                |  4 +-
 src/frame/frame_base.cpp                    |  6 +-
 src/landmark/landmark_base.cpp              |  2 +-
 8 files changed, 66 insertions(+), 49 deletions(-)

diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index 70fee2c15..0a52b6193 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.h
@@ -81,7 +81,7 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
         const FactorBasePtrList& getConstrainedByList() const;
 
         // State blocks
-        const std::string& getStructure() const;
+        const StateStructure& getStructure() const;
         StateBlockPtr getStateBlock(const std::string& _key) const;
         StateBlockPtr getStateBlock(const char _key) const { return getStateBlock(std::string(1, _key)); }
         StateBlockPtr getSensorP() const;
diff --git a/include/core/common/wolf.h b/include/core/common/wolf.h
index 9f548fd32..b06c4e7da 100644
--- a/include/core/common/wolf.h
+++ b/include/core/common/wolf.h
@@ -106,6 +106,11 @@ typedef Matrix<double,Dynamic,Dynamic,RowMajor> MatrixRowXd; ///< dynamic rowmaj
 
 namespace wolf {
 
+/// State of nodes containing several state blocks
+typedef std::unordered_map<std::string, Eigen::VectorXd> State;
+typedef std::list<std::string> StateStructure;
+
+
 //////////////////////////////////////////////////////////
 /** Check Eigen Matrix sizes, both statically and dynamically
  *
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index 14bcc2139..74c2b059a 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -69,7 +69,7 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
          **/        
         FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr, StateBlockPtr _v_ptr = nullptr);
 
-        FrameBase(const std::string _frame_structure, const SizeEigen _dim, const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXd& _x);
+        FrameBase(const StateStructure& _frame_structure, const SizeEigen _dim, const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXd& _x);
 
         virtual ~FrameBase();
         virtual void remove(bool viral_remove_empty_parent=false);
diff --git a/include/core/processor/is_motion.h b/include/core/processor/is_motion.h
index 8c99fe72e..f9f77b10c 100644
--- a/include/core/processor/is_motion.h
+++ b/include/core/processor/is_motion.h
@@ -9,6 +9,7 @@
 #define PROCESSOR_IS_MOTION_H_
 
 #include "core/common/wolf.h"
+//#include "core/state_block/has_state_blocks.h"
 
 namespace wolf
 {
@@ -43,11 +44,11 @@ class IsMotion
         TimeStamp       getCurrentTimeStamp() const;
         Eigen::VectorXd getState(const TimeStamp& _ts) const;
 
-        std::string getStateStructure(){return state_structure_;};
-        void setStateStructure(std::string _state_structure){state_structure_ = _state_structure;};
+        const StateStructure& getStateStructure(){return state_structure_;};
+        void setStateStructure(const StateStructure& _state_structure){state_structure_ = _state_structure;};
     
     protected:
-        std::string state_structure_; ///< The structure of the state vector (to retrieve state blocks from frames)
+        StateStructure state_structure_; ///< The structure of the state vector (to retrieve state blocks from frames)
 
 };
 
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index 76df765d2..b1f59dc71 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -10,25 +10,26 @@
 
 #include "core/common/wolf.h"
 
-#include <map>
+#include <unordered_map>
 
 namespace wolf
 {
 
-/// State of nodes containing several state blocks
-typedef std::unordered_map<std::string, Eigen::VectorXd> State;
+///// State of nodes containing several state blocks
+//typedef std::unordered_map<std::string, Eigen::VectorXd> State;
+//typedef std::list<std::string> StateStructure;
 
 
 class HasStateBlocks
 {
     public:
         HasStateBlocks();
-        HasStateBlocks(const std::string& _structure) : structure_(_structure), state_block_map_() {}
+        HasStateBlocks(const StateStructure& _structure) : structure_(_structure), state_block_map_() {}
         virtual ~HasStateBlocks();
 
-        const std::string& getStructure() const { return structure_; }
-        void appendToStructure(const std::string& _frame_type){ structure_ += _frame_type; }
-        bool isInStructure(const std::string& _sb_type) { return structure_.find(_sb_type) != std::string::npos; }
+        const StateStructure& getStructure() const { return structure_; }
+        void appendToStructure(const std::string& _frame_type){ structure_.push_back(_frame_type); }
+        bool isInStructure(const std::string& _sb_type) { return std::find(structure_.begin(), structure_.end(), _sb_type) != structure_.end(); }
 
         const std::map<std::string, StateBlockPtr>& getStateBlockMap() const;
         std::vector<StateBlockPtr> getStateBlockVec() const;
@@ -68,16 +69,16 @@ class HasStateBlocks
         void removeStateBlocks(ProblemPtr _problem);
 
         // States
-        inline void setState(const Eigen::VectorXd& _state, std::string _sub_structure="", const bool _notify = true);
-        void getState(Eigen::VectorXd& _state, std::string structure="") const;
-        Eigen::VectorXd getState(std::string structure="") const;
-        unsigned int getSize(std::string _sub_structure="") const;
-        unsigned int getLocalSize(std::string _sub_structure="") const;
+        inline void setState(const Eigen::VectorXd& _state, const StateStructure& _sub_structure = StateStructure(), const bool _notify = true);
+        void getState(Eigen::VectorXd& _state, const StateStructure& _sub_structure = StateStructure()) const;
+        Eigen::VectorXd getState(const StateStructure& structure = StateStructure()) const;
+        unsigned int getSize(const StateStructure& _sub_structure = StateStructure()) const;
+        unsigned int getLocalSize(const StateStructure& _sub_structure = StateStructure()) const;
 
         State getStateComposite();
 
     private:
-        std::string structure_;
+        StateStructure structure_;
         std::map<std::string, StateBlockPtr> state_block_map_;
 
 };
@@ -92,7 +93,7 @@ class HasStateBlocks
 namespace wolf{
 
 inline HasStateBlocks::HasStateBlocks() :
-        structure_(std::string("")),
+        structure_(),
         state_block_map_()
 {
     //
@@ -161,7 +162,7 @@ inline StateBlockPtr HasStateBlocks::emplaceStateBlock(const std::string& _sb_ty
 
 inline bool HasStateBlocks::setStateBlock(const std::string _sb_type, const StateBlockPtr& _sb)
 {
-    assert (structure_.find(_sb_type) > 0 && "Cannot set a state block out of the state structure! Use addStateBlock instead.");
+    assert (std::find(structure_.begin(), structure_.end(),_sb_type) != structure_.end() && "Cannot set a state block out of the state structure! Use addStateBlock instead.");
     assert ( state_block_map_.count(_sb_type) > 0 && "Cannot set an inexistent state block! Use addStateBlock instead.");
     state_block_map_.at(_sb_type) = _sb;
     return true; // success
@@ -221,16 +222,19 @@ inline bool HasStateBlocks::isFixed() const
     return fixed;
 }
 
-inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, std::string _sub_structure, const bool _notify)
+inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, const StateStructure& _sub_structure, const bool _notify)
 {
-    if (_sub_structure == ""){
-        _sub_structure = structure_;
-    }
-    int size = getSize(_sub_structure);
+    StateStructure sub_structure;
+    if (_sub_structure.empty())
+        sub_structure = structure_;
+    else
+        sub_structure = _sub_structure;
+
+    int size = getSize(sub_structure);
     assert(_state.size() == size && "In FrameBase::setState wrong state size");
 
     unsigned int index = 0;
-    for (const char key : _sub_structure)
+    for (const auto& key : sub_structure)
     {
         const auto& sb = getStateBlock(key);
         if (!sb){
@@ -243,15 +247,17 @@ inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, std::string
 }
 
 // _sub_structure can be either stateblock structure of the node or a subset of this structure
-inline void HasStateBlocks::getState(Eigen::VectorXd& _state, std::string _sub_structure) const
+inline void HasStateBlocks::getState(Eigen::VectorXd& _state, const StateStructure& _sub_structure) const
 {
-    if (_sub_structure == ""){
-        _sub_structure = structure_;
-    }
-    _state.resize(getSize(_sub_structure));
+    StateStructure sub_structure;
+    if (_sub_structure.empty())
+        sub_structure = structure_;
+    else
+        sub_structure = _sub_structure;
+    _state.resize(getSize(sub_structure));
 
     unsigned int index = 0;
-    for (const char key : _sub_structure)
+    for (const auto& key : sub_structure)
     {
         const auto& sb = getStateBlock(key);
         if (!sb){
@@ -263,7 +269,7 @@ inline void HasStateBlocks::getState(Eigen::VectorXd& _state, std::string _sub_s
 
 }
 
-inline Eigen::VectorXd HasStateBlocks::getState(std::string _sub_structure) const
+inline Eigen::VectorXd HasStateBlocks::getState(const StateStructure& _sub_structure) const
 {
     Eigen::VectorXd state;
 
@@ -282,13 +288,16 @@ inline State HasStateBlocks::getStateComposite()
     return state;
 }
 
-inline unsigned int HasStateBlocks::getSize(std::string _sub_structure) const
+inline unsigned int HasStateBlocks::getSize(const StateStructure& _sub_structure) const
 {
-    if (_sub_structure == ""){
-        _sub_structure = structure_;
-    }
+    StateStructure sub_structure;
+    if (_sub_structure.empty())
+        sub_structure = structure_;
+    else
+        sub_structure = _sub_structure;
+
     unsigned int size = 0;
-    for (const char key : _sub_structure)
+    for (const auto& key : sub_structure)
     {
         const auto& sb = getStateBlock(key);
         if (!sb){
@@ -300,13 +309,15 @@ inline unsigned int HasStateBlocks::getSize(std::string _sub_structure) const
     return size;
 }
 
-inline unsigned int HasStateBlocks::getLocalSize(std::string _sub_structure) const
+inline unsigned int HasStateBlocks::getLocalSize(const StateStructure& _sub_structure) const
 {
-    if (_sub_structure == ""){
-        _sub_structure = structure_;
-    }
+    StateStructure sub_structure;
+    if (_sub_structure.empty())
+        sub_structure = structure_;
+    else
+        sub_structure = _sub_structure;
     unsigned int size = 0;
-    for (const char key : _sub_structure)
+    for (const auto& key : sub_structure)
     {
         const auto& sb = getStateBlock(key);
         if (!sb){
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 6e78e69a8..aacaec893 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -14,7 +14,7 @@ CaptureBase::CaptureBase(const std::string& _type,
                          StateBlockPtr _o_ptr,
                          StateBlockPtr _intr_ptr) :
     NodeBase("CAPTURE", _type),
-    HasStateBlocks(""),
+    HasStateBlocks(StateStructure()),
     frame_ptr_(), // nullptr
     sensor_ptr_(_sensor_ptr),
     calib_size_(0),
@@ -130,7 +130,7 @@ void CaptureBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
     constrained_by_list_.remove(_fac_ptr);
 }
 
-const std::string& CaptureBase::getStructure() const
+const StateStructure& CaptureBase::getStructure() const
 {
     if (getSensor())
         return getSensor()->getStructure();
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index d437515e4..74131253f 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -13,7 +13,7 @@ unsigned int FrameBase::frame_id_count_ = 0;
 
 FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) :
             NodeBase("FRAME", "Base"),
-            HasStateBlocks(""),
+            HasStateBlocks(StateStructure()),
             trajectory_ptr_(),
             frame_id_(++frame_id_count_),
             type_(NON_ESTIMATED),
@@ -35,7 +35,7 @@ FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _
 
 FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) :
             NodeBase("FRAME", "Base"),
-            HasStateBlocks(""),
+            HasStateBlocks(StateStructure()),
             trajectory_ptr_(),
             frame_id_(++frame_id_count_),
             type_(_tp),
@@ -55,7 +55,7 @@ FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr
     }
 }
 
-FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXd& _x) :
+FrameBase::FrameBase(const StateStructure& _frame_structure, const SizeEigen _dim, const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXd& _x) :
            NodeBase("FRAME", "Base"),
            HasStateBlocks(_frame_structure),
            trajectory_ptr_(),
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 1e24627be..1a1d785a3 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -14,7 +14,7 @@ unsigned int LandmarkBase::landmark_id_count_ = 0;
 
 LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr) :
         NodeBase("LANDMARK", _type),
-        HasStateBlocks(""),
+        HasStateBlocks(StateStructure()),
         map_ptr_(),
         state_block_vec_(0), // Resize in derived constructors if needed.
         landmark_id_(++landmark_id_count_)
-- 
GitLab