State machine, and checkTimeTolerance()

src/processor_tracker.cpp

@@ -15,7 +15,11 @@ namespace wolf
 {
 
 ProcessorTracker::ProcessorTracker(const std::string& _type, const unsigned int _max_new_features, const Scalar& _time_tolerance) :
-        ProcessorBase(_type, _time_tolerance), origin_ptr_(nullptr), last_ptr_(nullptr), incoming_ptr_(nullptr),
+        ProcessorBase(_type, _time_tolerance),
+        processing_step_(FIRST_TIME),
+        origin_ptr_(nullptr),
+        last_ptr_(nullptr),
+        incoming_ptr_(nullptr),
         max_new_features_(_max_new_features)
 {
     //
@@ -32,8 +36,8 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
 
     if (_incoming_ptr == nullptr)
     {
-      WOLF_ERROR("Received capture is nullptr.");
-      return;
+        WOLF_ERROR("Received capture is nullptr.");
+        return;
     }
 
     if ( !kf_pack_buffer_.empty() )
@@ -64,165 +68,173 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
 
     preProcess();
 
+    computeProcessingStep();
 
-    // FIRST TIME
-    if (origin_ptr_ == nullptr && last_ptr_ == nullptr)
+    switch (processing_step_)
     {
-        WOLF_DEBUG( "FIRST TIME" );
+        case FIRST_TIME :
+        {
+            WOLF_DEBUG( "FIRST TIME" );
 
-        // advance
-        advance();
+            // advance
+            advance();
 
-        // advance this
-        last_ptr_ = incoming_ptr_;
-        incoming_ptr_ = nullptr;
+            // advance this
+            last_ptr_ = incoming_ptr_;
+            incoming_ptr_ = nullptr;
 
-        // keyframe creation on last
-        FrameBasePtr closest_key_frm = getProblem()->getTrajectoryPtr()->closestKeyFrameToTimeStamp(last_ptr_->getTimeStamp());
-        if (closest_key_frm && abs(closest_key_frm->getTimeStamp() - last_ptr_->getTimeStamp()) <= time_tolerance_)
-        {
-            // Set KF
-            closest_key_frm->addCapture(last_ptr_);
-            closest_key_frm->setKey();
-            WOLF_DEBUG( "Last appended to existing F, set KF" , closest_key_frm->id() );
-        }
-        else
-        {
-            // Make KF
-            FrameBasePtr new_frame_ptr = getProblem()->emplaceFrame(KEY_FRAME,
-                                                                   getProblem()->getState(last_ptr_->getTimeStamp()),
-                                                                   last_ptr_->getTimeStamp());
-            new_frame_ptr->addCapture(last_ptr_); // Add incoming Capture to the new Frame
-            WOLF_DEBUG( "Last appended to new KF" , new_frame_ptr->id() );
-
-            getProblem()->keyFrameCallback(new_frame_ptr, shared_from_this(), time_tolerance_);
-        }
+            // keyframe creation on last
+            FrameBasePtr closest_key_frm = getProblem()->getTrajectoryPtr()->closestKeyFrameToTimeStamp(last_ptr_->getTimeStamp());
+            if (closest_key_frm && checkTimeTolerance(closest_key_frm, last_ptr_))
+            {
+                // Set KF
+                closest_key_frm->addCapture(last_ptr_);
+                closest_key_frm->setKey();
+                WOLF_DEBUG( "Last appended to existing F",  closest_key_frm->id(), " ==> set KF" , closest_key_frm->id() );
+            }
+            else
+            {
+                // Make KF
+                FrameBasePtr new_frame_ptr = getProblem()->emplaceFrame(KEY_FRAME,
+                                                                        getProblem()->getState(last_ptr_->getTimeStamp()),
+                                                                        last_ptr_->getTimeStamp());
+                new_frame_ptr->addCapture(last_ptr_); // Add incoming Capture to the new Frame
+                WOLF_DEBUG( "Last appended to new KF" , new_frame_ptr->id() );
+
+                getProblem()->keyFrameCallback(new_frame_ptr, shared_from_this(), time_tolerance_);
+            }
 
-        // Detect new Features, initialize Landmarks, create Constraints, ...
-        processNew(max_new_features_);
+            // Detect new Features, initialize Landmarks, create Constraints, ...
+            processNew(max_new_features_);
 
-        // Establish constraints from last
-        establishConstraints();
-    }
+            // Establish constraints from last
+            establishConstraints();
 
-    // SECOND TIME or after KEY FRAME CALLBACK
-    else if (origin_ptr_ == nullptr)
-    {
-        WOLF_DEBUG("SECOND TIME or after KEY FRAME CALLBACK");
+            break;
+        }
 
-        // First we track the known Features
-        processKnown();
 
-        // Create a new non-key Frame in the Trajectory with the incoming Capture
-        FrameBasePtr closest_key_frm = getProblem()->getTrajectoryPtr()->closestKeyFrameToTimeStamp(incoming_ptr_->getTimeStamp());
-        if (closest_key_frm && abs(closest_key_frm->getTimeStamp() - incoming_ptr_->getTimeStamp()) <= time_tolerance_)
+        case SECOND_TIME :
         {
-            // Just append the Capture to the existing keyframe
-            closest_key_frm->addCapture(incoming_ptr_);
-            WOLF_DEBUG("Incoming appended to F" , closest_key_frm->id() );
-        }
-        else
-        {
-            // Create a frame to hold what will become the last Capture
-            FrameBasePtr new_frame_ptr = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp());
-            new_frame_ptr->addCapture(incoming_ptr_); // Add incoming Capture to the new Frame
-            WOLF_DEBUG("Incoming in new F" , new_frame_ptr->id() );
-        }
+            WOLF_DEBUG("SECOND TIME or after KEY FRAME CALLBACK");
 
-        // Reset the derived Tracker
-        reset();
-
-        // Reset this
-        origin_ptr_     = last_ptr_;
-        last_ptr_       = incoming_ptr_;
-        incoming_ptr_   = nullptr; // This line is not really needed, but it makes things clearer.
+            // First we track the known Features between last and incoming
+            processKnown();
 
+            // Create a new non-key Frame in the Trajectory with the incoming Capture
+            FrameBasePtr closest_key_frm = getProblem()->getTrajectoryPtr()->closestKeyFrameToTimeStamp(incoming_ptr_->getTimeStamp());
+            if (closest_key_frm && checkTimeTolerance(closest_key_frm, incoming_ptr_))
+            {
+                // Just append the Capture to the existing keyframe
+                closest_key_frm->addCapture(incoming_ptr_);
+                WOLF_DEBUG("Incoming appended to F" , closest_key_frm->id() );
+            }
+            else
+            {
+                // Create a frame to hold what will become the last Capture
+                FrameBasePtr new_frame_ptr = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp());
+                new_frame_ptr->addCapture(incoming_ptr_); // Add incoming Capture to the new Frame
+                WOLF_DEBUG("Incoming in new F" , new_frame_ptr->id() );
+            }
 
-    }
+            // Reset the derived Tracker
+            reset();
 
-    // OTHER TIMES
-    else
-    {
-        WOLF_DEBUG("OTHER TIMES");
+            // Reset this
+            origin_ptr_     = last_ptr_;
+            last_ptr_       = incoming_ptr_;
+            incoming_ptr_   = nullptr; // This line is not really needed, but it makes things clearer.
 
-        // 1. First we track the known Features and create new constraints as needed
+            // Establish constraints from last
+            establishConstraints();
 
-        processKnown();
+            break;
+        }
 
-        // 2. Then we see if we want and we are allowed to create a KeyFrame
-        // Three conditions to make a KF:
-        //   - We vote for KF
-        //   - Problem allows us to make keyframe
-        //   - There is no existing KF very close to our Time Stamp <--- NOT SURE OF THIS
+        default:
+        {
+            WOLF_DEBUG("OTHER TIMES");
 
-        FrameBasePtr closest_key_frm_to_last = last_ptr_->getFramePtr(); // start with the same last's frame
-        if ( ! ( closest_key_frm_to_last && closest_key_frm_to_last->isKey() ) ) // last F is not KF
-            closest_key_frm_to_last = getProblem()->getTrajectoryPtr()->closestKeyFrameToTimeStamp(last_ptr_->getTimeStamp());
+            // 1. First we track the known Features and create new constraints as needed
 
-        if (closest_key_frm_to_last && abs(closest_key_frm_to_last->getTimeStamp() - last_ptr_->getTimeStamp()) > time_tolerance_) // closest KF is not close enough
-            closest_key_frm_to_last = nullptr;
+            processKnown();
 
-        if ( !( (voteForKeyFrame() && permittedKeyFrame() ) || closest_key_frm_to_last ) )
-        {
+            // 2. Then we see if we want and we are allowed to create a KeyFrame
+            // Three conditions to make a KF:
+            //   - We vote for KF
+            //   - Problem allows us to make keyframe
+            //   - There is no existing KF very close to our Time Stamp <--- NOT SURE OF THIS
 
-            // 2.a. We did not create a KeyFrame:
+            FrameBasePtr closest_key_frm_to_last = last_ptr_->getFramePtr(); // start with the same last's frame
+            if ( ! ( closest_key_frm_to_last && closest_key_frm_to_last->isKey() ) ) // last F is not KF
+                closest_key_frm_to_last = getProblem()->getTrajectoryPtr()->closestKeyFrameToTimeStamp(last_ptr_->getTimeStamp());
 
-            // advance the derived tracker
-            advance();
+            if (closest_key_frm_to_last && !checkTimeTolerance(closest_key_frm_to_last, last_ptr_)) // closest KF is not close enough
+                closest_key_frm_to_last = nullptr;
 
-            // Advance this
-            last_ptr_->getFramePtr()->addCapture(incoming_ptr_); // Add incoming Capture to the tracker's last Frame
-            last_ptr_->remove();
-            incoming_ptr_->getFramePtr()->setTimeStamp(incoming_ptr_->getTimeStamp());
-            last_ptr_ = incoming_ptr_; // Incoming Capture takes the place of last Capture
-            incoming_ptr_ = nullptr; // This line is not really needed, but it makes things clearer.
+            if ( !( (voteForKeyFrame() && permittedKeyFrame() ) || closest_key_frm_to_last ) )
+            {
 
-            WOLF_DEBUG("last <-- incoming");
+                // 2.a. We did not create a KeyFrame:
 
-        }
-        else
-        {
+                // advance the derived tracker
+                advance();
 
-            // 2.b. We create a KF
+                // Advance this
+                last_ptr_->getFramePtr()->addCapture(incoming_ptr_); // Add incoming Capture to the tracker's last Frame
+                last_ptr_->remove();
+                incoming_ptr_->getFramePtr()->setTimeStamp(incoming_ptr_->getTimeStamp());
+                last_ptr_ = incoming_ptr_; // Incoming Capture takes the place of last Capture
+                incoming_ptr_ = nullptr; // This line is not really needed, but it makes things clearer.
 
-            // Detect new Features, initialize Landmarks, create Constraints, ...
-            processNew(max_new_features_);
+                WOLF_DEBUG("last <-- incoming");
 
-            FrameBasePtr key_frm = getProblem()->getTrajectoryPtr()->closestKeyFrameToTimeStamp(incoming_ptr_->getTimeStamp());
-            if ( abs(key_frm->getTimeStamp() - incoming_ptr_->getTimeStamp() ) < time_tolerance_)
-            {
-                // Append incoming to existing key-frame
-                key_frm->addCapture(incoming_ptr_);
-                WOLF_DEBUG("Incoming adhered to existing KF" , key_frm->id());
             }
             else
             {
-                // Create a new non-key Frame in the Trajectory with the incoming Capture
-                // Make a non-key-frame to hold incoming
-                FrameBasePtr new_frame_ptr = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp());
-                new_frame_ptr->addCapture(incoming_ptr_); // Add incoming Capture to the new Frame
-                WOLF_DEBUG( "Incoming adhered to new F" , key_frm->id() );
 
-                // Make the last Capture's Frame a KeyFrame
-                setKeyFrame(last_ptr_);
-                WOLF_DEBUG( "Set KEY to last F" , key_frm->id() );
-            }
-
-            // Establish constraints between last and origin
-            establishConstraints();
-
-            // Reset the derived Tracker
-            reset();
-
-            // Reset this
-            origin_ptr_ = last_ptr_;
-            last_ptr_ = incoming_ptr_;
-            incoming_ptr_ = nullptr; // This line is not really needed, but it makes things clearer.
-
-}
+                // 2.b. We create a KF
+
+                // Detect new Features, initialize Landmarks, create Constraints, ...
+                processNew(max_new_features_);
+
+                FrameBasePtr key_frm = getProblem()->getTrajectoryPtr()->closestKeyFrameToTimeStamp(incoming_ptr_->getTimeStamp());
+                if ( abs(key_frm->getTimeStamp() - incoming_ptr_->getTimeStamp() ) < time_tolerance_)
+                {
+                    // Append incoming to existing key-frame
+                    key_frm->addCapture(incoming_ptr_);
+                    WOLF_DEBUG("Incoming adhered to existing KF" , key_frm->id());
+                }
+                else
+                {
+                    // Create a new non-key Frame in the Trajectory with the incoming Capture
+                    // Make a non-key-frame to hold incoming
+                    FrameBasePtr new_frame_ptr = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp());
+                    new_frame_ptr->addCapture(incoming_ptr_); // Add incoming Capture to the new Frame
+                    WOLF_DEBUG( "Incoming adhered to new F" , key_frm->id() );
+
+                    // Make the last Capture's Frame a KeyFrame
+                    setKeyFrame(last_ptr_);
+                    WOLF_DEBUG( "Set KEY to last F" , key_frm->id() );
+                }
+
+                // Reset the derived Tracker
+                reset();
+
+                // Reset this
+                origin_ptr_ = last_ptr_;
+                last_ptr_ = incoming_ptr_;
+                incoming_ptr_ = nullptr; // This line is not really needed, but it makes things clearer.
+
+                // Establish constraints between last and origin
+                establishConstraints();
 
+            }
 
+            break;
+        }
     }
+
     postProcess();
 
     //std::cout << "-----End of process():" << std::endl;
@@ -284,6 +296,16 @@ void ProcessorTracker::setKeyFrame(CaptureBasePtr _capture_ptr)
     }
 }
 
+void ProcessorTracker::computeProcessingStep()
+{
+    if (origin_ptr_ == nullptr && last_ptr_ == nullptr)
+        processing_step_ = FIRST_TIME;
+    else if (origin_ptr_ == nullptr)
+        processing_step_ = SECOND_TIME;
+}
+
+
+
 
 } // namespace wolf
 

src/processor_tracker.h

@@ -67,7 +67,11 @@ struct ProcessorParamsTracker : public ProcessorParamsBase
  */
 class ProcessorTracker : public ProcessorBase
 {
+    public:
+        typedef enum {FIRST_TIME, SECOND_TIME} ProcessingStep ;
+
     protected:
+        ProcessingStep processing_step_;
         CaptureBasePtr origin_ptr_;             ///< Pointer to the origin of the tracker.
         CaptureBasePtr last_ptr_;               ///< Pointer to the last tracked capture.
         CaptureBasePtr incoming_ptr_;           ///< Pointer to the incoming capture being processed.
@@ -91,6 +95,11 @@ class ProcessorTracker : public ProcessorBase
 
         virtual bool keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other);
 
+        bool checkTimeTolerance(const TimeStamp& _ts1, const TimeStamp& _ts2);
+        bool checkTimeTolerance(const CaptureBasePtr _cap, const TimeStamp& _ts);
+        bool checkTimeTolerance(const FrameBasePtr _frm, const TimeStamp& _ts);
+        bool checkTimeTolerance(const FrameBasePtr _frm, const CaptureBasePtr _cap);
+
         virtual CaptureBasePtr getOriginPtr();
         virtual CaptureBasePtr getLastPtr();
         virtual CaptureBasePtr getIncomingPtr();
@@ -185,11 +194,14 @@ class ProcessorTracker : public ProcessorBase
 
     protected:
 
+        void computeProcessingStep();
+
         void addNewFeatureLast(FeatureBasePtr _feature_ptr);
 
         FeatureBaseList& getNewFeaturesListIncoming();
 
         void addNewFeatureIncoming(FeatureBasePtr _feature_ptr);
+
 };
 
 inline void ProcessorTracker::setMaxNewFeatures(const unsigned int& _max_new_features)
@@ -217,6 +229,26 @@ inline FeatureBaseList& ProcessorTracker::getNewFeaturesListIncoming()
     return new_features_incoming_;
 }
 
+inline bool ProcessorTracker::checkTimeTolerance(const TimeStamp& _ts1, const TimeStamp& _ts2)
+{
+    return (std::fabs(_ts2 - _ts2) < time_tolerance_);
+}
+
+inline bool ProcessorTracker::checkTimeTolerance(const CaptureBasePtr _cap, const TimeStamp& _ts)
+{
+    return checkTimeTolerance(_cap->getTimeStamp(), _ts);
+}
+
+inline bool ProcessorTracker::checkTimeTolerance(const FrameBasePtr _frm, const TimeStamp& _ts)
+{
+    return checkTimeTolerance(_frm->getTimeStamp(), _ts);
+}
+
+inline bool ProcessorTracker::checkTimeTolerance(const FrameBasePtr _frm, const CaptureBasePtr _cap)
+{
+    return checkTimeTolerance(_frm->getTimeStamp(), _cap->getTimeStamp());
+}
+
 inline void ProcessorTracker::addNewFeatureIncoming(FeatureBasePtr _feature_ptr)
 {
     new_features_incoming_.push_back(_feature_ptr);