Skip to content
Snippets Groups Projects
Commit 60ca451e authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Resize provided vector ref to appropriate size

also assert origin_ptr_
parent f064b9bf
No related branches found
No related tags found
1 merge request!321Resolve "Problem::getCurrentStateAndStamp(x) requires x of the proper size"
Pipeline #4314 passed
......@@ -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);
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment