diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index 08cd35ab851a5f683c3350fa4045acf94a0ea850..9a0cbb90d8284e1fb88a86fa2dd821d0d839b208 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -374,7 +374,7 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const VectorXd delta = capture_motion->correctDelta( motion.delta_integr_, delta_step); // ensure proper size of the provided reference - _x.resize( getProblem()->getFrameStructureSize() ); + _x.resize( state_0.size() ); // Compose on top of origin state using the buffered time stamp, not the query time stamp double dt = motion.ts_ - capture_motion->getBuffer().get().front().ts_; @@ -383,7 +383,7 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const else { // We could not find any CaptureMotion for the time stamp requested - WOLF_ERROR("Could not find any Capture for the time stamp requested. "); + WOLF_ERROR("Could not find any Capture for the time stamp requested. ", _ts); WOLF_TRACE("Did you forget to call Problem::setPrior() in your application?") return false; } @@ -567,7 +567,8 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp ++frame_rev_iter) { frame = *frame_rev_iter; - capture = frame->getCaptureOf(getSensor()); + auto sensor = getSensor(); + capture = frame->getCaptureOf(sensor); if (capture != nullptr) { // Rule 1 satisfied! We found a Capture belonging to this processor's Sensor ==> it is a CaptureMotion