Merge branch 'add-track-keyframes' into 'devel'

Add track keyframes

See merge request !285

include/core/frame/frame_base.h

@@ -80,6 +80,7 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
         bool isKeyOrAux() const;
 
         // set type
+        void setNonEstimated();
         void setKey();
         void setAux();
 

include/core/processor/track_matrix.h

@@ -78,6 +78,7 @@ class TrackMatrix
         TrackMatrix();
         virtual ~TrackMatrix();
 
+        // tracks across all Captures
         void            newTrack    (CaptureBasePtr _cap, FeatureBasePtr _ftr);
         void            add         (size_t _track_id, CaptureBasePtr _cap, FeatureBasePtr _ftr);
         void            remove      (FeatureBasePtr _ftr);
@@ -97,15 +98,23 @@ class TrackMatrix
         FeatureBasePtr  feature     (size_t _track_id, CaptureBasePtr _cap);
         CaptureBasePtr  firstCapture(size_t _track_id);
 
+        // tracks across captures that belong to keyframe
+        SizeStd         numKeyframeTracks();
+        Track           trackAtKeyframes(size_t _track_id);
+        bool            markKeyframe(CaptureBasePtr _capture);
+        bool            unmarkKeyframe(FrameBasePtr _keyframe);
+
     private:
 
         static SizeStd track_id_count_;
 
         // Along track: maps of Feature pointers indexed by time stamp.
+        // tracks across all Captures
         map<size_t, Track > tracks_;       // map indexed by track_Id   of ( maps indexed by TimeStamp  of ( features ) )
+        // tracks across captures that belong to keyframe
+        map<size_t, Track > tracks_kf_;    // map indexed by track_Id   of ( maps indexed by TimeStamp  of ( features ) )
 
         // Across track: maps of Feature pointers indexed by track_Id.
-//        map<size_t, Snapshot > snapshots_; // map indexed by capture_Id of ( maps indexed by track_Id of ( features ) )
         map<CaptureBasePtr, Snapshot > snapshots_; // map indexed by capture_ptr of ( maps indexed by track_Id of ( features ) )
 };
 

src/frame/frame_base.cpp

@@ -155,6 +155,20 @@ void FrameBase::removeStateBlocks()
     }
 }
 
+void FrameBase::setNonEstimated()
+{
+    // unregister if previously estimated
+    if (isKeyOrAux())
+        removeStateBlocks();
+
+    type_ = NON_ESTIMATED;
+    if (getTrajectory())
+    {
+        getTrajectory()->sortFrame(shared_from_this());
+        getTrajectory()->updateLastFrames();
+    }
+}
+
 void FrameBase::setKey()
 {
     // register if previously not estimated
@@ -163,8 +177,11 @@ void FrameBase::setKey()
 
     // WOLF_DEBUG("Set Key", this->id());
     type_ = KEY;
-    getTrajectory()->sortFrame(shared_from_this());
-    getTrajectory()->updateLastFrames();
+    if (getTrajectory())
+    {
+        getTrajectory()->sortFrame(shared_from_this());
+        getTrajectory()->updateLastFrames();
+    }
 }
 
 void FrameBase::setAux()
@@ -174,8 +191,11 @@ void FrameBase::setAux()
 
     // WOLF_DEBUG("Set Auxiliary", this->id());
     type_ = AUXILIARY;
-    getTrajectory()->sortFrame(shared_from_this());
-    getTrajectory()->updateLastFrames();
+    if (getTrajectory())
+    {
+        getTrajectory()->sortFrame(shared_from_this());
+        getTrajectory()->updateLastFrames();
+    }
 }
 
 void FrameBase::fix()

src/processor/track_matrix.cpp

@@ -53,6 +53,8 @@ void TrackMatrix::add(size_t _track_id, CaptureBasePtr _cap, FeatureBasePtr _ftr
         _ftr->setCapture(_cap);
     tracks_[_track_id].emplace(_cap->getTimeStamp(), _ftr);
     snapshots_[_cap].emplace(_track_id, _ftr);        // will create new snapshot if _cap_id   is not present
+    if (_cap->getFrame() && _cap->getFrame()->isKey())
+        tracks_kf_[_track_id].emplace(_cap->getTimeStamp(), _ftr);
 }
 
 void TrackMatrix::remove(size_t _track_id)
@@ -62,13 +64,6 @@ void TrackMatrix::remove(size_t _track_id)
     {
         for (auto const& pair_time_ftr : tracks_.at(_track_id))
         {
-//            SizeStd cap_id = pair_time_ftr.second->getCapture()->id();
-//            snapshots_.at(cap_id).erase(_track_id);
-//            if (snapshots_.at(cap_id).empty())
-//                snapshots_.erase(cap_id);
-
-
-
             CaptureBasePtr cap = pair_time_ftr.second->getCapture();
             snapshots_.at(cap).erase(_track_id);
             if (snapshots_.at(cap).empty())
@@ -79,6 +74,9 @@ void TrackMatrix::remove(size_t _track_id)
         // Remove track
         tracks_.erase(_track_id);
     }
+    if (tracks_kf_.count(_track_id))
+        // Remove track
+        tracks_kf_.erase(_track_id);
 }
 
 void TrackMatrix::remove(CaptureBasePtr _cap)
@@ -93,6 +91,12 @@ void TrackMatrix::remove(CaptureBasePtr _cap)
             tracks_.at(trk_id).erase(ts);
             if (tracks_.at(trk_id).empty())
                 tracks_.erase(trk_id);
+            if (_cap->getFrame() && _cap->getFrame()->isKey())
+            {
+                tracks_kf_.at(trk_id).erase(ts);
+                if (tracks_kf_.at(trk_id).empty())
+                    tracks_kf_.erase(trk_id);
+            }
         }
 
         // remove snapshot
@@ -107,10 +111,17 @@ void TrackMatrix::remove(FeatureBasePtr _ftr)
     {
         if (auto cap = _ftr->getCapture())
         {
-            tracks_   .at(_ftr->trackId()).erase(cap->getTimeStamp());
+            tracks_    .at(_ftr->trackId()).erase(cap->getTimeStamp());
             if (tracks_.at(_ftr->trackId()).empty())
                 tracks_.erase(_ftr->trackId());
 
+            if (cap->getFrame() && cap->getFrame()->isKey())
+            {
+                tracks_kf_    .at(_ftr->trackId()).erase(cap->getTimeStamp());
+                if (tracks_kf_.at(_ftr->trackId()).empty())
+                    tracks_kf_.erase(_ftr->trackId());
+            }
+
             snapshots_.at(cap)      .erase(_ftr->trackId());
             if (snapshots_.at(cap).empty())
                 snapshots_.erase(cap);
@@ -123,6 +134,11 @@ size_t TrackMatrix::numTracks()
     return tracks_.size();
 }
 
+size_t TrackMatrix::numKeyframeTracks()
+{
+    return tracks_kf_.size();
+}
+
 size_t TrackMatrix::trackSize(size_t _track_id)
 {
     return track(_track_id).size();
@@ -202,4 +218,50 @@ CaptureBasePtr TrackMatrix::firstCapture(size_t _track_id)
     return firstFeature(_track_id)->getCapture();
 }
 
+Track TrackMatrix::trackAtKeyframes(size_t _track_id)
+{
+    if (tracks_kf_.count(_track_id) > 0)
+        return tracks_kf_.at(_track_id);
+    else
+        return Track();
+}
+
+bool TrackMatrix::markKeyframe(CaptureBasePtr _capture)
+{
+    if (_capture->getFrame() && _capture->getFrame()->isKey())
+    {
+        auto snap = snapshot(_capture);
+
+        if (snap.empty())
+            return false;
+
+        for (auto pair_trkid_ftr : snap)
+        {
+            auto track_id = pair_trkid_ftr.first;
+            auto ftr      = pair_trkid_ftr.second;
+            auto ts       = _capture->getFrame()->getTimeStamp();
+            tracks_kf_[track_id][ts] = ftr;
+        }
+        return true;
+    }
+    return false;
+}
+
+bool TrackMatrix::unmarkKeyframe(FrameBasePtr _keyframe)
+{
+    bool removed = false;
+    auto ts = _keyframe->getTimeStamp();
+    for (auto pair_id_trk : tracks_kf_)
+    {
+        if (pair_id_trk.second.erase(ts))
+        {
+            if (pair_id_trk.second.empty())
+                tracks_kf_.erase(pair_id_trk.first);
+            removed = true;
+        }
+    }
+    return removed;
+}
+
+
 }

test/gtest_track_matrix.cpp

@@ -19,11 +19,18 @@ class TrackMatrixTest : public testing::Test
         Eigen::Vector2s m;
         Eigen::Matrix2s m_cov = Eigen::Matrix2s::Identity()*0.01;
 
+        FrameBasePtr   F0, F1, F2, F3, F4;
         CaptureBasePtr C0, C1, C2, C3, C4;
         FeatureBasePtr f0, f1, f2, f3, f4;
 
         virtual void SetUp()
         {
+            F0 = std::make_shared<FrameBase>(0.0, nullptr);
+            F1 = std::make_shared<FrameBase>(1.0, nullptr);
+            F2 = std::make_shared<FrameBase>(2.0, nullptr);
+            F3 = std::make_shared<FrameBase>(3.0, nullptr);
+            F4 = std::make_shared<FrameBase>(4.0, nullptr);
+
             C0 = std::make_shared<CaptureBase>("BASE", 0.0);
             C1 = std::make_shared<CaptureBase>("BASE", 1.0);
             C2 = std::make_shared<CaptureBase>("BASE", 2.0);
@@ -35,6 +42,17 @@ class TrackMatrixTest : public testing::Test
             f2 = std::make_shared<FeatureBase>("BASE", m, m_cov);
             f3 = std::make_shared<FeatureBase>("BASE", m, m_cov);
             f4 = std::make_shared<FeatureBase>("BASE", m, m_cov);
+
+            // F0 and F4 are keyframes
+            F0->setKey();
+            F4->setKey();
+
+            // link captures
+            C0->link(F0);
+            C1->link(F1);
+            C2->link(F2);
+            C3->link(F3);
+            C4->link(F4);
         }
 };
 
@@ -42,30 +60,58 @@ TEST_F(TrackMatrixTest, newTrack)
 {
     track_matrix.newTrack(C0, f0);
     ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
+    ASSERT_EQ(track_matrix.numKeyframeTracks(), (unsigned int) 1);
 
     track_matrix.newTrack(C0, f1);
     ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 2);
+    ASSERT_EQ(track_matrix.numKeyframeTracks(), (unsigned int) 2);
+
+    track_matrix.newTrack(C1, f2);
+    ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 3);
+    ASSERT_EQ(track_matrix.numKeyframeTracks(), (unsigned int) 2);
+
+    track_matrix.newTrack(C1, f3);
+    ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 4);
+    ASSERT_EQ(track_matrix.numKeyframeTracks(), (unsigned int) 2);
 }
 
 TEST_F(TrackMatrixTest, add)
 {
     track_matrix.newTrack(C0, f0);
+    ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
+    ASSERT_EQ(track_matrix.numKeyframeTracks(), (unsigned int) 1);
 
     track_matrix.add(f0->trackId(), C1, f1);
-    /*  C0   C1   C2   snapshots
+    /* KC0   C1   C2   snapshots
      *
      *  f0---f1        trk 0
      */
     ASSERT_EQ(track_matrix.trackSize(f1->trackId()), (unsigned int) 2);
     ASSERT_EQ(f1->trackId(), f0->trackId());
+    ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
+    ASSERT_EQ(track_matrix.numKeyframeTracks(), (unsigned int) 1);
 
     track_matrix.add(f0->trackId(), C2, f2);
-    /*  C0   C1   C2   snapshots
+    /* KC0   C1   C2   snapshots
      *
      *  f0---f1---f2   trk 0
      */
     ASSERT_EQ(track_matrix.trackSize(f2->trackId()), (unsigned int) 3);
     ASSERT_EQ(f2->trackId(), f0->trackId());
+    ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
+    ASSERT_EQ(track_matrix.numKeyframeTracks(), (unsigned int) 1);
+
+    track_matrix.add(f0->trackId(), C2, f2);
+    /* KC0   C1   C2   snapshots
+     *
+     *  f0---f1---f2   trk 0
+     *       f3        trk 1
+     */
+    track_matrix.add(1, C1, f3);
+    ASSERT_EQ(track_matrix.trackSize(f3->trackId()), (unsigned int) 1);
+    ASSERT_NE(f3->trackId(), f0->trackId());
+    ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 2);
+    ASSERT_EQ(track_matrix.numKeyframeTracks(), (unsigned int) 1);
 }
 
 TEST_F(TrackMatrixTest, numTracks)
@@ -136,6 +182,7 @@ TEST_F(TrackMatrixTest, remove_ftr)
      */
 
     ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 2);
+    ASSERT_EQ(track_matrix.numKeyframeTracks(), (unsigned int) 2);
 
     track_matrix.remove(f0);
     /*  C0   C1   C2   snapshots
@@ -151,10 +198,23 @@ TEST_F(TrackMatrixTest, remove_ftr)
     ASSERT_EQ(track_matrix.snapshot(C0).at(f2->trackId()), f2);
     ASSERT_EQ(track_matrix.snapshot(C1).size(), (unsigned int) 1);
     ASSERT_EQ(track_matrix.snapshot(C1).at(f0->trackId()), f1);
+    ASSERT_EQ(track_matrix.numKeyframeTracks(), (unsigned int) 1);
 
     track_matrix.remove(f1);
+    /*  C0   C1   C2   snapshots
+     *
+     *  f2             trk 1
+     */
     ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
     ASSERT_EQ(track_matrix.firstFeature(f2->trackId()), f2);
+    ASSERT_EQ(track_matrix.numKeyframeTracks(), (unsigned int) 1);
+
+    track_matrix.remove(f2);
+    /*  C0   C1   C2   snapshots
+     *
+     */
+    ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 0);
+    ASSERT_EQ(track_matrix.numKeyframeTracks(), (unsigned int) 0);
 }
 
 TEST_F(TrackMatrixTest, remove_trk)
@@ -171,13 +231,16 @@ TEST_F(TrackMatrixTest, remove_trk)
      */
 
     ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 2);
+    ASSERT_EQ(track_matrix.numKeyframeTracks(), (unsigned int) 2);
 
     track_matrix.remove(f0->trackId());
     ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
     ASSERT_EQ(track_matrix.firstFeature(f2->trackId()), f2);
+    ASSERT_EQ(track_matrix.numKeyframeTracks(), (unsigned int) 1);
 
     track_matrix.remove(f2->trackId());
     ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 0);
+    ASSERT_EQ(track_matrix.numKeyframeTracks(), (unsigned int) 0);
 }
 
 TEST_F(TrackMatrixTest, remove_snapshot)
@@ -194,6 +257,7 @@ TEST_F(TrackMatrixTest, remove_snapshot)
      */
 
     ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 2);
+    ASSERT_EQ(track_matrix.numKeyframeTracks(), (unsigned int) 2);
 
     track_matrix.remove(C0);
     /*  C1   C2   snapshots
@@ -202,9 +266,11 @@ TEST_F(TrackMatrixTest, remove_snapshot)
      */
     ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 1);
     ASSERT_EQ(track_matrix.firstFeature(f1->trackId()), f1);
+    ASSERT_EQ(track_matrix.numKeyframeTracks(), (unsigned int) 0);
 
     track_matrix.remove(C1);
     ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 0);
+    ASSERT_EQ(track_matrix.numKeyframeTracks(), (unsigned int) 0);
 }
 
 TEST_F(TrackMatrixTest, track)
@@ -321,6 +387,88 @@ TEST_F(TrackMatrixTest, matches)
     ASSERT_EQ(pairs.size(), (unsigned int) 0);
 }
 
+TEST_F(TrackMatrixTest, trackAtKeyframes)
+{
+    track_matrix.newTrack(C0, f0);
+    track_matrix.add(f0->trackId(), C1, f1);
+    track_matrix.add(f0->trackId(), C2, f2);
+    track_matrix.add(f0->trackId(), C4, f4);
+    track_matrix.newTrack(C1, f3);
+    /* KC0   C1   C2   C3  KC4 snapshots
+     *
+     *  f0---f1---f2--------f4 trk 0
+     *       |
+     *       f3                trk 1
+     */
+
+    wolf::Track trk_kf_0 = track_matrix.trackAtKeyframes(f0->trackId());
+    ASSERT_EQ(trk_kf_0.size(), 2);
+
+    wolf::Track trk_kf_1 = track_matrix.trackAtKeyframes(f3->trackId());
+    ASSERT_EQ(trk_kf_1.size(), 0);
+}
+
+TEST_F(TrackMatrixTest, markKeyframe)
+{
+    track_matrix.newTrack(C0, f0);
+    track_matrix.add(f0->trackId(), C1, f1);
+    track_matrix.add(f0->trackId(), C2, f2);
+    track_matrix.add(1, C1, f3);
+    /* KC0   C1   C2   C3   snapshots
+     *
+     *  f0---f1---f2        trk 0
+     *       |
+     *       f3             trk 1
+     */
+
+    ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 2);
+    ASSERT_EQ(track_matrix.numKeyframeTracks(), (unsigned int) 1);
+
+    C1->getFrame()->setKey();
+    track_matrix.markKeyframe(C1);
+    /* KC0  KC1   C2   C3   snapshots
+     *
+     *  f0---f1---f2        trk 0
+     *       |
+     *       f3             trk 1
+     */
+
+    ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 2);
+    ASSERT_EQ(track_matrix.numKeyframeTracks(), (unsigned int) 2);
+
+}
+
+TEST_F(TrackMatrixTest, unmarkKeyframe)
+{
+    track_matrix.newTrack(C0, f0);
+    track_matrix.add(f0->trackId(), C1, f1);
+    track_matrix.add(f0->trackId(), C2, f2);
+    track_matrix.add(1, C1, f3);
+    /* KC0   C1   C2   C3   snapshots
+     *
+     *  f0---f1---f2        trk 0
+     *       |
+     *       f3             trk 1
+     */
+
+    ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 2);
+    ASSERT_EQ(track_matrix.numKeyframeTracks(), (unsigned int) 1);
+
+    F0->setNonEstimated();
+    track_matrix.unmarkKeyframe(F0);
+    /*  C0   C1   C2   C3   snapshots
+     *
+     *  f0---f1---f2        trk 0
+     *       |
+     *       f3             trk 1
+     */
+
+    ASSERT_EQ(track_matrix.numTracks(), (unsigned int) 2);
+    ASSERT_EQ(track_matrix.numKeyframeTracks(), (unsigned int) 0);
+
+
+}
+
 int main(int argc, char **argv)
 {
     testing::InitGoogleTest(&argc, argv);