diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h index 3bf0b996153858d5904181832a463ec4c1854e1c..3902c1f8337d8cae0bf46d08627ef784102d1f0d 100644 --- a/include/core/processor/processor_motion.h +++ b/include/core/processor/processor_motion.h @@ -271,6 +271,10 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider bool voteForKeyFrame() const override; double updateDt(); + /** \brief Make one step of motion integration + * + * Integrate motiondata in incoming_ptr_ and store all results in the MotionBuffer in last_ptr_ + */ void integrateOneStep(); void reintegrateBuffer(CaptureMotionPtr _capture_ptr) const; void splitBuffer(const wolf::CaptureMotionPtr& capture_source, diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index 4dae381477341ee3ae1139bb0b24925ce071cf7c..3c014ca6a55e4b9416b1e39c41ed0b25d4921418 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -438,7 +438,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) * x : any capture * o : origin capture * l : last capture -> we'll make a KF here - * i : incoming capture + * i : incoming capture -> data has already been integrated into last capture * n : new capture -> * --- : buffer history * @@ -560,8 +560,6 @@ VectorComposite ProcessorMotion::getState(const StateStructure& _structure) cons auto calib_preint = last_ptr_->getCalibrationPreint(); VectorComposite state; - //WOLF_INFO("processorMotion last timestamp: ", last_ptr_->getTimeStamp()); - //WOLF_INFO("processorMotion origin timestamp: ", origin_ptr_->getTimeStamp()); if ( hasCalibration()) { // Get current calibration -- from origin capture @@ -606,7 +604,6 @@ VectorComposite ProcessorMotion::getState(const StateStructure& _structure) cons -// _x needs to have the size of the processor state VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateStructure& _structure) const { assert(_ts.ok()); @@ -759,6 +756,9 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame) void ProcessorMotion::integrateOneStep() { + /* Integrate motiondata in incoming_ptr_ and store all results in the MotionBuffer in last_ptr_ + */ + // Set dt dt_ = updateDt(); assert(dt_ >= 0 && "Time interval _dt is negative!");