Move state accessors to HasStateBlocks

include/core/frame/frame_base.h

@@ -39,7 +39,6 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
         TrajectoryBaseWPtr trajectory_ptr_;
         CaptureBasePtrList capture_list_;
         FactorBasePtrList constrained_by_list_;
-        std::string structure_;
 
         static unsigned int frame_id_count_;
 
@@ -107,10 +106,6 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
         // States
     public:
         void setState(const Eigen::VectorXs& _state);
-        Eigen::VectorXs getState() const;
-        void getState(Eigen::VectorXs& _state) const;
-        unsigned int getSize() const;
-        unsigned int getLocalSize() const;
         bool getCovariance(Eigen::MatrixXs& _cov) const;
 
         // Wolf tree access ---------------------------------------------------

include/core/state_block/has_state_blocks.h

@@ -9,7 +9,6 @@
 #define STATE_BLOCK_HAS_STATE_BLOCKS_H_
 
 #include "core/common/wolf.h"
-#include "core/state_block/state_block.h"
 
 #include <map>
 
@@ -20,8 +19,12 @@ class HasStateBlocks
 {
     public:
         HasStateBlocks();
+        HasStateBlocks(const std::string& _structure) : structure_(_structure){}
         virtual ~HasStateBlocks();
 
+        const std::string& getFrameStructure() const {return structure_;}
+        std::string& getFrameStructure(){return structure_;}
+        void appendToStructure(const std::string& _frame_type){structure_ += _frame_type;}
         const std::map<std::string, StateBlockPtr>& getStateBlockMap() const;
         std::map<std::string, StateBlockPtr>& getStateBlockMap();
 
@@ -52,14 +55,8 @@ class HasStateBlocks
         }
         StateBlockPtr getStateBlock(const std::string& _sb_type) const;
         StateBlockPtr getStateBlock(const char& _sb_type) const {return getStateBlock(std::string(1,_sb_type));}
-        unsigned int removeStateBlock(const std::string& _sb_type)
-        {
-            return state_block_map_.erase(_sb_type);
-        }
-        unsigned int removeStateBlock(const char& _sb_type)
-        {
-            return removeStateBlock(std::string(1,_sb_type));
-        }
+        unsigned int removeStateBlock(const std::string& _sb_type);
+        unsigned int removeStateBlock(const char& _sb_type);
 
         // Emplace derived state blocks (angle, quaternion, etc).
         template<typename SB, typename ... Args>
@@ -69,12 +66,29 @@ class HasStateBlocks
         template<typename ... Args>
         inline std::__1::shared_ptr<StateBlock> emplaceStateBlock(const std::string& _sb_type, Args&&... _args_of_base_state_block_constructor);
 
+        // States
+    public:
+        void setState(const Eigen::VectorXs& _state, const bool _notify = true);
+        Eigen::VectorXs getState() const;
+        void getState(Eigen::VectorXs& _state) const;
+        unsigned int getSize() const;
+        unsigned int getLocalSize() const;
 
     private:
         std::map<std::string, StateBlockPtr> state_block_map_;
+        std::string structure_;
 
 };
 
+} // namespace wolf
+
+
+//////////// IMPLEMENTATION /////////////
+
+#include "core/state_block/state_block.h"
+
+namespace wolf{
+
 inline HasStateBlocks::HasStateBlocks() :
         state_block_map_()
 {
@@ -103,6 +117,16 @@ inline wolf::StateBlockPtr HasStateBlocks::setStateBlock(const std::string& _sb_
     return _sb;
 }
 
+inline unsigned int HasStateBlocks::removeStateBlock(const char& _sb_type)
+{
+    return removeStateBlock(std::string(1, _sb_type));
+}
+
+inline unsigned int HasStateBlocks::removeStateBlock(const std::string& _sb_type)
+{
+    return state_block_map_.erase(_sb_type);
+}
+
 template<typename SB, typename ... Args>
 inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const std::string& _sb_type, Args&&... _args_of_derived_state_block_constructor)
 {
@@ -205,5 +229,63 @@ inline bool HasStateBlocks::isFixed() const
     return fixed;
 }
 
+inline void HasStateBlocks::setState(const Eigen::VectorXs& _state, const bool _notify)
+{
+    int size = getSize();
+
+    assert(_state.size() == size && "In FrameBase::setState wrong state size");
+
+    unsigned int index = 0;
+    for (const char& key : getFrameStructure()) // note: key is a char
+    {
+        const auto& sb = getStateBlock(key); // note: we need a string, not a char. Use string constructor s(1,char).
+
+        sb->setState(_state.segment(index, sb->getSize()), _notify); // do not notify if state block is not estimated by the solver
+        index += sb->getSize();
+    }
+
+}
+
+inline void HasStateBlocks::getState(Eigen::VectorXs& _state) const
+{
+    _state.resize(getSize());
+
+    unsigned int index = 0;
+    for (const char& key : getFrameStructure()) // note: key is a char
+    {
+        const auto& sb = getStateBlock(key); // note: we need a string, not a char. Use string constructor s(1,char).
+
+        _state.segment(index,sb->getSize()) = sb->getState();
+        index += sb->getSize();
+    }
+
+}
+
+inline Eigen::VectorXs HasStateBlocks::getState() const
+{
+    Eigen::VectorXs state;
+
+    getState(state);
+
+    return state;
+}
+
+inline unsigned int HasStateBlocks::getSize() const
+{
+    unsigned int size = 0;
+    for (const auto& pair_key_sb : getStateBlockMap())
+            size += pair_key_sb.second->getSize();
+    return size;
+}
+
+inline unsigned int HasStateBlocks::getLocalSize() const
+{
+    unsigned int size = 0;
+    for (const auto& pair_key_sb : getStateBlockMap())
+            size += pair_key_sb.second->getLocalSize();
+    return size;
+}
+
+
 } // namespace wolf
 #endif /* STATE_BLOCK_HAS_STATE_BLOCKS_H_ */

src/frame/frame_base.cpp

@@ -13,8 +13,8 @@ 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(""),
             trajectory_ptr_(),
-            structure_(""),
             frame_id_(++frame_id_count_),
             type_(NON_ESTIMATED),
             time_stamp_(_ts)
@@ -22,24 +22,24 @@ FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _
     if (_p_ptr)
     {
         setStateBlock("P", _p_ptr);
-        structure_ += "P";
+        appendToStructure("P");
     }
     if (_o_ptr)
     {
         setStateBlock("O", _o_ptr);
-        structure_ += "O";
+        appendToStructure("O");
     }
     if (_v_ptr)
     {
         setStateBlock("V", _v_ptr);
-        structure_ += "V";
+        appendToStructure("V");
     }
 }
 
 FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) :
             NodeBase("FRAME", "Base"),
+            HasStateBlocks(""),
             trajectory_ptr_(),
-            structure_(""),
             frame_id_(++frame_id_count_),
             type_(_tp),
             time_stamp_(_ts)
@@ -47,24 +47,24 @@ FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr
     if (_p_ptr)
     {
         setStateBlock("P", _p_ptr);
-        structure_ += "P";
+        appendToStructure("P");
     }
     if (_o_ptr)
     {
         setStateBlock("O", _o_ptr);
-        structure_ += "O";
+        appendToStructure("O");
     }
     if (_v_ptr)
     {
         setStateBlock("V", _v_ptr);
-        structure_ += "V";
+        appendToStructure("V");
     }
 }
 
 FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXs& _x) :
            NodeBase("FRAME", "Base"),
+           HasStateBlocks(_frame_structure),
            trajectory_ptr_(),
-           structure_(_frame_structure),
            frame_id_(++frame_id_count_),
            type_(_tp),
            time_stamp_(_ts)
@@ -154,7 +154,7 @@ void FrameBase::registerNewStateBlocks()
 
 void FrameBase::removeStateBlocks()
 {
-    for (const char& key : structure_) // note: key is a char
+    for (const char& key : getFrameStructure()) // note: key is a char
     {
         auto sbp = getStateBlock(key);
         if (sbp != nullptr)
@@ -213,59 +213,7 @@ void FrameBase::setAux()
 
 void FrameBase::setState(const Eigen::VectorXs& _state)
 {
-    int size = getSize();
-
-    assert(_state.size() == size && "In FrameBase::setState wrong state size");
-
-    unsigned int index = 0;
-    for (const char& key : structure_) // note: key is a char
-    {
-        const auto& sb = getStateBlock(key); // note: we need a string, not a char. Use string constructor s(1,char).
-
-        sb->setState(_state.segment(index, sb->getSize()), isKeyOrAux()); // do not notify if state block is not estimated by the solver
-        index += sb->getSize();
-    }
-
-}
-
-void FrameBase::getState(Eigen::VectorXs& _state) const
-{
-    _state.resize(getSize());
-
-    unsigned int index = 0;
-    for (const char& key : structure_) // note: key is a char
-    {
-        const auto& sb = getStateBlock(key); // note: we need a string, not a char. Use string constructor s(1,char).
-
-        _state.segment(index,sb->getSize()) = sb->getState();
-        index += sb->getSize();
-    }
-
-}
-
-Eigen::VectorXs FrameBase::getState() const
-{
-    Eigen::VectorXs state;
-
-    getState(state);
-
-    return state;
-}
-
-unsigned int FrameBase::getSize() const
-{
-    unsigned int size = 0;
-    for (const auto& pair_key_sb : getStateBlockMap())
-            size += pair_key_sb.second->getSize();
-    return size;
-}
-
-unsigned int FrameBase::getLocalSize() const
-{
-    unsigned int size = 0;
-    for (const auto& pair_key_sb : getStateBlockMap())
-            size += pair_key_sb.second->getLocalSize();
-    return size;
+    HasStateBlocks::setState(_state, isKeyOrAux());
 }
 
 bool FrameBase::getCovariance(Eigen::MatrixXs& _cov) const