From 6270c258cd4aa84bbcab7d8aba3c3e72450e6c36 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Mon, 7 Jan 2019 13:49:11 +0100
Subject: [PATCH] Fix bug with provided map...

The first processed capture had no features. Now `processKnown()` is called always.

A fix was required in processor_traker_feature for the case of `last_ptr_==nullptr` typical of the `FIRST_TIME_XXX` in the state machine.
---
 src/processor_tracker.cpp         | 5 ++++-
 src/processor_tracker_feature.cpp | 5 +++++
 2 files changed, 9 insertions(+), 1 deletion(-)

diff --git a/src/processor_tracker.cpp b/src/processor_tracker.cpp
index 1fb61cdef..9361d298f 100644
--- a/src/processor_tracker.cpp
+++ b/src/processor_tracker.cpp
@@ -61,6 +61,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
             pack->key_frame->addCapture(incoming_ptr_);
 
             // Process info
+            processKnown();
             // We only process new features in Last, here last = nullptr, so we do not have anything to do.
 
             // Update pointers
@@ -77,6 +78,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
             kfrm->addCapture(incoming_ptr_);
 
             // Process info
+            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
@@ -95,7 +97,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
         	// No-break case only for debug. Next case will be executed too.
             PackKeyFramePtr pack = kf_pack_buffer_.selectPack( incoming_ptr_, params_tracker_->time_tolerance);
             WOLF_DEBUG( "PT ", getName(), ": KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp().get() );
-        }
+        } // @suppress("No break at end of case")
         case SECOND_TIME_WITHOUT_PACK :
         {
             FrameBasePtr frm = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp());
@@ -104,6 +106,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
             // We have a last_ Capture with no features, so we do not process known features, and we do not vote for KF.
 
             // Process info
+            processKnown();
             processNew(params_tracker_->max_new_features);
 
             // Establish constraints
diff --git a/src/processor_tracker_feature.cpp b/src/processor_tracker_feature.cpp
index 05c5e2e9a..049e0bf5c 100644
--- a/src/processor_tracker_feature.cpp
+++ b/src/processor_tracker_feature.cpp
@@ -61,6 +61,11 @@ unsigned int ProcessorTrackerFeature::processKnown()
     assert(matches_last_from_incoming_.size() == 0
             && "In ProcessorTrackerFeature::processKnown(): match list from last to incoming must be empty before processKnown()");
 
+    if (!last_ptr_ || last_ptr_->getFeatureList().empty())
+    {
+        return 0;
+    }
+
     // Track features from last_ptr_ to incoming_ptr_
     trackFeatures(last_ptr_->getFeatureList(), known_features_incoming_, matches_last_from_incoming_);
     for (auto match : matches_last_from_incoming_)
-- 
GitLab