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

In the current implementation of:

1. Problem::getCurrentState(Eigen::VectorXs& state)
2. Problem::getCurrentStateAndStamp(Eigen::VectorXs& state, TimeStamp& ts)
3. ProcessorMotion::getCurrentState(Eigen::VectorXs& x) Assume the provided state to be of the proper size but there is no assertion to check that.

The case 2 just calls 1. Case 1 calls 3 if any processor, otherwise state is overwritten with the proper size. I think this shouldn't happen. There are some places where the resize of the provided state could be done.

A. In Problem::getCurrentState(Eigen::VectorXs& state): Before calling processor_motion_->getCurrentState(state), we could resize the state with:
state = Eigen::VectorXs(getFrameStructureSize());

B. In ProcessorMotion::getCurrentState(Eigen::VectorXs& x): Resizing x at the beginning of the function would avoid a potential assertion failure when calling the derived class function:
statePlusDelta(origin_ptr_->getFrame()->getState(), last_ptr_->getDeltaCorrected(origin_ptr_->getCalibration()), Dt, _x);

C. Ask the derived processors's statePlusDelta(const Eigen::VectorXs& _x, const Eigen::VectorXs& _delta, const Scalar _dt, Eigen::VectorXs& _x_plus_delta) to deal with wrongly sized _x_plus_delta inputs.

I think C may be too much (this function may be called several times).

I propose B because I think it is the safest alternative. It only requires to add a line at the top of ProcessorMotion::getCurrentState(Eigen::VectorXs& x):
x = Eigen::VectorXs(origin_ptr_->getFrame()->getSize());
or
x = Eigen::VectorXs(getProblem()->getFrameStructureSize()); (it should deal with nullptr problem..)

BTW: The whole function assumes that origin_ptr_ is not nullptr... is this assumable? should we at least put an assertion?