diff --git a/include/core/common/factory.h b/include/core/common/factory.h index d533ff0da8b627dd0d12ca9a32637956da871af5..1e18985f9a047dea575af0852b24eac6ff6305bd 100644 --- a/include/core/common/factory.h +++ b/include/core/common/factory.h @@ -307,6 +307,8 @@ class Factory static bool unregisterCreator(const std::string& _type); static TypeBasePtr create(const std::string& _type, TypeInput... _input); std::string getClass() const; + static void printAddress(); + static void printCallbacks(); private: CallbackMap callbacks_; @@ -321,9 +323,15 @@ class Factory void operator=(Factory const&) = delete; private: Factory() { } - ~Factory() { } + ~Factory(); }; +template<class TypeBase, typename... TypeInput> +inline Factory<TypeBase, TypeInput...>::~Factory<TypeBase, TypeInput...>() +{ + std::cout << " Factory destructor " << this->getClass() << std::endl; +} + template<class TypeBase, typename... TypeInput> inline bool Factory<TypeBase, TypeInput...>::registerCreator(const std::string& _type, CreatorCallback createFn) { @@ -358,7 +366,7 @@ inline typename Factory<TypeBase, TypeInput...>::TypeBasePtr Factory<TypeBase, T template<class TypeBase, typename... TypeInput> inline Factory<TypeBase, TypeInput...>& Factory<TypeBase, TypeInput...>::get() { - static Factory instance_; + static Factory<TypeBase, TypeInput...> instance_; return instance_; } @@ -368,6 +376,21 @@ inline std::string Factory<TypeBase, TypeInput...>::getClass() const return "Factory<class TypeBase>"; } +template<class TypeBase, typename... TypeInput> +inline void Factory<TypeBase, TypeInput...>::printAddress() +{ + std::cout << get().getClass() << " address: " << &get() << std::endl; +} + +template<class TypeBase, typename... TypeInput> +inline void Factory<TypeBase, TypeInput...>::printCallbacks() +{ + std::cout << get().getClass() << " callbacks size: " << get().callbacks_.size() << std::endl; + for (auto cb: get().callbacks_){ + std::cout << "\t" << cb.first << std::endl; + } +} + } // namespace wolf namespace wolf 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) {