Simplification of has_stateblock changes

include/core/state_block/has_state_blocks.h

@@ -64,15 +64,11 @@ class HasStateBlocks
         void removeStateBlocks(ProblemPtr _problem);
 
         // States
-        virtual void setState(const Eigen::VectorXd& _state, const bool _notify = true);
-        void getState(std::string structure, Eigen::VectorXd& _state) const;
-        Eigen::VectorXd getState(std::string structure) const;
-        Eigen::VectorXd getState() const;
-        void getState(Eigen::VectorXd& _state) const;
-        unsigned int getSize() const;
-        unsigned int getSize(std::string _sub_structure) const;
-        unsigned int getLocalSize() const;
-        unsigned int getLocalSize(std::string _sub_structure) const;
+        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;
 
     private:
         std::string structure_;
@@ -219,17 +215,21 @@ inline bool HasStateBlocks::isFixed() const
     return fixed;
 }
 
-inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, const bool _notify)
+inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, std::string _sub_structure, const bool _notify)
 {
-    int size = getSize();
-
+    if (_sub_structure == ""){
+        _sub_structure = structure_;
+    }
+    int size = getSize(_sub_structure);
     assert(_state.size() == size && "In FrameBase::setState wrong state size");
 
     unsigned int index = 0;
-    for (const char key : getStructure())
+    for (const char key : _sub_structure)
     {
         const auto& sb = getStateBlock(key);
-
+        if (!sb){
+            WOLF_ERROR("Stateblock key ", key, " not in the structure");
+        }
         sb->setState(_state.segment(index, sb->getSize()), _notify); // do not notify if state block is not estimated by the solver
         index += sb->getSize();
     }
@@ -237,20 +237,20 @@ inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, const bool _
 }
 
 // _sub_structure can be either stateblock structure of the node or a subset of this structure
-inline void HasStateBlocks::getState(std::string _sub_structure, Eigen::VectorXd& _state) const
+inline void HasStateBlocks::getState(Eigen::VectorXd& _state, std::string _sub_structure) const
 {
-
+    if (_sub_structure == ""){
+        _sub_structure = structure_;
+    }
     _state.resize(getSize(_sub_structure));
 
     unsigned int index = 0;
     for (const char key : _sub_structure)
     {
         const auto& sb = getStateBlock(key);
-
         if (!sb){
             WOLF_ERROR("Stateblock key ", key, " not in the structure");
         }
-
         _state.segment(index,sb->getSize()) = sb->getState();
         index += sb->getSize();
     }
@@ -261,33 +261,17 @@ inline Eigen::VectorXd HasStateBlocks::getState(std::string _sub_structure) cons
 {
     Eigen::VectorXd state;
 
-    getState(_sub_structure, state);
+    getState(state, _sub_structure);
 
     return state;
 }
 
-inline void HasStateBlocks::getState(Eigen::VectorXd& _state) const
-{
-    getState(getStructure(), _state);
-}
-
-inline Eigen::VectorXd HasStateBlocks::getState() const
-{
-    Eigen::VectorXd state;
-
-    getState(state);
-
-    return state;
-}
-
-
-inline unsigned int HasStateBlocks::getSize() const
-{
-    return getSize(structure_);
-}
 
 inline unsigned int HasStateBlocks::getSize(std::string _sub_structure) const
 {
+    if (_sub_structure == ""){
+        _sub_structure = structure_;
+    }
     unsigned int size = 0;
     for (const char key : _sub_structure)
     {
@@ -301,13 +285,11 @@ inline unsigned int HasStateBlocks::getSize(std::string _sub_structure) const
     return size;
 }
 
-inline unsigned int HasStateBlocks::getLocalSize() const
-{
-    return getLocalSize(structure_);
-}
-
 inline unsigned int HasStateBlocks::getLocalSize(std::string _sub_structure) const
 {
+    if (_sub_structure == ""){
+        _sub_structure = structure_;
+    }
     unsigned int size = 0;
     for (const char key : _sub_structure)
     {