diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index 84e7bad959378dff89025e915b8606b0390e6ef5..f7f39a673eda53a8f00aca93d52e4d2b8e916c64 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -448,10 +448,9 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts) const CaptureMotionPtr capture_motion = findCaptureContainingTimeStamp(_ts); - assert( capture_motion && "There is no motion data at the queried time stamp"); -// if (capture_motion) // We found a CaptureMotion whose buffer contains the time stamp -// { + if (capture_motion) // We found a CaptureMotion whose buffer contains the time stamp + { // if buffer is empty --> we did not advance from origin! if (capture_motion->getBuffer().empty()) { @@ -511,53 +510,9 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts) const // return success return state; -// } -// else -// return false; - -/* - CaptureMotionPtr capture_motion; - if (origin_ptr_ && _ts >= origin_ptr_->getTimeStamp()) - { - // timestamp found in the current processor buffer - capture_motion = last_ptr_; - } - else - { - // We need to search in previous keyframes for the capture containing a motion buffer with the queried time stamp - capture_motion = findCaptureContainingTimeStamp(_ts); - } - - if (capture_motion) // We found a CaptureMotion whose buffer contains the time stamp - { - // Get origin state and calibration - CaptureBasePtr cap_orig = capture_motion->getOriginCapture(); - const auto& state_0 = cap_orig->getFrame()->getState(state_structure_); - VectorXd calib = cap_orig->getCalibration(); - - // Get delta and correct it with new calibration params - VectorXd calib_preint = capture_motion->getCalibrationPreint(); - Motion motion = capture_motion->getBuffer().getMotion(_ts); - - VectorXd delta_step = motion.jacobian_calib_ * (calib - calib_preint); - VectorXd delta = capture_motion->correctDelta( motion.delta_integr_, delta_step); - - // ensure proper size of the provided reference - _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().front().ts_; - statePlusDelta(state_0, delta, dt, _x); } else - { - // We could not find any CaptureMotion 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; - } - return true; - */ + return VectorComposite(); // return empty state } FrameBasePtr ProcessorMotion::setOrigin(const VectorComposite& _x_origin, const TimeStamp& _ts_origin)