Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • mobile_robotics/wolf_projects/wolf_lib/wolf
1 result
Show changes
Commits on Source (3)
Hide whitespace changes
Inline Side-by-side
include/core/common/factory.h
View file @ 627f138e
......@@ -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
......
include/core/processor/track_matrix.h
View file @ 627f138e
......@@ -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:
......
src/processor/track_matrix.cpp
View file @ 627f138e
......@@ -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;
}
......
test/gtest_track_matrix.cpp
View file @ 627f138e
......@@ -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)
{
......