diff --git a/include/core/processor/track_matrix.h b/include/core/processor/track_matrix.h
index f609dc8efefbbb6fb7c74b7dac46ec8f052b0403..3b06c46381d24a2191ec271b7a741b121b26fbbb 100644
--- a/include/core/processor/track_matrix.h
+++ b/include/core/processor/track_matrix.h
@@ -45,8 +45,8 @@ using std::list;
using std::pair;
using std::shared_ptr;
-typedef map<TimeStamp, FeatureBasePtr> Track;
-typedef map<TimeStamp, FeatureBaseConstPtr> TrackConst;
+typedef map<TimeStamp, FeatureBasePtr> Track;
+typedef map<TimeStamp, FeatureBaseConstPtr> TrackConst;
typedef map<SizeStd, FeatureBasePtr > Snapshot;
typedef map<SizeStd, FeatureBaseConstPtr > SnapshotConst;
typedef map<SizeStd, pair<FeatureBasePtr, FeatureBasePtr> > TrackMatches; // matched feature pairs indexed by track_id
@@ -102,41 +102,40 @@ class TrackMatrix
TrackMatrix();
virtual ~TrackMatrix();
- void newTrack (FeatureBasePtr _ftr);
- void add (const SizeStd& _track_id, const FeatureBasePtr& _ftr);
- void add (const FeatureBasePtr& _ftr_existing, const FeatureBasePtr& _ftr_new);
- void remove (FeatureBasePtr _ftr);
- void remove (const SizeStd& _track_id);
- void remove (CaptureBasePtr _cap);
- SizeStd numTracks () const;
- SizeStd trackSize (const SizeStd& _track_id) const;
- TrackConst track (const SizeStd& _track_id) const;
- Track track (const SizeStd& _track_id);
- SnapshotConst snapshot (CaptureBaseConstPtr _capture) const;
- Snapshot snapshot (CaptureBasePtr _capture);
+ void newTrack (FeatureBasePtr _ftr);
+ void add (const SizeStd& _track_id, const FeatureBasePtr& _ftr);
+ void add (const FeatureBasePtr& _ftr_existing, const FeatureBasePtr& _ftr_new);
+ void remove (FeatureBasePtr _ftr);
+ void remove (const SizeStd& _track_id);
+ void remove (CaptureBasePtr _cap);
+
+ SizeStd numTracks () const;
+ SizeStd trackSize (const SizeStd& _track_id) const;
+ TrackConst track (const SizeStd& _track_id) const;
+ Track track (const SizeStd& _track_id);
+ SnapshotConst snapshot (CaptureBaseConstPtr _capture) const;
+ Snapshot snapshot (CaptureBasePtr _capture);
vector<FeatureBaseConstPtr> trackAsVector(const SizeStd& _track_id) const;
- vector<FeatureBasePtr> trackAsVector(const SizeStd& _track_id);
- FeatureBaseConstPtrList snapshotAsList(CaptureBaseConstPtr _cap) const;
- FeatureBasePtrList snapshotAsList(CaptureBasePtr _cap);
- TrackMatchesConst matches (CaptureBaseConstPtr _cap_1, CaptureBaseConstPtr _cap_2) const;
- TrackMatches matches (CaptureBasePtr _cap_1, CaptureBasePtr _cap_2);
- FeatureBaseConstPtr firstFeature(const SizeStd& _track_id) const;
- FeatureBasePtr firstFeature(const SizeStd& _track_id);
- FeatureBaseConstPtr lastFeature (const SizeStd& _track_id) const;
- FeatureBasePtr lastFeature (const SizeStd& _track_id);
- FeatureBaseConstPtr feature (const SizeStd& _track_id, CaptureBaseConstPtr _cap) const;
- FeatureBasePtr feature (const SizeStd& _track_id, CaptureBasePtr _cap);
- CaptureBaseConstPtr firstCapture(const SizeStd& _track_id) const;
- CaptureBasePtr firstCapture(const SizeStd& _track_id);
-
- list<SizeStd> trackIds() const;
+ vector<FeatureBasePtr> trackAsVector(const SizeStd& _track_id);
+ FeatureBaseConstPtrList snapshotAsList(CaptureBaseConstPtr _cap) const;
+ FeatureBasePtrList snapshotAsList(CaptureBasePtr _cap);
+ TrackMatchesConst matches (CaptureBaseConstPtr _cap_1, CaptureBaseConstPtr _cap_2) const;
+ TrackMatches matches (CaptureBasePtr _cap_1, CaptureBasePtr _cap_2);
+
+ FeatureBaseConstPtr firstFeature(const SizeStd& _track_id) const;
+ FeatureBasePtr firstFeature(const SizeStd& _track_id);
+ FeatureBaseConstPtr lastFeature (const SizeStd& _track_id) const;
+ FeatureBasePtr lastFeature (const SizeStd& _track_id);
+ FeatureBaseConstPtr feature (const SizeStd& _track_id, CaptureBaseConstPtr _cap) const;
+ FeatureBasePtr feature (const SizeStd& _track_id, CaptureBasePtr _cap);
+ CaptureBaseConstPtr firstCapture(const SizeStd& _track_id) const;
+ CaptureBasePtr firstCapture(const SizeStd& _track_id);
+
+ list<SizeStd> trackIds(CaptureBaseConstPtr _capture = nullptr) const;
// tracks across captures that belong to keyframe
- TrackConst trackAtKeyframes(const SizeStd& _track_id) const;
- Track trackAtKeyframes(const SizeStd& _track_id);
-
- const map<SizeStd, Track>& getTracks() {return tracks_;}
- const map<CaptureBasePtr, Snapshot >& getSnapshots() {return snapshots_;}
+ TrackConst trackAtKeyframes(const SizeStd& _track_id) const;
+ Track trackAtKeyframes(const SizeStd& _track_id);
private:
diff --git a/src/processor/track_matrix.cpp b/src/processor/track_matrix.cpp
index de101750edfe66eec72fae0c3be295164e8b7d45..ed4215d12e13403c135b433c440f7f1493612bca 100644
--- a/src/processor/track_matrix.cpp
+++ b/src/processor/track_matrix.cpp
@@ -369,12 +369,26 @@ Track TrackMatrix::trackAtKeyframes(const SizeStd& _track_id)
return Track();
}
-list<SizeStd> TrackMatrix::trackIds() const
+list<SizeStd> TrackMatrix::trackIds(CaptureBaseConstPtr _capture) const
{
list<SizeStd> track_ids;
- for (auto track : tracks_)
+
+ if (not _capture)
+ for (auto track_pair : tracks_)
+ track_ids.push_back(track_pair.first);
+ else
{
- track_ids.push_back(track.first);
+ auto it = std::find_if(snapshots_.begin(),
+ snapshots_.end(),
+ [_capture](const std::pair<CaptureBasePtr, Snapshot>& pair)
+ {
+ return pair.first == _capture;
+ }
+ );
+
+ if (it != snapshots_.end())
+ for (auto track_pair : it->second)
+ track_ids.push_back(track_pair.first);
}
return track_ids;
}
diff --git a/test/gtest_track_matrix.cpp b/test/gtest_track_matrix.cpp
index 130a3495c8469a0b4dbcecb8ebbc68b55412835c..aabfada74ff97166ff7468ffe48cd8c46db4034f 100644
--- a/test/gtest_track_matrix.cpp
+++ b/test/gtest_track_matrix.cpp
@@ -42,7 +42,7 @@ class TrackMatrixTest : public testing::Test
FrameBasePtr F0, F1, F2, F3, F4;
CaptureBasePtr C0, C1, C2, C3, C4;
- FeatureBasePtr f0, f1, f2, f3, f4;
+ FeatureBasePtr f0, f1, f2, f3, f4, f5;
ProblemPtr problem;
void SetUp() override
@@ -71,6 +71,7 @@ class TrackMatrixTest : public testing::Test
f2 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov);
f3 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov);
f4 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov);
+ f5 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov);
// F0 and F4 are keyframes
F0->link(problem);
@@ -507,6 +508,37 @@ TEST_F(TrackMatrixTest, trackAtKeyframes)
ASSERT_EQ(trk_kf_1.size(), 0);
}
+TEST_F(TrackMatrixTest, trackIds)
+{
+ f0->link(C0);
+ f1->link(C1);
+ f2->link(C1);
+ f3->link(C1);
+ f4->link(C2);
+ f5->link(C2);
+
+ track_matrix.newTrack(f0);
+ track_matrix.add(f0->trackId(), f1);
+ track_matrix.newTrack(f2);
+ track_matrix.newTrack(f3);
+ track_matrix.add(f0->trackId(), f4);
+ track_matrix.add(f2->trackId(), f5);
+
+ /* KC0 C1 C2
+ *
+ * f0---f1---f4 trk 0
+ * | |
+ * f2---f5 trk 1
+ * |
+ * f3 trk 2
+ */
+
+ ASSERT_EQ(track_matrix.trackIds().size(), 3);
+ ASSERT_EQ(track_matrix.trackIds(C0).size(), 1);
+ ASSERT_EQ(track_matrix.trackIds(C1).size(), 3);
+ ASSERT_EQ(track_matrix.trackIds(C2).size(), 2);
+}
+
int main(int argc, char **argv)
{