dealing with nullptr origin

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();
 }
 

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

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);