diff --git a/include/core/processor/track_matrix.h b/include/core/processor/track_matrix.h
index 62a6a548edd037bf1f9dfd91c8cdf7014f4de70d..7ccf8a73fbdf7df6d4e69f9e8fcc4bbf1d2e3909 100644
--- a/include/core/processor/track_matrix.h
+++ b/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 ) )
};
diff --git a/src/processor/track_matrix.cpp b/src/processor/track_matrix.cpp
index 8ab0c78822a4cd5dc2462b4a413e0a9e95a222af..56e87f15e3b94fb94705e909cbc928f39b701a31 100644
--- a/src/processor/track_matrix.cpp
+++ b/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;
+}
+
+
}
diff --git a/test/gtest_track_matrix.cpp b/test/gtest_track_matrix.cpp
index ff930b512b8f8da960b3921f81bc2bd73eac03b5..1b84496eb1b3c49138ffbc9f97666558617133f5 100644
--- a/test/gtest_track_matrix.cpp
+++ b/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);