Merge branch '254-problem-getcurrentstateandstamp-x-requires-x-of-the-proper-size' into 'devel'

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

Closes #254

See merge request !321

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);
 }