diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h index 4df3a67fb9dcf99d07b6009f11f9906f7c4c9377..80017c423035ff9bd981bea1f85eb7adf06e9a67 100644 --- a/include/core/processor/processor_motion.h +++ b/include/core/processor/processor_motion.h @@ -542,11 +542,13 @@ inline double ProcessorMotion::updateDt() inline const MotionBuffer& ProcessorMotion::getBuffer() const { + assert(last_ptr_); return last_ptr_->getBuffer(); } inline MotionBuffer& ProcessorMotion::getBuffer() { + assert(last_ptr_); return last_ptr_->getBuffer(); } diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp index 4c7fd3ffbf662f887b2ceea92d00a49241758693..da17a57af80971b21f4a69322ba0c2ade54225d4 100644 --- a/src/capture/capture_motion.cpp +++ b/src/capture/capture_motion.cpp @@ -49,6 +49,8 @@ CaptureMotion::~CaptureMotion() bool CaptureMotion::containsTimeStamp (const TimeStamp& _ts, double _time_tolerance) { + assert(_ts.ok()); + // the same capture is within tolerance if (this->time_stamp_ - _time_tolerance <= _ts && _ts <= this->time_stamp_ + _time_tolerance) return true; @@ -58,9 +60,10 @@ bool CaptureMotion::containsTimeStamp (const TimeStamp& _ts, double _time_tolera return false; // buffer encloses timestamp, if ts is: - // from : origin.tx + tt not included - // to : capture.ts + tt included - if (this->getOriginCapture()->getTimeStamp() +_time_tolerance < _ts and _ts <= this->getBuffer().back().ts_ + _time_tolerance) + // from : buffer.first.ts - tt + // to : buffer.last.ts + tt + if (_ts >= this->getBuffer().front().ts_ - _time_tolerance and + _ts <= this->getBuffer().back().ts_ + _time_tolerance) return true; // not found anywhere diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index 23dd66ba1656808066c272d65eb2569dc825560c..181ea7f662b6c6622112f88d733faafd9417bb61 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -93,6 +93,14 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) incoming_ptr_ = std::dynamic_pointer_cast<CaptureMotion>(_incoming_ptr); assert(incoming_ptr_ != nullptr && ("Capture type mismatch. Processor " + getName() + " can only process captures of type CaptureMotion").c_str()); + // if origin_ was removed reset (let it die) + if (origin_ptr_ and origin_ptr_->isRemoving()) + { + origin_ptr_ = nullptr; + if (last_ptr_) + last_ptr_->remove(); + } + preProcess(); // Derived class operations PackKeyFramePtr pack = computeProcessingStep(); @@ -233,6 +241,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) // auto new_ctr = emplaceFactor(feature_existing, capture_for_keyframe_callback); // fac_to_remove ->remove(); // remove old factor now (otherwise c->remove() gets propagated to f, C, F, etc.) } + break; } @@ -402,8 +411,10 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) VectorComposite ProcessorMotion::getState() const { - - if (last_ptr_ == nullptr or last_ptr_->getFrame() == nullptr) // We do not have any info of where to find a valid state + if (origin_ptr_ == nullptr or + origin_ptr_->isRemoving() or + last_ptr_ == nullptr or + last_ptr_->getFrame() == nullptr) // We do not have any info of where to find a valid state // Further checking here for origin_ptr is redundant: if last=null, then origin=null too. { WOLF_DEBUG("Processor has no state. Returning an empty VectorComposite with no blocks"); @@ -434,7 +445,7 @@ VectorComposite ProcessorMotion::getState() const */ // Get state of origin - const auto& x_origin = getOrigin()->getFrame()->getState(); + const auto& x_origin = origin_ptr_->getFrame()->getState(); // Get most rescent motion const auto& motion = last_ptr_->getBuffer().back(); @@ -446,7 +457,7 @@ VectorComposite ProcessorMotion::getState() const const auto& calib_preint = last_ptr_->getCalibrationPreint(); VectorComposite state; - if ( hasCalibration() ) + if ( hasCalibration()) { // Get current calibration -- from origin capture const auto& calib = getCalibration(origin_ptr_); @@ -476,6 +487,7 @@ VectorComposite ProcessorMotion::getState() const // _x needs to have the size of the processor state VectorComposite ProcessorMotion::getState(const TimeStamp& _ts) const { + assert(_ts.ok()); // We need to search for the capture containing a motion buffer with the queried time stamp CaptureMotionPtr capture_motion = findCaptureContainingTimeStamp(_ts); @@ -721,6 +733,8 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr) CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp& _ts) const { + assert(_ts.ok()); + // We need to search in previous keyframes for the capture containing a motion buffer with the queried time stamp // Note: since the buffer goes from a KF in the past until the next KF, we need to: // 1. See that the KF contains a CaptureMotion @@ -830,7 +844,9 @@ bool ProcessorMotion::storeCapture(CaptureBasePtr _cap_ptr) TimeStamp ProcessorMotion::getTimeStamp ( ) const { - if (not last_ptr_) + if (not origin_ptr_ or + origin_ptr_->isRemoving() or + not last_ptr_) { WOLF_DEBUG("Processor has no time stamp. Returning a non-valid timestamp equal to 0"); return TimeStamp(0);