Skip to content
Snippets Groups Projects
Commit 9e126f62 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

wip trackmatrix

parent 08ecf08e
No related branches found
No related tags found
1 merge request!443Resolve "Work on const / non-const in wolf base classes"
Pipeline #9715 failed
...@@ -45,9 +45,12 @@ using std::list; ...@@ -45,9 +45,12 @@ using std::list;
using std::pair; using std::pair;
using std::shared_ptr; using std::shared_ptr;
typedef map<TimeStamp, FeatureBasePtr> Track; typedef map<TimeStamp, FeatureBasePtr> Track;
typedef map<size_t, FeatureBasePtr > Snapshot; typedef map<TimeStamp, FeatureBaseConstPtr> TrackConst;
typedef map<size_t, pair<FeatureBasePtr, FeatureBasePtr> > TrackMatches; // matched feature pairs indexed by track_id 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) /** \brief Matrix of tracked features, by track and by snapshot (Captures or time stamps)
* This class implements the following data structure: * This class implements the following data structure:
...@@ -107,21 +110,29 @@ class TrackMatrix ...@@ -107,21 +110,29 @@ class TrackMatrix
void remove (CaptureBasePtr _cap); void remove (CaptureBasePtr _cap);
SizeStd numTracks () const; SizeStd numTracks () const;
SizeStd trackSize (const SizeStd& _track_id) const; SizeStd trackSize (const SizeStd& _track_id) const;
Track track (const SizeStd& _track_id) const; TrackConst track (const SizeStd& _track_id) const;
Snapshot snapshot (CaptureBasePtr _capture) const; Track track (const SizeStd& _track_id);
vector<FeatureBasePtr> SnapshotConst snapshot (CaptureBaseConstPtr _capture) const;
trackAsVector(const SizeStd& _track_id) const; Snapshot snapshot (CaptureBasePtr _capture);
list<FeatureBasePtr> vector<FeatureBaseConstPtr> trackAsVector(const SizeStd& _track_id) const;
snapshotAsList(CaptureBasePtr _cap) const; vector<FeatureBasePtr> trackAsVector(const SizeStd& _track_id);
TrackMatches matches (CaptureBasePtr _cap_1, CaptureBasePtr _cap_2) const; FeatureBaseConstPtrList snapshotAsList(CaptureBaseConstPtr _cap) const;
FeatureBasePtr firstFeature(const SizeStd& _track_id) const; FeatureBasePtrList snapshotAsList(CaptureBasePtr _cap);
FeatureBasePtr lastFeature (const SizeStd& _track_id) const; TrackMatchesConst matches (CaptureBaseConstPtr _cap_1, CaptureBaseConstPtr _cap_2) const;
FeatureBasePtr feature (const SizeStd& _track_id, CaptureBasePtr _cap) const; TrackMatches matches (CaptureBasePtr _cap_1, CaptureBasePtr _cap_2);
CaptureBasePtr firstCapture(const SizeStd& _track_id) const; 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 // tracks across captures that belong to keyframe
// SizeStd numKeyframeTracks(); // 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 markKeyframe(CaptureBasePtr _capture);
// bool unmarkKeyframe(CaptureBasePtr _capture); // bool unmarkKeyframe(CaptureBasePtr _capture);
...@@ -131,11 +142,13 @@ class TrackMatrix ...@@ -131,11 +142,13 @@ class TrackMatrix
// tracks across all Captures // tracks across all Captures
map<SizeStd, Track > tracks_; // map indexed by track_Id of ( maps indexed by TimeStamp of ( features ) ) 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 // // 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 ) ) // 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. // 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<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 */ } /* namespace wolf */
......
...@@ -43,7 +43,15 @@ TrackMatrix::~TrackMatrix() ...@@ -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) if (tracks_.count(_track_id) > 0)
return tracks_.at(_track_id); return tracks_.at(_track_id);
...@@ -51,7 +59,15 @@ Track TrackMatrix::track(const SizeStd& _track_id) const ...@@ -51,7 +59,15 @@ Track TrackMatrix::track(const SizeStd& _track_id) const
return Track(); 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) if (_capture && snapshots_.count(_capture) > 0)
return snapshots_.at(_capture); return snapshots_.at(_capture);
...@@ -72,7 +88,9 @@ void TrackMatrix::add(const SizeStd& _track_id, const FeatureBasePtr& _ftr) ...@@ -72,7 +88,9 @@ void TrackMatrix::add(const SizeStd& _track_id, const FeatureBasePtr& _ftr)
_ftr->setTrackId(_track_id); _ftr->setTrackId(_track_id);
tracks_[_track_id].emplace(_ftr->getCapture()->getTimeStamp(), _ftr); 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_[_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) void TrackMatrix::add(const FeatureBasePtr& _ftr_existing,const FeatureBasePtr& _ftr_new)
...@@ -89,13 +107,16 @@ void TrackMatrix::remove(const SizeStd& _track_id) ...@@ -89,13 +107,16 @@ void TrackMatrix::remove(const SizeStd& _track_id)
{ {
CaptureBasePtr cap = pair_time_ftr.second->getCapture(); CaptureBasePtr cap = pair_time_ftr.second->getCapture();
snapshots_.at(cap).erase(_track_id); snapshots_.at(cap).erase(_track_id);
snapshots_const_.at(cap).erase(_track_id);
if (snapshots_.at(cap).empty()) if (snapshots_.at(cap).empty())
snapshots_.erase(cap); snapshots_.erase(cap);
if (snapshots_const_.at(cap).empty())
snapshots_const_.erase(cap);
} }
// Remove track // Remove track
tracks_.erase(_track_id); tracks_.erase(_track_id);
tracks_const_.erase(_track_id);
} }
} }
...@@ -109,12 +130,16 @@ void TrackMatrix::remove(CaptureBasePtr _cap) ...@@ -109,12 +130,16 @@ void TrackMatrix::remove(CaptureBasePtr _cap)
{ {
SizeStd trk_id = pair_trkid_ftr.first; SizeStd trk_id = pair_trkid_ftr.first;
tracks_.at(trk_id).erase(ts); tracks_.at(trk_id).erase(ts);
tracks_const_.at(trk_id).erase(ts);
if (tracks_.at(trk_id).empty()) if (tracks_.at(trk_id).empty())
tracks_.erase(trk_id); tracks_.erase(trk_id);
if (tracks_const_.at(trk_id).empty())
tracks_const_.erase(trk_id);
} }
// remove snapshot // remove snapshot
snapshots_.erase(_cap); snapshots_.erase(_cap);
snapshots_const_.erase(_cap);
} }
} }
...@@ -127,15 +152,21 @@ void TrackMatrix::remove(FeatureBasePtr _ftr) ...@@ -127,15 +152,21 @@ void TrackMatrix::remove(FeatureBasePtr _ftr)
{ {
if(tracks_.count(_ftr->trackId()) && tracks_.at(_ftr->trackId()).count(cap->getTimeStamp())) 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()) if (tracks_.at(_ftr->trackId()).empty())
tracks_.erase(_ftr->trackId()); 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())) 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()) if (snapshots_.at(cap).empty())
snapshots_.erase(cap); 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 ...@@ -151,7 +182,15 @@ size_t TrackMatrix::trackSize(const SizeStd& _track_id) const
return track(_track_id).size(); 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) if (tracks_.count(_track_id) > 0)
return tracks_.at(_track_id).begin()->second; return tracks_.at(_track_id).begin()->second;
...@@ -159,7 +198,15 @@ FeatureBasePtr TrackMatrix::firstFeature(const SizeStd& _track_id) const ...@@ -159,7 +198,15 @@ FeatureBasePtr TrackMatrix::firstFeature(const SizeStd& _track_id) const
return nullptr; 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) if (tracks_.count(_track_id) > 0)
return tracks_.at(_track_id).rbegin()->second; return tracks_.at(_track_id).rbegin()->second;
...@@ -167,7 +214,19 @@ FeatureBasePtr TrackMatrix::lastFeature(const SizeStd& _track_id) const ...@@ -167,7 +214,19 @@ FeatureBasePtr TrackMatrix::lastFeature(const SizeStd& _track_id) const
return nullptr; 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; vector<FeatureBasePtr> vec;
if (tracks_.count(_track_id)) if (tracks_.count(_track_id))
...@@ -179,23 +238,56 @@ vector<FeatureBasePtr> TrackMatrix::trackAsVector(const SizeStd& _track_id) cons ...@@ -179,23 +238,56 @@ vector<FeatureBasePtr> TrackMatrix::trackAsVector(const SizeStd& _track_id) cons
return vec; 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)) if (snapshots_.count(_cap))
for (auto const& pair_trkid_ftr : snapshots_.at(_cap)) for (auto const& pair_trkid_ftr : snapshots_.at(_cap))
lst.push_back(pair_trkid_ftr.second); lst.push_back(pair_trkid_ftr.second);
return lst; 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; TrackMatches pairs;
Snapshot s_1 = snapshot(_cap_1); auto s_1 = snapshot(_cap_1);
Snapshot s_2 = snapshot(_cap_2); auto s_2 = snapshot(_cap_2);
Snapshot s_short = s_1; auto s_short = s_1;
Snapshot s_long = s_2; auto s_long = s_2;
if (s_1.size() > s_2.size()) if (s_1.size() > s_2.size())
{ {
s_long = s_1; s_long = s_1;
...@@ -212,7 +304,7 @@ TrackMatches TrackMatrix::matches(CaptureBasePtr _cap_1, CaptureBasePtr _cap_2) ...@@ -212,7 +304,7 @@ TrackMatches TrackMatrix::matches(CaptureBasePtr _cap_1, CaptureBasePtr _cap_2)
return pairs; 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)) if (snapshot(_cap).count(_track_id))
return snapshot(_cap).at(_track_id); return snapshot(_cap).at(_track_id);
...@@ -220,12 +312,44 @@ FeatureBasePtr TrackMatrix::feature(const SizeStd& _track_id, CaptureBasePtr _ca ...@@ -220,12 +312,44 @@ FeatureBasePtr TrackMatrix::feature(const SizeStd& _track_id, CaptureBasePtr _ca
return nullptr; 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(); 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. // We assemble a track_kf on the fly by checking each capture's frame.
if (tracks_.count(_track_id)) if (tracks_.count(_track_id))
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment