Merge remote-tracking branch 'origin/devel' into...

Merge remote-tracking branch 'origin/devel' into 343-problem-getstate-structure-doesn-t-care-about-structure

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_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"))

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

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