diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 95d300f9729db24c49382d522d159e98f84ff95d..02afc83ee6f21e8569edabd78d65f455bd696b6c 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -375,15 +375,17 @@ void Problem::getState(const TimeStamp& _ts, Eigen::VectorXd& _state) const
 
             int idx = 0;
             for (char sb_name: proc->getStateStructure()){
-                if (states_to_concat_map.find(sb_name) != states_to_concat_map.end()){
-                    // not already there
+                // not already there
+                if (states_to_concat_map.find(sb_name) == states_to_concat_map.end()){
                     if (sb_name == 'O'){
-                        states_to_concat_map[sb_name] = proc_state.segment<4>(idx);
-                        idx += 4;
+                        int size_sb = dim_ == 3 ? 4 : 1;  // really bad...
+                        states_to_concat_map[sb_name] = proc_state.segment(idx, size_sb);
+                        idx += size_sb;
                     }
                     else{
-                        states_to_concat_map[sb_name] = proc_state.segment<3>(idx);
-                        idx += 3;
+                        int size_sb = dim_ == 3 ? 3 : 2;
+                        states_to_concat_map[sb_name] = proc_state.segment(idx, size_sb);
+                        idx += size_sb;
                     } 
                 }
             }
@@ -393,18 +395,20 @@ void Problem::getState(const TimeStamp& _ts, Eigen::VectorXd& _state) const
         for (auto state_map_it: states_to_concat_map){
             concat_size += state_map_it.second.size();
         }
-        assert(concat_size == state_size_  && "Problem with the concatenated size");
+        assert(concat_size == state_size_  && "Problem with the concatenated size: " );
 
         // fill the state value from the state concatenation in the order dictated by frame_structure_
         int idx = 0;
         for (char sb_name: frame_structure_){
             if (sb_name == 'O'){
-                _state.segment<4>(idx) = states_to_concat_map[sb_name];
-                idx += 4;
+                int size_sb = dim_ == 3 ? 4 : 1;  // really bad...
+                _state.segment(idx, size_sb) = states_to_concat_map[sb_name];
+                idx += size_sb;
             }
             else {
-                _state.segment<3>(idx) = states_to_concat_map[sb_name];
-                idx += 3;
+                int size_sb = dim_ == 3 ? 3 : 2;
+                _state.segment(idx, size_sb) = states_to_concat_map[sb_name];
+                idx += size_sb;
             }
         }
     }
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 4259c0b7ccfebee600f9740039452b9a1cfa2f8e..b0a601c2365172f27f4a7b4bce2d139bf910e7b2 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -415,8 +415,9 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
     // Make non-key-frame for last Capture
     TimeStamp origin_ts = _origin_frame->getTimeStamp();
     auto new_frame_ptr  = getProblem()->emplaceFrame(NON_ESTIMATED,
-                                                     getProblem()->getState(origin_ts),
+                                                      _origin_frame->getState(),
                                                      origin_ts);
+                                        
     // emplace (emtpy) last Capture
     last_ptr_ = emplaceCapture(new_frame_ptr,
                                getSensor(),