From b23f6c99867d5fa321f32d1e42e7f8f3115fa368 Mon Sep 17 00:00:00 2001 From: Xiaohan Fei Date: Thu, 5 Sep 2019 21:35:28 -0700 Subject: [PATCH] make the visibility graph a singleton --- src/estimator.cpp | 18 +++++++-- src/estimator.h | 2 +- src/graph.cpp | 17 ++++++++ src/graph.h | 10 ++++- src/manager.cpp | 77 +++++++++++++++++++------------------ src/test/test_optimizer.cpp | 2 - src/update.cpp | 2 +- 7 files changed, 82 insertions(+), 46 deletions(-) diff --git a/src/estimator.cpp b/src/estimator.cpp index fb3568b8..444d2f64 100644 --- a/src/estimator.cpp +++ b/src/estimator.cpp @@ -263,12 +263,16 @@ Estimator::Estimator(const Json::Value &cfg) Qimu_ *= Qimu_; LOG(INFO) << "Covariance of IMU measurement noise loaded"; + // ///////////////////////////// // initialize memory manager + // ///////////////////////////// MemoryManager::Create(cfg_["memory"].get("max_features", 256).asInt(), cfg_["memory"].get("max_groups", 128).asInt()); LOG(INFO) << "Memory management unit created"; + // ///////////////////////////// // initialize tracker + // ///////////////////////////// auto tracker_cfg = cfg_["tracker_cfg"].isString() ? LoadJson(cfg_["tracker_cfg"].asString()) : cfg_["tracker_cfg"]; @@ -283,6 +287,11 @@ Estimator::Estimator(const Json::Value &cfg) LOG(INFO) << "R=" << R_ << " ;Roos=" << Roos_; + // ///////////////////////////// + // Initialize the visibility graph + // ///////////////////////////// + Graph::Create(); + // ///////////////////////////// // Load initial std on feature state // ///////////////////////////// @@ -972,9 +981,10 @@ std::tuple Estimator::HuberOnInnovation(const Vec2 &inn, std::vector Estimator::DiscardGroups(const std::vector &discards) { std::vector nullref_features; + Graph& graph{*Graph::instance()}; for (auto g : discards) { // transfer ownership of the remaining features whose reference is this one - auto failed = graph_.TransferFeatureOwnership(g, gbc()); + auto failed = graph.TransferFeatureOwnership(g, gbc()); nullref_features.insert(nullref_features.end(), failed.begin(), failed.end()); @@ -983,7 +993,7 @@ Estimator::DiscardGroups(const std::vector &discards) { gauge_group_ = -1; } - graph_.RemoveGroup(g); + graph.RemoveGroup(g); if (g->instate()) { RemoveGroupFromState(g); } @@ -994,7 +1004,7 @@ Estimator::DiscardGroups(const std::vector &discards) { } void Estimator::DiscardFeatures(const std::vector &discards) { - graph_.RemoveFeatures(discards); + Graph::instance()->RemoveFeatures(discards); for (auto f : discards) { if (f->instate()) { RemoveFeatureFromState(f); @@ -1005,7 +1015,7 @@ void Estimator::DiscardFeatures(const std::vector &discards) { void Estimator::SwitchRefGroup() { auto candidates = - graph_.GetGroupsIf([](GroupPtr g) -> bool { return g->instate(); }); + Graph::instance()->GetGroupsIf([](GroupPtr g) -> bool { return g->instate(); }); if (!candidates.empty()) { // FIXME: in addition to the variance, also take account of the number of // instate features diff --git a/src/estimator.h b/src/estimator.h index ff5824fc..7057b91a 100644 --- a/src/estimator.h +++ b/src/estimator.h @@ -150,7 +150,7 @@ class Estimator : public Component { std::vector instate_features_; // in-state features std::vector oos_features_; // out-of-state features std::vector instate_groups_; // in-state groups - Graph graph_; + // Graph graph_; int gauge_group_; // index of the selected gauge group, -1 for none private: diff --git a/src/graph.cpp b/src/graph.cpp index 5ec8756c..adbf78dd 100644 --- a/src/graph.cpp +++ b/src/graph.cpp @@ -10,6 +10,23 @@ void FeatureAdj::Remove(int id) { erase(id); } void GroupAdj::Add(int id) { insert(id); } void GroupAdj::Remove(int id) { erase(id); } +std::unique_ptr Graph::instance_ = nullptr; + +Graph* Graph::Create() { + if (instance_ == nullptr) { + instance_ = std::unique_ptr(new Graph); + } + return instance_.get(); +} + +Graph* Graph::instance() { + if (instance_ == nullptr) { + LOG(WARNING) << "Graph not created yet! Creating one ..."; + Graph::Create(); + } + return instance_.get(); +} + bool Graph::HasGroup(GroupPtr g) const { return HasGroup(g->id()); } bool Graph::HasGroup(int gid) const { diff --git a/src/graph.h b/src/graph.h index a0c789f8..0b5768b5 100644 --- a/src/graph.h +++ b/src/graph.h @@ -1,4 +1,4 @@ -// The feature-group graph. +// The feature-group visibility graph. // Author: Xiaohan Fei (feixh@cs.ucla.edu) #pragma once #include @@ -23,6 +23,9 @@ struct GroupAdj : public std::unordered_set { class Graph { public: + static Graph* Create(); + static Graph* instance(); + void RemoveFeature(const FeaturePtr f); void RemoveFeatures(const std::vector &); @@ -68,6 +71,11 @@ class Graph { void CleanIsolatedNodes(); private: + Graph() = default; + Graph(const Graph&) = delete; + Graph& operator=(const Graph&) = delete; + static std::unique_ptr instance_; + // 2 types of nodes: feature and group std::unordered_map features_; std::unordered_map groups_; diff --git a/src/manager.cpp b/src/manager.cpp index bdc65ee7..5f5e906e 100644 --- a/src/manager.cpp +++ b/src/manager.cpp @@ -24,11 +24,14 @@ void Estimator::ProcessTracks(const timestamp_t &ts, oos_features_.clear(); instate_groups_.clear(); + // retrieve the visibility graph + Graph& graph{*Graph::instance()}; + // increment lifetime of all features and groups - for (auto f : graph_.GetFeatures()) { + for (auto f : graph.GetFeatures()) { f->IncrementLifetime(); } - for (auto g : graph_.GetGroups()) { + for (auto g : graph.GetGroups()) { g->IncrementLifetime(); } @@ -51,7 +54,7 @@ void Estimator::ProcessTracks(const timestamp_t &ts, it = tracks.erase(it); } else if ((f->instate() && f->track_status() == TrackStatus::DROPPED) || f->track_status() == TrackStatus::REJECTED) { - graph_.RemoveFeature(f); + graph.RemoveFeature(f); if (f->instate()) { RemoveFeatureFromState(f); affected_groups.insert(f->ref()); @@ -65,7 +68,7 @@ void Estimator::ProcessTracks(const timestamp_t &ts, oos_features_.push_back(f); } else { // just remove - graph_.RemoveFeature(f); + graph.RemoveFeature(f); Feature::Delete(f); } it = tracks.erase(it); @@ -91,7 +94,7 @@ void Estimator::ProcessTracks(const timestamp_t &ts, f->SubfilterUpdate(gsb(), gbc(), subfilter_options_); // std::cout << "outlier score=" << f->outlier_counter() << std::endl; if (f->outlier_counter() > 10) { - graph_.RemoveFeature(f); + graph.RemoveFeature(f); Feature::Delete(f); it = tracks.erase(it); } else { @@ -103,7 +106,7 @@ void Estimator::ProcessTracks(const timestamp_t &ts, // remaining in tracks: just created (not in graph yet) and being tracked well // (may or may not be in graph, for those in graph, may or may not in state) - instate_features_ = graph_.GetFeaturesIf([](FeaturePtr f) -> bool { + instate_features_ = graph.GetFeaturesIf([](FeaturePtr f) -> bool { return f->status() == FeatureStatus::INSTATE; }); @@ -113,7 +116,7 @@ void Estimator::ProcessTracks(const timestamp_t &ts, // choose the instate-candidate criterion auto criterion = vision_counter_ < 5 ? Criteria::Candidate : Criteria::CandidateStrict; - auto candidates = graph_.GetFeaturesIf(criterion); + auto candidates = graph.GetFeaturesIf(criterion); MakePtrVectorUnique(candidates); std::sort(candidates.begin(), candidates.end(), @@ -127,7 +130,7 @@ void Estimator::ProcessTracks(const timestamp_t &ts, auto f = *it; if (use_depth_opt_) { - auto obs = graph_.GetObservationsOf(f); + auto obs = graph.GetObservationsOf(f); if (obs.size() > 1) { if (f->RefineDepth(gbc(), obs, refinement_options_)) { ++it; @@ -154,9 +157,9 @@ void Estimator::ProcessTracks(const timestamp_t &ts, AddFeatureToState(f); // insert f to state vector and covariance if (!f->ref()->instate()) { #ifndef NDEBUG - CHECK(graph_.HasGroup(f->ref())); - CHECK(graph_.GetGroupAdj(f->ref()).count(f->id())); - CHECK(graph_.GetFeatureAdj(f).count(f->ref()->id())); + CHECK(graph.HasGroup(f->ref())); + CHECK(graph.GetGroupAdj(f->ref()).count(f->id())); + CHECK(graph.GetFeatureAdj(f).count(f->ref()->id())); #endif // need to add reference group to state if it's not yet instate AddGroupToState(f->ref()); @@ -172,7 +175,7 @@ void Estimator::ProcessTracks(const timestamp_t &ts, std::vector bad_features; for (auto it = oos_features_.begin(); it != oos_features_.end();) { auto f = *it; - auto obs = graph_.GetObservationsOf(f); + auto obs = graph.GetObservationsOf(f); if (obs.size() > 1 && f->RefineDepth(gbc(), obs, refinement_options_)) { ++it; } else { @@ -190,7 +193,7 @@ void Estimator::ProcessTracks(const timestamp_t &ts, MakePtrVectorUnique(instate_features_); instate_groups_ = - graph_.GetGroupsIf([](GroupPtr g) { return g->instate(); }); + graph.GetGroupsIf([](GroupPtr g) { return g->instate(); }); Update(); } @@ -199,7 +202,7 @@ void Estimator::ProcessTracks(const timestamp_t &ts, #ifndef NDEBUG CHECK(!f->instate()); #endif - graph_.RemoveFeature(f); + graph.RemoveFeature(f); Feature::Delete(f); } @@ -209,7 +212,7 @@ void Estimator::ProcessTracks(const timestamp_t &ts, // 2) detach the feature from the reference group // 3) remove the group if it lost all the instate features - auto rejected_features = graph_.GetFeaturesIf([](FeaturePtr f) -> bool { + auto rejected_features = graph.GetFeaturesIf([](FeaturePtr f) -> bool { return f->status() == FeatureStatus::REJECTED_BY_FILTER; }); // std::cout << "#rejected=" << rejected_features.size() << std::endl; @@ -225,7 +228,7 @@ void Estimator::ProcessTracks(const timestamp_t &ts, #endif affected_groups.insert(f->ref()); } - graph_.RemoveFeatures(rejected_features); + graph.RemoveFeatures(rejected_features); for (auto f : rejected_features) { RemoveFeatureFromState(f); Feature::Delete(f); @@ -238,7 +241,7 @@ void Estimator::ProcessTracks(const timestamp_t &ts, // remove floating groups (with no instate features) and // floating features (not instate and reference group is floating) for (auto g : affected_groups) { - const auto &adj_f = graph_.GetFeaturesOf(g); + const auto &adj_f = graph.GetFeaturesOf(g); if (std::none_of(adj_f.begin(), adj_f.end(), [g](FeaturePtr f) { return f->ref() == g && f->instate(); })) { @@ -249,7 +252,7 @@ void Estimator::ProcessTracks(const timestamp_t &ts, } else { // if not enough slots, remove old instate groups and recycle some spaces std::vector groups = - graph_.GetGroupsIf([](GroupPtr g) { return g->instate(); }); + graph.GetGroupsIf([](GroupPtr g) { return g->instate(); }); if (groups.size() == kMaxGroup) { int oos_discard_step = cfg_.get("oos_discard_step", 3).asInt(); // sort such that oldest groups are at the front of the vector @@ -264,7 +267,7 @@ void Estimator::ProcessTracks(const timestamp_t &ts, // if the group does not have instate features referring back to // itself, auto g = groups[i]; - auto adj_f = graph_.GetFeaturesOf(g); + auto adj_f = graph.GetFeaturesOf(g); if (std::none_of(adj_f.begin(), adj_f.end(), [g](FeaturePtr f) { return f->instate() && f->ref() == g; })) { @@ -283,7 +286,7 @@ void Estimator::ProcessTracks(const timestamp_t &ts, // initialize those newly detected featuers // create a new group and associate newly detected features to the new group GroupPtr g = Group::Create(X_.Rsb, X_.Tsb); - graph_.AddGroup(g); + graph.AddGroup(g); if (use_OOS_) { // In OOS mode, always try to add groups to state. AddGroupToState(g); @@ -302,15 +305,15 @@ void Estimator::ProcessTracks(const timestamp_t &ts, f->SetRef(g); f->Initialize(init_z_, {init_std_x_, init_std_y_, init_std_z_}); - graph_.AddFeature(f); - graph_.AddFeatureToGroup(f, g); - graph_.AddGroupToFeature(g, f); + graph.AddFeature(f); + graph.AddFeatureToGroup(f, g); + graph.AddGroupToFeature(g, f); // put back the detected feature tracks.push_back(f); } - auto tracked_features = graph_.GetFeaturesIf([](FeaturePtr f) -> bool { + auto tracked_features = graph.GetFeaturesIf([](FeaturePtr f) -> bool { return f->track_status() == TrackStatus::TRACKED; }); for (auto f : tracked_features) { @@ -319,15 +322,15 @@ void Estimator::ProcessTracks(const timestamp_t &ts, #endif // attach the new group to all the features being tracked - graph_.AddFeatureToGroup(f, g); - graph_.AddGroupToFeature(g, f); + graph.AddFeatureToGroup(f, g); + graph.AddGroupToFeature(g, f); // put back the tracked feature tracks.push_back(f); } // adapt initial depth to average depth of features currently visible - auto depth_features = graph_.GetFeaturesIf([this](FeaturePtr f) -> bool { + auto depth_features = graph.GetFeaturesIf([this](FeaturePtr f) -> bool { return f->status() == FeatureStatus::INSTATE || (f->status() == FeatureStatus::READY && f->lifetime() > 5); }); @@ -349,13 +352,13 @@ void Estimator::ProcessTracks(const timestamp_t &ts, if (!use_OOS_) { // remove non-reference groups - auto all_groups = graph_.GetGroups(); + auto all_groups = graph.GetGroups(); int max_group_lifetime = cfg_.get("max_group_lifetime", 1).asInt(); for (auto g : all_groups) { if (g->lifetime() > max_group_lifetime) { - const auto &adj = graph_.GetGroupAdj(g); - if (std::none_of(adj.begin(), adj.end(), [this, g](int fid) { - return graph_.GetFeature(fid)->ref() == g; + const auto &adj = graph.GetGroupAdj(g); + if (std::none_of(adj.begin(), adj.end(), [&graph, g](int fid) { + return graph.GetFeature(fid)->ref() == g; })) { // for groups which have no reference features, they cannot be instate // anyway @@ -363,21 +366,21 @@ void Estimator::ProcessTracks(const timestamp_t &ts, CHECK(!g->instate()); #endif - graph_.RemoveGroup(g); + graph.RemoveGroup(g); Group::Delete(g); } } } } - // std::cout << "#groups=" << graph_.GetGroups().size() << std::endl; + // std::cout << "#groups=" << graph.GetGroups().size() << std::endl; // check & clean graph - // graph_.SanityCheck(); + // graph.SanityCheck(); // // remove isolated groups - // auto empty_groups = graph_.GetGroupsIf([this](GroupPtr g)->bool { - // return graph_.GetGroupAdj(g).empty(); }); + // auto empty_groups = graph.GetGroupsIf([this](GroupPtr g)->bool { + // return graph.GetGroupAdj(g).empty(); }); // LOG(INFO) << "#empty groups=" << empty_groups.size(); - // graph_.RemoveGroups(empty_groups); + // graph.RemoveGroups(empty_groups); // for (auto g : empty_groups) { // CHECK(!g->instate()); // Group::Delete(g); diff --git a/src/test/test_optimizer.cpp b/src/test/test_optimizer.cpp index 0f869562..a692e84a 100644 --- a/src/test/test_optimizer.cpp +++ b/src/test/test_optimizer.cpp @@ -74,7 +74,5 @@ int main() { } optimizer->AddFeature(FeatureAdapter{gsb.size() + i, noisy_pt}, obs); } - optimizer->Solve(10); - } diff --git a/src/update.cpp b/src/update.cpp index 056fa35b..30b76589 100644 --- a/src/update.cpp +++ b/src/update.cpp @@ -89,7 +89,7 @@ void Estimator::Update() { // std::vector oos_jacs; // jacobians w.r.t. feature // parametrization for (auto f : oos_features_) { - auto vobs = graph_.GetObservationsOf(f); + auto vobs = Graph::instance()->GetObservationsOf(f); int oos_jac_size = f->ComputeOOSJacobian(vobs, X_.Rbc, X_.Tbc); if (oos_jac_size > 0) { total_oos_jac_size += oos_jac_size;