Resolve "Problem::getCurrentStateAndStamp(x) requires x of the proper size"

Closes #254 (closed)

include/core/processor/processor_motion.h

@@ -515,6 +515,13 @@ inline Eigen::VectorXs ProcessorMotion::getCurrentState()
 
 inline void ProcessorMotion::getCurrentState(Eigen::VectorXs& _x)
 {
+    // ensure integrity
+    assert(origin_ptr_ && "Trying to access origin_ptr_ but it is nullptr!");
+
+    // ensure proper size of the provided reference
+    _x.resize( getProblem()->getFrameStructureSize() );
+
+    // do get timestamp and state corrected by possible self-calibrations
     Scalar Dt = getCurrentTimeStamp() - origin_ptr_->getTimeStamp();
     statePlusDelta(origin_ptr_->getFrame()->getState(), last_ptr_->getDeltaCorrected(origin_ptr_->getCalibration()), Dt, _x);
 }