diff --git a/include/core/processor/track_matrix.h b/include/core/processor/track_matrix.h
index 87511440e721baf20d2b3aa05123f0499caa4c84..215f02060b94a4f3429c4f042a78a94949f2272c 100644
--- a/include/core/processor/track_matrix.h
+++ b/include/core/processor/track_matrix.h
@@ -45,9 +45,12 @@ using std::list;
using std::pair;
using std::shared_ptr;
-typedef map<TimeStamp, FeatureBasePtr> Track;
-typedef map<size_t, FeatureBasePtr > Snapshot;
-typedef map<size_t, pair<FeatureBasePtr, FeatureBasePtr> > TrackMatches; // matched feature pairs indexed by track_id
+typedef map<TimeStamp, FeatureBasePtr> Track;
+typedef map<TimeStamp, FeatureBaseConstPtr> TrackConst;
+typedef map<size_t, FeatureBasePtr > Snapshot;
+typedef map<size_t, FeatureBaseConstPtr > SnapshotConst;
+typedef map<size_t, pair<FeatureBasePtr, FeatureBasePtr> > TrackMatches; // matched feature pairs indexed by track_id
+typedef map<size_t, pair<FeatureBaseConstPtr, FeatureBaseConstPtr> > TrackMatchesConst; // matched feature pairs indexed by track_id
/** \brief Matrix of tracked features, by track and by snapshot (Captures or time stamps)
* This class implements the following data structure:
@@ -107,21 +110,29 @@ class TrackMatrix
void remove (CaptureBasePtr _cap);
SizeStd numTracks () const;
SizeStd trackSize (const SizeStd& _track_id) const;
- Track track (const SizeStd& _track_id) const;
- Snapshot snapshot (CaptureBasePtr _capture) const;
- vector<FeatureBasePtr>
- trackAsVector(const SizeStd& _track_id) const;
- list<FeatureBasePtr>
- snapshotAsList(CaptureBasePtr _cap) const;
- TrackMatches matches (CaptureBasePtr _cap_1, CaptureBasePtr _cap_2) const;
- FeatureBasePtr firstFeature(const SizeStd& _track_id) const;
- FeatureBasePtr lastFeature (const SizeStd& _track_id) const;
- FeatureBasePtr feature (const SizeStd& _track_id, CaptureBasePtr _cap) const;
- CaptureBasePtr firstCapture(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);
// tracks across captures that belong to keyframe
// SizeStd numKeyframeTracks();
- Track trackAtKeyframes(size_t _track_id) const;
+ TrackConst trackAtKeyframes(size_t _track_id) const;
+ Track trackAtKeyframes(size_t _track_id);
// bool markKeyframe(CaptureBasePtr _capture);
// bool unmarkKeyframe(CaptureBasePtr _capture);
@@ -131,11 +142,13 @@ class TrackMatrix
// tracks across all Captures
map<SizeStd, Track > tracks_; // map indexed by track_Id of ( maps indexed by TimeStamp of ( features ) )
+ map<SizeStd, TrackConst > tracks_const_; // 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<CaptureBasePtr, Snapshot > snapshots_; // map indexed by capture_ptr of ( maps indexed by track_Id of ( features ) )
+ map<CaptureBaseConstPtr, SnapshotConst > snapshots_const_; // map indexed by capture_ptr of ( maps indexed by track_Id of ( features ) )
};
} /* namespace wolf */
diff --git a/src/processor/track_matrix.cpp b/src/processor/track_matrix.cpp
index 1830270b13d7fea4d6462f2c97c7977b0879308f..6f6e02c3bc841293f264c880cd037a377efd1bfc 100644
--- a/src/processor/track_matrix.cpp
+++ b/src/processor/track_matrix.cpp
@@ -43,7 +43,15 @@ TrackMatrix::~TrackMatrix()
//
}
-Track TrackMatrix::track(const SizeStd& _track_id) const
+TrackConst TrackMatrix::track(const SizeStd& _track_id) const
+{
+ if (tracks_const_.count(_track_id) > 0)
+ return tracks_const_.at(_track_id);
+ else
+ return TrackConst();
+}
+
+Track TrackMatrix::track(const SizeStd& _track_id)
{
if (tracks_.count(_track_id) > 0)
return tracks_.at(_track_id);
@@ -51,7 +59,15 @@ Track TrackMatrix::track(const SizeStd& _track_id) const
return Track();
}
-Snapshot TrackMatrix::snapshot(CaptureBasePtr _capture) const
+SnapshotConst TrackMatrix::snapshot(CaptureBaseConstPtr _capture) const
+{
+ if (_capture && snapshots_const_.count(_capture) > 0)
+ return snapshots_const_.at(_capture);
+ else
+ return SnapshotConst();
+}
+
+Snapshot TrackMatrix::snapshot(CaptureBasePtr _capture)
{
if (_capture && snapshots_.count(_capture) > 0)
return snapshots_.at(_capture);
@@ -72,7 +88,9 @@ void TrackMatrix::add(const SizeStd& _track_id, const FeatureBasePtr& _ftr)
_ftr->setTrackId(_track_id);
tracks_[_track_id].emplace(_ftr->getCapture()->getTimeStamp(), _ftr);
+ tracks_const_[_track_id].emplace(_ftr->getCapture()->getTimeStamp(), _ftr);
snapshots_[_ftr->getCapture()].emplace(_track_id, _ftr); // will create new snapshot if _cap_id is not present
+ snapshots_const_[_ftr->getCapture()].emplace(_track_id, _ftr); // will create new snapshot if _cap_id is not present
}
void TrackMatrix::add(const FeatureBasePtr& _ftr_existing,const FeatureBasePtr& _ftr_new)
@@ -89,13 +107,16 @@ void TrackMatrix::remove(const SizeStd& _track_id)
{
CaptureBasePtr cap = pair_time_ftr.second->getCapture();
snapshots_.at(cap).erase(_track_id);
+ snapshots_const_.at(cap).erase(_track_id);
if (snapshots_.at(cap).empty())
snapshots_.erase(cap);
-
+ if (snapshots_const_.at(cap).empty())
+ snapshots_const_.erase(cap);
}
// Remove track
tracks_.erase(_track_id);
+ tracks_const_.erase(_track_id);
}
}
@@ -109,12 +130,16 @@ void TrackMatrix::remove(CaptureBasePtr _cap)
{
SizeStd trk_id = pair_trkid_ftr.first;
tracks_.at(trk_id).erase(ts);
+ tracks_const_.at(trk_id).erase(ts);
if (tracks_.at(trk_id).empty())
tracks_.erase(trk_id);
+ if (tracks_const_.at(trk_id).empty())
+ tracks_const_.erase(trk_id);
}
// remove snapshot
snapshots_.erase(_cap);
+ snapshots_const_.erase(_cap);
}
}
@@ -127,15 +152,21 @@ void TrackMatrix::remove(FeatureBasePtr _ftr)
{
if(tracks_.count(_ftr->trackId()) && tracks_.at(_ftr->trackId()).count(cap->getTimeStamp()))
{
- tracks_ .at(_ftr->trackId()).erase(cap->getTimeStamp());
+ tracks_ .at(_ftr->trackId()).erase(cap->getTimeStamp());
+ tracks_const_.at(_ftr->trackId()).erase(cap->getTimeStamp());
if (tracks_.at(_ftr->trackId()).empty())
tracks_.erase(_ftr->trackId());
+ if (tracks_const_.at(_ftr->trackId()).empty())
+ tracks_const_.erase(_ftr->trackId());
}
if(snapshots_.count(cap) && snapshots_.at(cap).count(_ftr->trackId()))
{
- snapshots_. at(cap).erase(_ftr->trackId());
+ snapshots_ .at(cap).erase(_ftr->trackId());
+ snapshots_const_.at(cap).erase(_ftr->trackId());
if (snapshots_.at(cap).empty())
snapshots_.erase(cap);
+ if (snapshots_const_.at(cap).empty())
+ snapshots_const_.erase(cap);
}
}
}
@@ -151,7 +182,15 @@ size_t TrackMatrix::trackSize(const SizeStd& _track_id) const
return track(_track_id).size();
}
-FeatureBasePtr TrackMatrix::firstFeature(const SizeStd& _track_id) const
+FeatureBaseConstPtr TrackMatrix::firstFeature(const SizeStd& _track_id) const
+{
+ if (tracks_const_.count(_track_id) > 0)
+ return tracks_const_.at(_track_id).begin()->second;
+ else
+ return nullptr;
+}
+
+FeatureBasePtr TrackMatrix::firstFeature(const SizeStd& _track_id)
{
if (tracks_.count(_track_id) > 0)
return tracks_.at(_track_id).begin()->second;
@@ -159,7 +198,15 @@ FeatureBasePtr TrackMatrix::firstFeature(const SizeStd& _track_id) const
return nullptr;
}
-FeatureBasePtr TrackMatrix::lastFeature(const SizeStd& _track_id) const
+FeatureBaseConstPtr TrackMatrix::lastFeature(const SizeStd& _track_id) const
+{
+ if (tracks_const_.count(_track_id) > 0)
+ return tracks_const_.at(_track_id).rbegin()->second;
+ else
+ return nullptr;
+}
+
+FeatureBasePtr TrackMatrix::lastFeature(const SizeStd& _track_id)
{
if (tracks_.count(_track_id) > 0)
return tracks_.at(_track_id).rbegin()->second;
@@ -167,7 +214,19 @@ FeatureBasePtr TrackMatrix::lastFeature(const SizeStd& _track_id) const
return nullptr;
}
-vector<FeatureBasePtr> TrackMatrix::trackAsVector(const SizeStd& _track_id) const
+vector<FeatureBaseConstPtr> TrackMatrix::trackAsVector(const SizeStd& _track_id) const
+{
+ vector<FeatureBaseConstPtr> vec;
+ if (tracks_.count(_track_id))
+ {
+ vec.reserve(trackSize(_track_id));
+ for (auto const& pair_time_ftr : tracks_const_.at(_track_id))
+ vec.push_back(pair_time_ftr.second);
+ }
+ return vec;
+}
+
+vector<FeatureBasePtr> TrackMatrix::trackAsVector(const SizeStd& _track_id)
{
vector<FeatureBasePtr> vec;
if (tracks_.count(_track_id))
@@ -179,23 +238,56 @@ vector<FeatureBasePtr> TrackMatrix::trackAsVector(const SizeStd& _track_id) cons
return vec;
}
-std::list<FeatureBasePtr> TrackMatrix::snapshotAsList(CaptureBasePtr _cap) const
+FeatureBaseConstPtrList TrackMatrix::snapshotAsList(CaptureBaseConstPtr _cap) const
{
- std::list<FeatureBasePtr> lst;
+ FeatureBaseConstPtrList lst;
+ if (snapshots_const_.count(_cap))
+ for (auto const& pair_trkid_ftr : snapshots_const_.at(_cap))
+ lst.push_back(pair_trkid_ftr.second);
+ return lst;
+}
+
+FeatureBasePtrList TrackMatrix::snapshotAsList(CaptureBasePtr _cap)
+{
+ FeatureBasePtrList lst;
if (snapshots_.count(_cap))
for (auto const& pair_trkid_ftr : snapshots_.at(_cap))
lst.push_back(pair_trkid_ftr.second);
return lst;
}
-TrackMatches TrackMatrix::matches(CaptureBasePtr _cap_1, CaptureBasePtr _cap_2) const
+TrackMatchesConst TrackMatrix::matches(CaptureBaseConstPtr _cap_1, CaptureBaseConstPtr _cap_2) const
+{
+ TrackMatchesConst pairs;
+
+ auto s_1 = snapshot(_cap_1);
+ auto s_2 = snapshot(_cap_2);
+ auto s_short = s_1;
+ auto s_long = s_2;
+ if (s_1.size() > s_2.size())
+ {
+ s_long = s_1;
+ s_short = s_2;
+ }
+
+ for (auto const & pair_trkid_ftr : s_short)
+ {
+ SizeStd trk_id = pair_trkid_ftr.first;
+ if (s_long.count(trk_id))
+ pairs[trk_id] = pair<FeatureBaseConstPtr, FeatureBaseConstPtr>(s_1.at(trk_id), s_2.at(trk_id));
+ }
+
+ return pairs;
+}
+
+TrackMatches TrackMatrix::matches(CaptureBasePtr _cap_1, CaptureBasePtr _cap_2)
{
TrackMatches pairs;
- Snapshot s_1 = snapshot(_cap_1);
- Snapshot s_2 = snapshot(_cap_2);
- Snapshot s_short = s_1;
- Snapshot s_long = s_2;
+ auto s_1 = snapshot(_cap_1);
+ auto s_2 = snapshot(_cap_2);
+ auto s_short = s_1;
+ auto s_long = s_2;
if (s_1.size() > s_2.size())
{
s_long = s_1;
@@ -212,7 +304,7 @@ TrackMatches TrackMatrix::matches(CaptureBasePtr _cap_1, CaptureBasePtr _cap_2)
return pairs;
}
-FeatureBasePtr TrackMatrix::feature(const SizeStd& _track_id, CaptureBasePtr _cap) const
+FeatureBaseConstPtr TrackMatrix::feature(const SizeStd& _track_id, CaptureBaseConstPtr _cap) const
{
if (snapshot(_cap).count(_track_id))
return snapshot(_cap).at(_track_id);
@@ -220,12 +312,44 @@ FeatureBasePtr TrackMatrix::feature(const SizeStd& _track_id, CaptureBasePtr _ca
return nullptr;
}
-CaptureBasePtr TrackMatrix::firstCapture(const SizeStd& _track_id) const
+FeatureBasePtr TrackMatrix::feature(const SizeStd& _track_id, CaptureBasePtr _cap)
+{
+ if (snapshot(_cap).count(_track_id))
+ return snapshot(_cap).at(_track_id);
+ else
+ return nullptr;
+}
+
+CaptureBaseConstPtr TrackMatrix::firstCapture(const SizeStd& _track_id) const
+{
+ return firstFeature(_track_id)->getCapture();
+}
+
+CaptureBasePtr TrackMatrix::firstCapture(const SizeStd& _track_id)
{
return firstFeature(_track_id)->getCapture();
}
-Track TrackMatrix::trackAtKeyframes(size_t _track_id) const
+TrackConst TrackMatrix::trackAtKeyframes(size_t _track_id) const
+{
+ // We assemble a track_kf on the fly by checking each capture's frame.
+ if (tracks_.count(_track_id))
+ {
+ TrackConst 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()->getProblem())
+ track_kf[ts] = ftr;
+ }
+ return track_kf;
+ }
+ else
+ return TrackConst();
+}
+
+Track TrackMatrix::trackAtKeyframes(size_t _track_id)
{
// We assemble a track_kf on the fly by checking each capture's frame.
if (tracks_.count(_track_id))