Resolve "Complete state vector new data structure?"

Closes #287 (closed)

include/core/state_block/has_state_blocks.h

@@ -73,7 +73,9 @@ class HasStateBlocks
         // States
         VectorComposite getState(const StateStructure& structure="") const;
         void setState(const VectorComposite& _state, const bool _notify = true);
+        void setState(const Eigen::VectorXd& _state, const StateStructure& _structure, const std::list<SizeEigen>& _sizes, const bool _notify = true);
         void setState(const Eigen::VectorXd& _state, const StateStructure& _sub_structure="", const bool _notify = true);
+        void setState(const StateStructure& _structure, const std::list<VectorXd>& _vectors, const bool _notify = true);
 
         VectorXd getStateVector(const StateStructure& structure="") const;
         unsigned int getSize(const StateStructure& _sub_structure="") const;
@@ -235,18 +237,55 @@ inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, const StateS
     assert(_state.size() == size && "In FrameBase::setState wrong state size");
 
     unsigned int index = 0;
-    for (const char key : structure)
+    for (const char ckey : structure)
     {
-        const auto& sb = getStateBlock(key);
-        if (!sb){
-            WOLF_ERROR("Stateblock key ", key, " not in the structure");
-        }
+        const auto& key = string(1,ckey);
+        const auto& sb  = getStateBlock(key);
+        assert (sb && "Stateblock 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();
     }
 
 }
 
+inline void HasStateBlocks::setState (const Eigen::VectorXd& _state,
+                                      const StateStructure& _structure,
+                                      const std::list<SizeEigen>& _sizes,
+                                      const bool _notify)
+{
+    SizeEigen index = 0;
+    auto size_it = _sizes.begin();
+    for (const auto& ckey : _structure)
+    {
+        const auto& key = string(1,ckey);
+        const auto& sb = getStateBlock(key);
+        assert (sb && "Stateblock key not in the structure");
+        assert(*size_it == sb->getSize() && "State block size mismatch");
+
+        sb->setState(_state.segment(index, *size_it), _notify);
+        index += *size_it;
+        size_it ++;
+    }
+}
+
+
+inline void HasStateBlocks::setState(const StateStructure& _structure, const std::list<VectorXd>& _vectors, const bool _notify)
+{
+    auto vec_it = _vectors.begin();
+    for (const auto ckey : _structure)
+    {
+        const auto key = string(1,ckey);
+        const auto& sb = getStateBlock(key);
+        assert (sb && "Stateblock key not in the structure");
+        assert(vec_it->size() == sb->getSize() && "State block size mismatch");
+
+        sb->setState(*vec_it, _notify);
+        vec_it ++;
+    }
+}
+
+
 //// _sub_structure can be either stateblock structure of the node or a subset of this structure
 inline VectorXd HasStateBlocks::getStateVector(const StateStructure& _sub_structure) const
 {