WIP revert to making tracks on KFs on the fly

This way we avoid having to maintain consistency between track_matrix and the wolf tree each time kfs are created / removed or frames are changed key/auxiliar/non_estimated status

src/processor/track_matrix.cpp

@@ -220,8 +220,26 @@ CaptureBasePtr TrackMatrix::firstCapture(size_t _track_id)
 
 Track TrackMatrix::trackAtKeyframes(size_t _track_id)
 {
-    if (tracks_kf_.count(_track_id) > 0)
-        return tracks_kf_.at(_track_id);
+//    // This requires tracks_kf_
+//    if (tracks_kf_.count(_track_id) > 0)
+//        return tracks_kf_.at(_track_id);
+//    else
+//        return Track();
+
+
+    // This does not require any tracks_kf_; it assembles a track_kf on the fly by checking each capture's frame.
+    if (tracks_.count(_track_id))
+    {
+        Track track_kf;
+        for (auto& pair_ts_ftr : tracks_.at(_track_id))
+        {
+            auto& ts  = pair_ts_ftr.first;
+            auto& ftr = pair_ts_ftr.second;
+            if (ftr && ftr->getCapture() && ftr->getCapture()->getFrame() && ftr->getCapture()->getFrame()->isKey())
+                track_kf[ts] = ftr;
+        }
+        return track_kf;
+    }
     else
         return Track();
 }
@@ -250,14 +268,21 @@ bool TrackMatrix::markKeyframe(CaptureBasePtr _capture)
 bool TrackMatrix::unmarkKeyframe(CaptureBasePtr _capture)
 {
     bool removed = false;
-    auto ts = _capture->getTimeStamp();
-    for (auto pair_id_trk : tracks_kf_)
+    if (_capture)
     {
-        if (pair_id_trk.second.erase(ts))
+        auto ts = _capture->getTimeStamp();
+        for (auto& pair_id_trk : tracks_kf_)
         {
-            if (pair_id_trk.second.empty())
-                tracks_kf_.erase(pair_id_trk.first);
-            removed = true;
+            if (pair_id_trk.second.erase(ts))   // remove feature from track
+            {
+                if (pair_id_trk.second.empty()) // remove empty tracks
+                {
+                    tracks_kf_.erase(pair_id_trk.first);
+                    if (tracks_kf_.empty())
+                        break;
+                }
+                removed = true;
+            }
         }
     }
     return removed;

test/gtest_track_matrix.cpp

@@ -403,6 +403,8 @@ TEST_F(TrackMatrixTest, trackAtKeyframes)
 
     wolf::Track trk_kf_0 = track_matrix.trackAtKeyframes(f0->trackId());
     ASSERT_EQ(trk_kf_0.size(), 2);
+    ASSERT_EQ(trk_kf_0[0.0], f0);
+    ASSERT_EQ(trk_kf_0[4.0], f4);
 
     wolf::Track trk_kf_1 = track_matrix.trackAtKeyframes(f3->trackId());
     ASSERT_EQ(trk_kf_1.size(), 0);