Merge branch 'devel' of...

Merge branch 'devel' of ssh://gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/wolf into devel

src/processor/processor_tracker.cpp

@@ -112,15 +112,15 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
             incoming_ptr_->link(keyframe);
 
             // Process info
+            // TrackerFeature:  We only process new features in Last, here last = nullptr, so we do not have anything to do.
+            // TrackerLandmark: If we have been given a map, all landmarks in the map are known. Process them.
             processKnown();
-            // We only process new features in Last, here last = nullptr, so we do not have anything to do.
 
             // Issue KF callback with new KF
             getProblem()->keyFrameCallback(keyframe, shared_from_this());
 
-            resetDerived();
-
             // Update pointers
+            resetDerived();
             origin_ptr_ = incoming_ptr_;
             last_ptr_   = incoming_ptr_;
             incoming_ptr_ = nullptr;
@@ -130,9 +130,17 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
         case SECOND_TIME_WITH_KEYFRAME :
         {
         	// No-break case only for debug. Next case will be executed too.
-            FrameBasePtr keyframe_from_callback = buffer_frame_.select( incoming_ptr_->getTimeStamp(),
+            FrameBasePtr keyframe_from_callback = buffer_frame_.select( last_ptr_->getTimeStamp(),
                                                                         params_tracker_->time_tolerance);
-
+            // This received KF is discarded since it is most likely the same KF we createed in FIRST_TIME, ...
+            // ... only that in FIRST_TIME we checked for incominig, and now we checked for last.
+            // Such KF however should have been removed from the buffer of keyframes with the call to buffer_frame_.removeUpTo()
+            
+            // The debug line is here to check if this is really the same KF
+            // or it is rather a new KF created by some other processor,
+            // which happens to be within tolerance of timestamps.
+            // In this case we discard it anyway because we already have a KF in last
+            // and we can't link a capture to two KFs.
             WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITH_KEYFRAME: KF" , keyframe_from_callback->id() , " callback unpacked with ts= " , keyframe_from_callback->getTimeStamp() );
         }
         // Fall through
@@ -140,14 +148,16 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
         {
             WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITHOUT_KEYFRAME" );
 
+            // Make a NON KEY Frame to hold incoming capture
             FrameBasePtr keyframe = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
                                                                 getProblem()->getFrameStructure(),
                                                                 getProblem()->getState());
             incoming_ptr_->link(keyframe);
-            // We have a last_ Capture with no features, so we do not process known features, and we do not vote for KF.
 
             // Process info
+            // TrackerLandmark: If we have been given a map, all landmarks in the map are known. Process them.
             processKnown();
+            // Both Trackers:  We have a last_ Capture with not enough features, so populate it.
             processNew(params_tracker_->max_new_features);
 
             // Establish factors
@@ -177,13 +187,13 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
             last_ptr_->move(keyframe_from_callback);
             last_old_frame->remove();
 
-            // Create new frame for incoming
+            // Create new NON KEY frame for incoming
             FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
                                                            getProblem()->getFrameStructure(),
                                                            getProblem()->getState());
             incoming_ptr_->link(frame);
 
-            // Detect new Features, initialize Landmarks, create Factors, ...
+            // Detect new Features, initialize Landmarks, ...
             processNew(params_tracker_->max_new_features);
 
             // Establish factors
@@ -209,8 +219,6 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
                 // process
                 processNew(params_tracker_->max_new_features);
 
-                //TODO abort KF if known_features_last_.size() < params_tracker_->min_features_for_keyframe
-
                 // We create a KF
                 // set KF on last
                 last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
@@ -222,14 +230,15 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
                 // Call the new keyframe callback in order to let the other processors to establish their factors
                 getProblem()->keyFrameCallback(last_ptr_->getFrame(), shared_from_this());
 
-                // Update pointers
-                resetDerived();
 
-                // make F; append incoming to new F
+                // make NON KEY frame; append incoming to new frame
                 FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
                                                                  getProblem()->getFrameStructure(),
                                                                  getProblem()->getState(incoming_ptr_->getTimeStamp()));
                 incoming_ptr_   ->link(frame);
+
+                // Update pointers
+                resetDerived();
                 origin_ptr_     = last_ptr_;
                 last_ptr_       = incoming_ptr_;
                 last_frame_ptr_ = frame;
@@ -243,7 +252,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
                 // Advance this
                 advanceDerived();
 
-                // Replace last frame for a new one in incoming
+                // Replace last frame for a new NON KEY frame in incoming
                 FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
                                                                  getProblem()->getFrameStructure(),
                                                                  getProblem()->getState(incoming_ptr_->getTimeStamp()));
@@ -278,7 +287,7 @@ void ProcessorTracker::computeProcessingStep()
     // Then combine with the existence (or not) of a keyframe callback pack
     switch (step)
     {
-        case FIRST_TIME :
+        case FIRST_TIME : // We check for KF in incoming
 
             if (buffer_frame_.select(incoming_ptr_->getTimeStamp(), params_tracker_->time_tolerance))
                 processing_step_ = FIRST_TIME_WITH_KEYFRAME;
@@ -286,7 +295,7 @@ void ProcessorTracker::computeProcessingStep()
                 processing_step_ = FIRST_TIME_WITHOUT_KEYFRAME;
         break;
 
-        case SECOND_TIME :
+        case SECOND_TIME : // We check for KF in last
 
             if (buffer_frame_.select(last_ptr_->getTimeStamp(), params_tracker_->time_tolerance))
                 processing_step_ = SECOND_TIME_WITH_KEYFRAME;
@@ -294,7 +303,7 @@ void ProcessorTracker::computeProcessingStep()
                 processing_step_ = SECOND_TIME_WITHOUT_KEYFRAME;
             break;
 
-        case RUNNING :
+        case RUNNING : // We check for KF in last
         default :
 
             if (buffer_frame_.select(last_ptr_->getTimeStamp(), params_tracker_->time_tolerance))