Several fixes to adapt the state size at different places

include/core/processor/processor_motion.h

@@ -509,7 +509,7 @@ inline bool ProcessorMotion::voteForKeyFrame() const
 
 inline Eigen::VectorXd ProcessorMotion::getState(const TimeStamp& _ts) const
 {
-    Eigen::VectorXd x(getProblem()->getFrameStructureSize());  // TODO -> wrong
+    Eigen::VectorXd x(x_size_);
     getState(_ts, x);
     return x;
 }

src/problem/problem.cpp

@@ -353,16 +353,21 @@ void Problem::getCurrentStateAndStamp(Eigen::VectorXd& _state, TimeStamp& _ts) c
 }
 
 
+// Problem of this implmentation: if more state
 void Problem::getState(const TimeStamp& _ts, Eigen::VectorXd& _state) const
 {
     // if _ts is too recent, for some of the processor is motion, they return the state corresponding to their last frame timestamp
+    FrameBasePtr closest_frame = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
+    Eigen::VectorXd closest_frame_state = closest_frame->getState();
     if (processor_is_motion_list_.empty()){
-        FrameBasePtr closest_frame = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
         if (closest_frame != nullptr)
-            closest_frame->getState(_state);
+            _state = closest_frame_state;
         else
             _state = zeroState();
     }
+
+    // RETRIEVE FROM PROCESSOR MOTION
+    // TODO: current implementation really messy, would be much easier with a state being an std::unordered_map
     else {
         // Iterate over the problem state structure and get the corresponding state
         // in the first processor is motion that provides it
@@ -395,21 +400,32 @@ 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;
+        _state.resize(state_size_);
         for (char sb_name: frame_structure_){
+            Eigen::VectorXd sb_state;
+            int size_sb;  // really bad...
             if (sb_name == 'O'){
-                int size_sb = dim_ == 3 ? 4 : 1;  // really bad...
-                _state.segment(idx, size_sb) = states_to_concat_map[sb_name];
-                idx += size_sb;
+                size_sb = dim_ == 3 ? 4 : 1;
+            }
+            else {
+                size_sb = dim_ == 3 ? 3 : 2;
+            }
+            if (states_to_concat_map.find(sb_name) != states_to_concat_map.end()){
+                sb_state = states_to_concat_map[sb_name];
             }
             else {
-                int size_sb = dim_ == 3 ? 3 : 2;
-                _state.segment(idx, size_sb) = states_to_concat_map[sb_name];
-                idx += size_sb;
+                // Should be taken from the last state but too messy already
+                sb_state.resize(size_sb); 
+                sb_state.setZero();
             }
+
+            _state.segment(idx, size_sb) = sb_state;
+            idx += size_sb;
+
         }
     }
 }
@@ -847,10 +863,16 @@ FrameBasePtr Problem::setPrior(const Eigen::VectorXd& _prior_state, const Eigen:
         // create origin capture with the given state as data
         // Capture fix only takes 3D position and Quaternion orientation
         CapturePosePtr init_capture;
-        if (this->getFrameStructure() == "POV" and this->getDim() == 3)
+        // if (this->getFrameStructure() == "POV" and this->getDim() == 3)
+        //     init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state.head(7), _prior_cov.topLeftCorner(6,6));
+        // else
+        //     init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state, _prior_cov);
+
+        if (this->getDim() == 3)
             init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state.head(7), _prior_cov.topLeftCorner(6,6));
         else
-            init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state, _prior_cov);
+            init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state.head(3), _prior_cov.topLeftCorner(3,3));
+
 
         // emplace feature and factor
         init_capture->emplaceFeatureAndFactor();

src/processor/processor_motion.cpp

@@ -25,7 +25,8 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
         origin_ptr_(),
         last_ptr_(),
         incoming_ptr_(),
-        dt_(0.0), x_(_state_size),
+        dt_(0.0), 
+        x_(_state_size),
         delta_(_delta_size),
         delta_cov_(_delta_cov_size, _delta_cov_size),
         delta_integrated_(_delta_size),
@@ -347,6 +348,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
     postProcess();
 }
 
+// _x needs to have the size of the processor state
 bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const
 {
     CaptureMotionPtr capture_motion;