diff --git a/hello_wolf/hello_wolf.cpp b/hello_wolf/hello_wolf.cpp
index e46557c44f1a0f6195eb28a531dd439a80951bbf..0e4695e5a8c82ebe74ae1c67c5fc37cfc396ecea 100644
--- a/hello_wolf/hello_wolf.cpp
+++ b/hello_wolf/hello_wolf.cpp
@@ -226,9 +226,9 @@ int main()
                 sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5);       // We perturb A LOT !
     for (auto kf : problem->getTrajectory()->getFrameList())
         if (kf->isKeyOrAux())
-            for (auto sb : kf->getStateBlockVec())
-                if (sb && !sb->isFixed())
-                    sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5);   // We perturb A LOT !
+            for (auto& pair_key_sb : kf->getStateBlockMap())
+                if (!pair_key_sb.second->isFixed())
+                    pair_key_sb.second->setState(pair_key_sb.second->getState() + VectorXs::Random(pair_key_sb.second->getSize()) * 0.5);   // We perturb A LOT !
     for (auto lmk : problem->getMap()->getLandmarkList())
         for (auto sb : lmk->getStateBlockVec())
             if (sb && !sb->isFixed())
diff --git a/hello_wolf/hello_wolf_autoconf.cpp b/hello_wolf/hello_wolf_autoconf.cpp
index bc53460f2410f7985fd02d73502c79bec9b2842d..5bc4c42b94f13f552f498a86f1ad1143d0782c48 100644
--- a/hello_wolf/hello_wolf_autoconf.cpp
+++ b/hello_wolf/hello_wolf_autoconf.cpp
@@ -219,9 +219,9 @@ int main()
                 sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5);       // We perturb A LOT !
     for (auto kf : problem->getTrajectory()->getFrameList())
         if (kf->isKeyOrAux())
-            for (auto sb : kf->getStateBlockVec())
-                if (sb && !sb->isFixed())
-                    sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5);   // We perturb A LOT !
+            for (auto& pair_key_sb : kf->getStateBlockMap())
+                if (pair_key_sb.second && !pair_key_sb.second->isFixed())
+                    pair_key_sb.second->setState(pair_key_sb.second->getState() + VectorXs::Random(pair_key_sb.second->getSize()) * 0.5);   // We perturb A LOT !
     for (auto lmk : problem->getMap()->getLandmarkList())
         for (auto sb : lmk->getStateBlockVec())
             if (sb && !sb->isFixed())
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index aed6ff774ed52c5d0d4023188515823feca7ffab..a27970fa474f50bbb27de5d384457e09be24dce0 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -7,28 +7,30 @@ namespace wolf{
 class TrajectoryBase;
 class CaptureBase;
 class StateBlock;
+
+/** \brief Enumeration of frame types
+ */
+typedef enum
+{
+    KEY = 2,          ///< key frame. It plays at optimizations (estimated).
+    AUXILIARY = 1,    ///< auxiliary frame. It plays at optimizations (estimated).
+    NON_ESTIMATED = 0 ///< regular frame. It does not play at optimization.
+} FrameType;
 }
 
 //Wolf includes
 #include "core/common/wolf.h"
 #include "core/common/time_stamp.h"
 #include "core/common/node_base.h"
+#include "core/state_block/has_state_blocks.h"
 
 //std includes
 
 namespace wolf {
 
-/** \brief Enumeration of frame types
- */
-typedef enum
-{
-    KEY = 2,          ///< key frame. It plays at optimizations (estimated).
-    AUXILIARY = 1,    ///< auxiliary frame. It plays at optimizations (estimated).
-    NON_ESTIMATED = 0 ///< regular frame. It does not play at optimization.
-} FrameType;
 
 //class FrameBase
-class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase>
+class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_shared_from_this<FrameBase>
 {
     friend CaptureBase;
     friend FactorBase;
@@ -37,7 +39,7 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
         TrajectoryBaseWPtr trajectory_ptr_;
         CaptureBasePtrList capture_list_;
         FactorBasePtrList constrained_by_list_;
-        std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, in the order: Position, Orientation, Velocity.
+//        std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, in the order: Position, Orientation, Velocity.
 
         static unsigned int frame_id_count_;
 
@@ -93,21 +95,21 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
         TimeStamp   getTimeStamp() const;
         void        getTimeStamp(TimeStamp& _ts) const;
 
-        // State blocks
-    public:
-        const std::vector<StateBlockPtr>& getStateBlockVec() const;
-        std::vector<StateBlockPtr>& getStateBlockVec();
-    protected:
-        StateBlockPtr getStateBlock(unsigned int _i) const;
-        void setStateBlock(unsigned int _i, const StateBlockPtr _sb_ptr);
-        void resizeStateBlockVec(unsigned int _size);
-
-    public:
-        StateBlockPtr getP() const;
-        StateBlockPtr getO() const;
+//        // State blocks
+//    public:
+//        const std::vector<StateBlockPtr>& getStateBlockVec() const;
+//        std::vector<StateBlockPtr>& getStateBlockVec();
+//    protected:
+//        StateBlockPtr getStateBlock(unsigned int _i) const;
+//        void setStateBlock(unsigned int _i, const StateBlockPtr _sb_ptr);
+//        void resizeStateBlockVec(unsigned int _size);
+//
+//    public:
+//        StateBlockPtr getP() const;
+//        StateBlockPtr getO() const;
         StateBlockPtr getV() const;
-        void setP(const StateBlockPtr _p_ptr);
-        void setO(const StateBlockPtr _o_ptr);
+//        void setP(const StateBlockPtr _p_ptr);
+//        void setO(const StateBlockPtr _o_ptr);
         void setV(const StateBlockPtr _v_ptr);
     protected:
         void registerNewStateBlocks();
@@ -115,10 +117,10 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
         virtual void setProblem(ProblemPtr _problem) final;
 
         // Fixed / Estimated
-    public:
-        void fix();
-        void unfix();
-        bool isFixed() const;
+//    public:
+//        void fix();
+//        void unfix();
+//        bool isFixed() const;
 
         // States
     public:
@@ -210,55 +212,55 @@ inline TimeStamp FrameBase::getTimeStamp() const
     return time_stamp_;
 }
 
-inline const std::vector<StateBlockPtr>& FrameBase::getStateBlockVec() const
-{
-    return state_block_vec_;
-}
-
-inline std::vector<StateBlockPtr>& FrameBase::getStateBlockVec()
-{
-    return state_block_vec_;
-}
-
-inline StateBlockPtr FrameBase::getP() const
-{
-    return state_block_vec_[0];
-}
-inline void FrameBase::setP(const StateBlockPtr _p_ptr)
-{
-    state_block_vec_[0] = _p_ptr;
-}
-
-inline StateBlockPtr FrameBase::getO() const
-{
-    return state_block_vec_[1];
-}
-inline void FrameBase::setO(const StateBlockPtr _o_ptr)
-{
-    state_block_vec_[1] = _o_ptr;
-}
+//inline const std::vector<StateBlockPtr>& FrameBase::getStateBlockVec() const
+//{
+//    return state_block_vec_;
+//}
+
+//inline std::vector<StateBlockPtr>& FrameBase::getStateBlockVec()
+//{
+//    return state_block_vec_;
+//}
+
+//inline StateBlockPtr FrameBase::getP() const
+//{
+//    return state_block_vec_[0];
+//}
+//inline void FrameBase::setP(const StateBlockPtr _p_ptr)
+//{
+//    state_block_vec_[0] = _p_ptr;
+//}
+
+//inline StateBlockPtr FrameBase::getO() const
+//{
+//    return state_block_vec_[1];
+//}
+//inline void FrameBase::setO(const StateBlockPtr _o_ptr)
+//{
+//    state_block_vec_[1] = _o_ptr;
+//}
 
 inline StateBlockPtr FrameBase::getV() const
 {
-    return state_block_vec_[2];
+    return getStateBlock("V");
 }
 
 inline void FrameBase::setV(const StateBlockPtr _v_ptr)
 {
-    state_block_vec_[2] = _v_ptr;
+    setStateBlock("V", _v_ptr);
 }
 
-inline StateBlockPtr FrameBase::getStateBlock(unsigned int _i) const
-{
-    assert (_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
-    return state_block_vec_[_i];
-}
-
-inline void FrameBase::setStateBlock(unsigned int _i, const StateBlockPtr _sb_ptr)
-{
-    assert (_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
-    state_block_vec_[_i] = _sb_ptr;
-}
+//inline StateBlockPtr FrameBase::getStateBlock(unsigned int _i) const
+//{
+//    assert (_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
+//    return state_block_vec_[_i];
+//}
+//
+//inline void FrameBase::setStateBlock(unsigned int _i, const StateBlockPtr _sb_ptr)
+//{
+//    assert (_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
+//    state_block_vec_[_i] = _sb_ptr;
+//}
 
 inline TrajectoryBasePtr FrameBase::getTrajectory() const
 {
@@ -270,11 +272,11 @@ inline const CaptureBasePtrList& FrameBase::getCaptureList() const
     return capture_list_;
 }
 
-inline void FrameBase::resizeStateBlockVec(unsigned int _size)
-{
-    if (_size > state_block_vec_.size())
-        state_block_vec_.resize(_size);
-}
+//inline void FrameBase::resizeStateBlockVec(unsigned int _size)
+//{
+//    if (_size > state_block_vec_.size())
+//        state_block_vec_.resize(_size);
+//}
 
 inline unsigned int FrameBase::getHits() const
 {
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index 19fa2e37da4f9897e1310efa5923b99e28cc8919..c84d345a96d6856e3911f145209c0db0bd8a33eb 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -29,14 +29,14 @@ class HasStateBlocks
         // Some typical shortcuts -- not all should be coded here, see notes below.
         StateBlockPtr getP() const;
         StateBlockPtr getO() const;
-        StateBlockPtr getV() const; // This probably not here but in derived classes only
-        StateBlockPtr getW() const; // This probably not here but in derived classes only
-        StateBlockPtr getIntrinsics() const; // This probably not here but in derived classes only
+//        StateBlockPtr getV() const; // This probably not here but in derived classes only
+//        StateBlockPtr getW() const; // This probably not here but in derived classes only
+//        StateBlockPtr getIntrinsics() const; // This probably not here but in derived classes only
         void setP(const StateBlockPtr _p_ptr);
         void setO(const StateBlockPtr _o_ptr);
-        void setV(const StateBlockPtr _v_ptr); // This probably not here but in derived classes only
-        void setW(const StateBlockPtr _w_ptr); // This probably not here but in derived classes only
-        void setIntrinsics(const StateBlockPtr _i_ptr); // This probably not here but in derived classes only
+//        void setV(const StateBlockPtr _v_ptr); // This probably not here but in derived classes only
+//        void setW(const StateBlockPtr _w_ptr); // This probably not here but in derived classes only
+//        void setIntrinsics(const StateBlockPtr _i_ptr); // This probably not here but in derived classes only
 
     public:
         // These act on all state blocks. Per-block action must be done through state_block.fix() or through extended API in derived classes of this.
@@ -47,6 +47,10 @@ class HasStateBlocks
     protected:
         StateBlockPtr setStateBlock(const std::string& _sb_type, const StateBlockPtr& _sb);
         StateBlockPtr getStateBlock(const std::string& _sb_type) const;
+        unsigned int removeStateBlock(const std::string& _sb_type)
+        {
+            return state_block_map_.erase(_sb_type);
+        }
 
         // Emplace derived state blocks (angle, quaternion, etc).
         template<typename SB, typename ... Args>
@@ -100,7 +104,7 @@ inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const std::string&
 }
 
 template<typename ... Args>
-inline StateBlockPtr HasStateBlocks::emplaceStateBlock<StateBlock>(const std::string& _sb_type, Args&&... _args_of_base_state_block_constructor)
+inline StateBlockPtr HasStateBlocks::emplaceStateBlock(const std::string& _sb_type, Args&&... _args_of_base_state_block_constructor)
 {
     assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type!");
     std::shared_ptr<StateBlock> sb = std::make_shared<StateBlock>(std::forward<Args>(_args_of_base_state_block_constructor)...);
@@ -110,8 +114,9 @@ inline StateBlockPtr HasStateBlocks::emplaceStateBlock<StateBlock>(const std::st
 
 inline wolf::StateBlockPtr HasStateBlocks::getStateBlock(const std::string& _sb_type) const
 {
-    if (state_block_map_.count(_sb_type))
-        return state_block_map_.at(_sb_type);
+    auto it = state_block_map_.find(_sb_type);
+    if (it != state_block_map_.end())
+        return it->second;
     else
         return nullptr;
 }
@@ -126,20 +131,20 @@ inline wolf::StateBlockPtr HasStateBlocks::getO() const
     return getStateBlock("O");
 }
 
-inline wolf::StateBlockPtr HasStateBlocks::getV() const
-{
-    return getStateBlock("V");
-}
-
-inline wolf::StateBlockPtr HasStateBlocks::getW() const
-{
-    return getStateBlock("W");
-}
-
-inline wolf::StateBlockPtr HasStateBlocks::getIntrinsics() const
-{
-    return getStateBlock("I");
-}
+//inline wolf::StateBlockPtr HasStateBlocks::getV() const
+//{
+//    return getStateBlock("V");
+//}
+//
+//inline wolf::StateBlockPtr HasStateBlocks::getW() const
+//{
+//    return getStateBlock("W");
+//}
+//
+//inline wolf::StateBlockPtr HasStateBlocks::getIntrinsics() const
+//{
+//    return getStateBlock("I");
+//}
 
 inline void HasStateBlocks::setP(const StateBlockPtr _p_ptr)
 {
@@ -151,42 +156,42 @@ inline void HasStateBlocks::setO(const StateBlockPtr _o_ptr)
     setStateBlock("O", _o_ptr);
 }
 
-inline void HasStateBlocks::setV(const StateBlockPtr _v_ptr)
-{
-    setStateBlock("V", _v_ptr);
-}
-
-inline void HasStateBlocks::setW(const StateBlockPtr _w_ptr)
-{
-    setStateBlock("W", _w_ptr);
-}
-
-inline void HasStateBlocks::setIntrinsics(const StateBlockPtr _i_ptr)
-{
-    setStateBlock("I", _i_ptr);
-}
+//inline void HasStateBlocks::setV(const StateBlockPtr _v_ptr)
+//{
+//    setStateBlock("V", _v_ptr);
+//}
+//
+//inline void HasStateBlocks::setW(const StateBlockPtr _w_ptr)
+//{
+//    setStateBlock("W", _w_ptr);
+//}
+//
+//inline void HasStateBlocks::setIntrinsics(const StateBlockPtr _i_ptr)
+//{
+//    setStateBlock("I", _i_ptr);
+//}
 
 inline void HasStateBlocks::fix()
 {
-    for (auto sbp : state_block_map_)
-        if (sbp.second != nullptr)
-            sbp.second->fix();
+    for (auto pair_key_sbp : state_block_map_)
+        if (pair_key_sbp.second != nullptr)
+            pair_key_sbp.second->fix();
 }
 
 inline void HasStateBlocks::unfix()
 {
-    for (auto sbp : state_block_map_)
-        if (sbp != nullptr)
-            sbp->unfix();
+    for (auto pair_key_sbp : state_block_map_)
+        if (pair_key_sbp.second != nullptr)
+            pair_key_sbp.second->unfix();
 }
 
 inline bool HasStateBlocks::isFixed() const
 {
     bool fixed = true;
-    for (auto sb : state_block_map_)
+    for (auto pair_key_sbp : state_block_map_)
     {
-        if (sb.second)
-            fixed &= sb.second->isFixed();
+        if (pair_key_sbp.second)
+            fixed &= pair_key_sbp.second->isFixed();
     }
     return fixed;
 }
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index 7d56eefa61707d3aa78d251c893bf37fef689c46..5e11a2d4f5bda019bd7cf96698371d5812d61f00 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -90,9 +90,11 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks
             //frame state blocks
             for(auto fr_ptr : wolf_problem_->getTrajectory()->getFrameList())
                 if (fr_ptr->isKeyOrAux())
-                    for (auto sb : fr_ptr->getStateBlockVec())
-                        if (sb)
-                            all_state_blocks.push_back(sb);
+                    for (const auto& pair_key_sb : fr_ptr->getStateBlockMap())
+                    {
+                        const auto& sb = pair_key_sb.second;
+                        all_state_blocks.push_back(sb);
+                    }
 
             // landmark state blocks
             for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
@@ -115,16 +117,16 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks
             // first create a vector containing all state blocks
             for(auto fr_ptr : wolf_problem_->getTrajectory()->getFrameList())
                 if (fr_ptr->isKeyOrAux())
-                    for (auto sb : fr_ptr->getStateBlockVec())
-                        if (sb)
-                            for(auto sb2 : fr_ptr->getStateBlockVec())
-                                if (sb2)
-                                {
-                                    state_block_pairs.emplace_back(sb, sb2);
-                                    double_pairs.emplace_back(getAssociatedMemBlockPtr(sb), getAssociatedMemBlockPtr(sb2));
-                                    if (sb == sb2)
-                                        break;
-                                }
+                    for (auto pair_key_sb : fr_ptr->getStateBlockMap())
+                        for(auto pair_key_sb2 : fr_ptr->getStateBlockMap())
+                        {
+                            const auto& sb  = pair_key_sb.second;
+                            const auto& sb2 = pair_key_sb2.second;
+                            state_block_pairs.emplace_back(sb, sb2);
+                            double_pairs.emplace_back(getAssociatedMemBlockPtr(sb), getAssociatedMemBlockPtr(sb2));
+                            if (sb == sb2)
+                                break;
+                        }
 
             // landmark state blocks
             for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index d4202bc490276d0bc8fc858ca6ec54645948619b..3269ff36bba7009ab85a954a4e78f7e92f17764b 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -14,33 +14,45 @@ unsigned int FrameBase::frame_id_count_ = 0;
 FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) :
             NodeBase("FRAME", "Base"),
             trajectory_ptr_(),
-            state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
+//            state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
             frame_id_(++frame_id_count_),
             type_(NON_ESTIMATED),
             time_stamp_(_ts)
 {
-    state_block_vec_[0] = _p_ptr;
-    state_block_vec_[1] = _o_ptr;
-    state_block_vec_[2] = _v_ptr;
+    if (_p_ptr)
+        setStateBlock("P", _p_ptr);
+    if (_o_ptr)
+        setStateBlock("O", _o_ptr);
+    if (_v_ptr)
+        setStateBlock("V", _v_ptr);
+//    state_block_vec_[0] = _p_ptr;
+//    state_block_vec_[1] = _o_ptr;
+//    state_block_vec_[2] = _v_ptr;
 }
 
 FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) :
             NodeBase("FRAME", "Base"),
             trajectory_ptr_(),
-            state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
+//            state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
             frame_id_(++frame_id_count_),
             type_(_tp),
             time_stamp_(_ts)
 {
-    state_block_vec_[0] = _p_ptr;
-    state_block_vec_[1] = _o_ptr;
-    state_block_vec_[2] = _v_ptr;
+    if (_p_ptr)
+        setStateBlock("P", _p_ptr);
+    if (_o_ptr)
+        setStateBlock("O", _o_ptr);
+    if (_v_ptr)
+        setStateBlock("V", _v_ptr);
+//    state_block_vec_[0] = _p_ptr;
+//    state_block_vec_[1] = _o_ptr;
+//    state_block_vec_[2] = _v_ptr;
 }
 
 FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXs& _x) :
            NodeBase("FRAME", "Base"),
            trajectory_ptr_(),
-           state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
+//           state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
            frame_id_(++frame_id_count_),
            type_(_tp),
            time_stamp_(_ts)
@@ -50,20 +62,22 @@ FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, c
         assert(_x.size() == 3 && "Wrong state vector size. Should be 3 for 2D!");
         StateBlockPtr p_ptr ( std::make_shared<StateBlock>    (_x.head    <2> ( ) ) );
         StateBlockPtr o_ptr ( std::make_shared<StateAngle>    ((Scalar) _x(2) ) );
-        StateBlockPtr v_ptr ( nullptr );
-        state_block_vec_[0] = p_ptr;
-        state_block_vec_[1] = o_ptr;
-        state_block_vec_[2] = v_ptr;
+        setStateBlock("P", p_ptr);
+        setStateBlock("O", o_ptr);
+//        state_block_vec_[0] = p_ptr;
+//        state_block_vec_[1] = o_ptr;
+//        state_block_vec_[2] = v_ptr;
         this->setType("PO 2D");
     }else if(_frame_structure.compare("PO") == 0 and _dim == 3){
         // auto _x = Eigen::VectorXs::Zero(7);
         assert(_x.size() == 7 && "Wrong state vector size. Should be 7 for 3D!");
         StateBlockPtr p_ptr ( std::make_shared<StateBlock>      (_x.head    <3> ( ) ) );
         StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.tail    <4> ( ) ) );
-        StateBlockPtr v_ptr ( nullptr );
-        state_block_vec_[0] = p_ptr;
-        state_block_vec_[1] = o_ptr;
-        state_block_vec_[2] = v_ptr;
+        setStateBlock("P", p_ptr);
+        setStateBlock("O", o_ptr);
+//        state_block_vec_[0] = p_ptr;
+//        state_block_vec_[1] = o_ptr;
+//        state_block_vec_[2] = v_ptr;
         this->setType("PO 3D");
     }else if(_frame_structure.compare("POV") == 0 and _dim == 3){
         // auto _x = Eigen::VectorXs::Zero(10);
@@ -71,15 +85,19 @@ FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, c
         StateBlockPtr p_ptr ( std::make_shared<StateBlock>      (_x.head    <3> ( ) ) );
         StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.segment <4> (3) ) );
         StateBlockPtr v_ptr ( std::make_shared<StateBlock>      (_x.tail    <3> ( ) ) );
-        state_block_vec_[0] = p_ptr;
-        state_block_vec_[1] = o_ptr;
-        state_block_vec_[2] = v_ptr;
+        setStateBlock("P", p_ptr);
+        setStateBlock("O", o_ptr);
+        setStateBlock("V", v_ptr);
+//        state_block_vec_[0] = p_ptr;
+//        state_block_vec_[1] = o_ptr;
+//        state_block_vec_[2] = v_ptr;
         this->setType("POV 3D");
     }else{
         std::cout << _frame_structure << " ^^ " << _dim << std::endl;
         throw std::runtime_error("Unknown frame structure");
     }
 
+
 }
 
 FrameBase::~FrameBase()
@@ -129,26 +147,41 @@ void FrameBase::registerNewStateBlocks()
 {
     if (getProblem() != nullptr)
     {
-        for (StateBlockPtr sbp : getStateBlockVec())
-            if (sbp != nullptr)
-                getProblem()->notifyStateBlock(sbp,ADD);
+        for (auto pair_key_sbp : getStateBlockMap())
+            if (pair_key_sbp.second != nullptr)
+                getProblem()->notifyStateBlock(pair_key_sbp.second,ADD);
     }
 }
 
 void FrameBase::removeStateBlocks()
 {
-    for (unsigned int i = 0; i < state_block_vec_.size(); i++)
+    for (auto pair_key_sb : getStateBlockMap())
     {
-        StateBlockPtr sbp = getStateBlock(i);
+        auto& key = pair_key_sb.first;
+        auto& sbp = pair_key_sb.second;
         if (sbp != nullptr)
         {
             if (getProblem() != nullptr)
             {
                 getProblem()->notifyStateBlock(sbp,REMOVE);
             }
-            setStateBlock(i, nullptr);
+            removeStateBlock(key);
         }
     }
+
+
+//    for (unsigned int i = 0; i < state_block_map_.size(); i++)
+//    {
+//        StateBlockPtr sbp = getStateBlock(i);
+//        if (sbp != nullptr)
+//        {
+//            if (getProblem() != nullptr)
+//            {
+//                getProblem()->notifyStateBlock(sbp,REMOVE);
+//            }
+//            setStateBlock(i, nullptr);
+//        }
+//    }
 }
 
 void FrameBase::setNonEstimated()
@@ -194,37 +227,39 @@ void FrameBase::setAux()
     }
 }
 
-void FrameBase::fix()
-{
-    for (auto sbp : state_block_vec_)
-        if (sbp != nullptr)
-            sbp->fix();
-}
-
-void FrameBase::unfix()
-{
-    for (auto sbp : state_block_vec_)
-        if (sbp != nullptr)
-            sbp->unfix();
-}
-
-bool FrameBase::isFixed() const
-{
-    bool fixed = true;
-    for (auto sb : getStateBlockVec())
-    {
-        if (sb)
-            fixed &= sb->isFixed();
-    }
-    return fixed;
-}
+//void FrameBase::fix()
+//{
+//    for (auto sbp : state_block_vec_)
+//        if (sbp != nullptr)
+//            sbp->fix();
+//}
+//
+//void FrameBase::unfix()
+//{
+//    for (auto sbp : state_block_vec_)
+//        if (sbp != nullptr)
+//            sbp->unfix();
+//}
+//
+//bool FrameBase::isFixed() const
+//{
+//    bool fixed = true;
+//    for (auto sb : getStateBlockVec())
+//    {
+//        if (sb)
+//            fixed &= sb->isFixed();
+//    }
+//    return fixed;
+//}
 
 void FrameBase::setState(const Eigen::VectorXs& _state)
 {
-    int size = 0;
-    for(unsigned int i = 0; i<state_block_vec_.size(); i++){
-        size += (state_block_vec_[i]==nullptr ? 0 : state_block_vec_[i]->getSize());
-    }
+    int size = getSize();
+//    for (const auto & pair_key_sb : getStateBlockMap())
+//        size += pair_key_sb.second->getSize();
+//    for(unsigned int i = 0; i<state_block_vec_.size(); i++){
+//        size += (state_block_vec_[i]==nullptr ? 0 : state_block_vec_[i]->getSize());
+//    }
 
     //State Vector size must be lower or equal to frame state size :
     // example : PQVBB -> if we initialize only position and orientation due to use of processorOdom3D
@@ -234,11 +269,11 @@ void FrameBase::setState(const Eigen::VectorXs& _state)
     const unsigned int _st_size = _state.size();
 
     //initialize the FrameBase StateBlocks while StateBlocks list's end not reached and input state_size can go further
-    for (StateBlockPtr sb : state_block_vec_)
-        if (sb && (index < _st_size))
+    for (auto& pair_key_sb : getStateBlockMap())
+        if (index < _st_size)
         {
-            sb->setState(_state.segment(index, sb->getSize()), isKeyOrAux()); // do not notify if state block is not estimated by the solver
-            index += sb->getSize();
+            pair_key_sb.second->setState(_state.segment(index, pair_key_sb.second->getSize()), isKeyOrAux()); // do not notify if state block is not estimated by the solver
+            index += pair_key_sb.second->getSize();
         }
 }
 
@@ -253,28 +288,21 @@ Eigen::VectorXs FrameBase::getState() const
 
 void FrameBase::getState(Eigen::VectorXs& _state) const
 {
-    SizeEigen size = 0;
-    for (StateBlockPtr sb : state_block_vec_)
-        if (sb)
-            size += sb->getSize();
-
-    _state = Eigen::VectorXs(size);
+    _state.resize(getSize());
 
     SizeEigen index = 0;
-    for (StateBlockPtr sb : state_block_vec_)
-        if (sb)
-        {
-            _state.segment(index,sb->getSize()) = sb->getState();
-            index += sb->getSize();
-        }
+    for (const auto& pair_key_sb : getStateBlockMap())
+    {
+        _state.segment(index,pair_key_sb.second->getSize()) = pair_key_sb.second->getState();
+        index += pair_key_sb.second->getSize();
+    }
 }
 
 unsigned int FrameBase::getSize() const
 {
     unsigned int size = 0;
-    for (auto st : state_block_vec_)
-        if (st)
-            size += st->getSize();
+    for (const auto& pair_key_sb : getStateBlockMap())
+            size += pair_key_sb.second->getSize();
     return size;
 }
 
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index c4b9677728e442846d422071719e4ec22b3c37d7..364808ae965963ca2e9d547f55bbab8113dda235 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -630,33 +630,29 @@ bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs&
     bool success(true);
     int i = 0, j = 0;
 
-    const auto& state_bloc_vec = _frame_ptr->getStateBlockVec();
+    const auto& state_bloc_map = _frame_ptr->getStateBlockMap();
 
     // computing size
     SizeEigen sz = 0;
-    for (const auto& sb : state_bloc_vec)
-        if (sb)
-            sz += sb->getLocalSize();
+    for (const auto& pair_key_sb : state_bloc_map)
+        sz += pair_key_sb.second->getLocalSize();
 
     // resizing
     _covariance = Eigen::MatrixXs(sz, sz);
 
     // filling covariance
-    for (const auto& sb_i : state_bloc_vec)
+    for (const auto& pair_key_sb_i : state_bloc_map)
     {
-        if (sb_i)
+        const auto& sb_i = pair_key_sb_i.second;
+        j = 0;
+        for (const auto& pair_key_sb_j : state_bloc_map)
         {
-            j = 0;
-            for (const auto& sb_j : state_bloc_vec)
-            {
-                if (sb_j)
-                {
-                    success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j);
-                    j += sb_j->getLocalSize();
-                }
-            }
-            i += sb_i->getLocalSize();
+            const auto& sb_j = pair_key_sb_j.second;
+            success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j);
+            j += sb_j->getLocalSize();
         }
+        i += sb_i->getLocalSize();
+
     }
     return success;
 }
@@ -916,9 +912,11 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) c
             if (state_blocks)
             {
                 cout << "    sb:";
-                for (auto sb : F->getStateBlockVec())
-                    if (sb != nullptr)
-                        cout << " " << (sb->isFixed() ? "Fix" : "Est");
+                for (const auto& pair_key_sb : F->getStateBlockMap())
+                {
+                    const auto& sb = pair_key_sb.second;
+                    cout << " " << (sb->isFixed() ? "Fix" : "Est");
+                }
                 cout << endl;
             }
             if (depth >= 2)
@@ -1141,8 +1139,9 @@ bool Problem::check(int verbose_level) const
             cout << (F->isKeyOrAux() ? (F->isKey() ? "  KF" : " EF") : "  F") << F->id() << " @ " << F.get() << endl;
             cout << "    -> P @ " << F->getProblem().get() << endl;
             cout << "    -> T @ " << F->getTrajectory().get() << endl;
-            for (auto sb : F->getStateBlockVec())
+            for (const auto& pair_key_sb : F->getStateBlockMap())
             {
+                const auto& sb = pair_key_sb.second;
                 cout <<  "    sb @ " << sb.get();
                 if (sb)
                 {
@@ -1344,7 +1343,7 @@ bool Problem::check(int verbose_level) const
                             }
                         }
                         // find in own Frame
-                        found = found || (std::find(F->getStateBlockVec().begin(), F->getStateBlockVec().end(), sb) != F->getStateBlockVec().end());
+                        found = found || (std::find_if(F->getStateBlockMap().begin(), F->getStateBlockMap().end(), [sb](const std::pair<std::string, StateBlockPtr> & t)->bool {return t.second == sb;}) != F->getStateBlockMap().end());
                         // find in own Capture
                         found = found || (std::find(C->getStateBlockVec().begin(), C->getStateBlockVec().end(), sb) != C->getStateBlockVec().end());
                         // find in own Sensor
@@ -1352,7 +1351,7 @@ bool Problem::check(int verbose_level) const
                             found = found || (std::find(S->getStateBlockVec().begin(), S->getStateBlockVec().end(), sb) != S->getStateBlockVec().end());
                         // find in constrained Frame
                         if (Fo)
-                            found = found || (std::find(Fo->getStateBlockVec().begin(), Fo->getStateBlockVec().end(), sb) != Fo->getStateBlockVec().end());
+                            found = found || (std::find_if(Fo->getStateBlockMap().begin(), Fo->getStateBlockMap().end(), [sb](const std::pair<std::string, StateBlockPtr> & t)->bool {return t.second == sb;}) != Fo->getStateBlockMap().end());
                         // find in constrained Capture
                         if (Co)
                             found = found || (std::find(Co->getStateBlockVec().begin(), Co->getStateBlockVec().end(), sb) != Co->getStateBlockVec().end());
@@ -1361,7 +1360,7 @@ bool Problem::check(int verbose_level) const
                         {
                             // find in constrained feature's Frame
                             FrameBasePtr foF = fo->getFrame();
-                            found = found || (std::find(foF->getStateBlockVec().begin(), foF->getStateBlockVec().end(), sb) != foF->getStateBlockVec().end());
+                            found = found || (std::find_if(foF->getStateBlockMap().begin(), foF->getStateBlockMap().end(), [sb](const std::pair<std::string, StateBlockPtr> & t)->bool {return t.second == sb;}) != foF->getStateBlockMap().end());
                             // find in constrained feature's Capture
                             CaptureBasePtr foC = fo->getCapture();
                             found = found || (std::find(foC->getStateBlockVec().begin(), foC->getStateBlockVec().end(), sb) != foC->getStateBlockVec().end());
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index 03f991c07c4a144f560a3759c4c081cdd2410ed1..639715e0754a5bc7d9167b2619a85cc1ea43d281 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -39,7 +39,7 @@ TEST(FrameBase, StateBlocks)
 {
     FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
 
-    ASSERT_EQ(F->getStateBlockVec().size(),   (unsigned int) 3);
+    ASSERT_EQ(F->getStateBlockMap().size(),(unsigned int) 2);
     ASSERT_EQ(F->getP()->getState().size(),(unsigned int) 2);
     ASSERT_EQ(F->getO()->getState().size(),(unsigned int) 1);
     ASSERT_EQ(F->getV(), nullptr);
@@ -142,7 +142,7 @@ TEST(FrameBase, GetSetState)
 
     // Set the state, check that state blocks hold the current states
     F.setState(x);
-    ASSERT_TRUE((p - F.getP()->getState()).isMuchSmallerThan(1, Constants::EPS_SMALL));
+    ASSERT_MATRIX_APPROX(p,  F.getP()->getState(), Constants::EPS_SMALL);
     ASSERT_TRUE((q - F.getO()->getState()).isMuchSmallerThan(1, Constants::EPS_SMALL));
     ASSERT_TRUE((v - F.getV()->getState()).isMuchSmallerThan(1, Constants::EPS_SMALL));