diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 3d3794e75b1a87461832695c26341b8f40fc11f5..b0e08b4b59240b359457565c95071d2b7c1385e6 100644
--- a/src/processor/processor_tracker.cpp
+++ b/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))