diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h index ef155cee11d49a9e0deb6c7c884e35d5cbc95b53..6d2b81e9ed4425a693c7f669779d1f51a0118845 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_base.cpp b/src/capture/capture_base.cpp index 8b31edc53276216811daa1ca640bf042e1c4f82b..880af68b86919dbab088f7f702f07ac41feefb98 100644 --- a/src/capture/capture_base.cpp +++ b/src/capture/capture_base.cpp @@ -20,6 +20,7 @@ CaptureBase::CaptureBase(const std::string& _type, capture_id_(++capture_id_count_), time_stamp_(_ts) { + assert(time_stamp_.ok() && "Creating a capture with an invalid timestamp!"); if (_sensor_ptr) { if (_sensor_ptr->getP() != nullptr and _sensor_ptr->isStateBlockDynamic("P")) diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp index 07078eeeec957963c8942256c39931301d3c9157..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 d7cd8460ffaf9ad331b517801638b21bd25d879e..65960d878ac414308b1032f913e4a9f9e69cf243 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -111,6 +111,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(); @@ -251,6 +259,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; } @@ -423,7 +432,10 @@ VectorComposite ProcessorMotion::getState(const StateStructure& _structure) cons const StateStructure& structure = (_structure == "" ? state_structure_ : _structure); - 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"); @@ -466,7 +478,7 @@ VectorComposite ProcessorMotion::getState(const StateStructure& _structure) cons 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_); @@ -513,6 +525,8 @@ 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()); + const StateStructure& structure = (_structure == "" ? state_structure_ : _structure); // We need to search for the capture containing a motion buffer with the queried time stamp @@ -775,6 +789,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 @@ -884,7 +900,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);