diff --git a/include/hydra/backend/dsg_updater.h b/include/hydra/backend/dsg_updater.h index 1dfed77e..ce3dd613 100644 --- a/include/hydra/backend/dsg_updater.h +++ b/include/hydra/backend/dsg_updater.h @@ -37,20 +37,14 @@ #include #include -#include #include #include -#include -#include -#include "hydra/backend/backend_input.h" -#include "hydra/backend/external_loop_closure_receiver.h" #include "hydra/backend/merge_tracker.h" -#include "hydra/backend/pgmo_configs.h" #include "hydra/common/output_sink.h" #include "hydra/common/shared_dsg_info.h" -#include "hydra/common/shared_module_state.h" #include "hydra/utils/data_directory.h" +#include "hydra/utils/logging.h" namespace hydra { @@ -62,16 +56,20 @@ class DsgUpdater { const kimera_pgmo::DeformationGraph&>; using NodeToRobotMap = std::unordered_map; - struct Config { + struct Config : VerbosityConfig { + using FunctorConfig = config::VirtualConfig; + + Config(); + //! Verbosity controls for functors + VerbosityConfig functor_logging; //! Enable combining multiple nodes together bool enable_node_merging = true; - //! Repeatedly run merge detection until no more merges are detected - bool enable_exhaustive_merging = false; //! If true, reset the private DSG with the unmerged graph on every loop closure bool reset_dsg_on_loop_closure = false; //! Update functors that get applied in the specified order - config::OrderedMap> - update_functors; + config::OrderedMap update_functors; + //! Names of functors to use exhaustive merging for + std::vector exhaustive_functors; } const config; DsgUpdater(const Config& config, @@ -91,7 +89,7 @@ class DsgUpdater { void callUpdateFunctions(size_t timestamp_ns, UpdateInfo::ConstPtr info); private: - MergeTracker merge_tracker; + GroupedMergeTracker merge_tracker; std::map update_functors_; DynamicSceneGraph::Ptr source_graph_; diff --git a/include/hydra/backend/merge_tracker.h b/include/hydra/backend/merge_tracker.h index 8b7dbc57..ccf8d66a 100644 --- a/include/hydra/backend/merge_tracker.h +++ b/include/hydra/backend/merge_tracker.h @@ -43,12 +43,30 @@ struct MergeTracker { SharedDsgInfo& dsg, const MergeFunc& merge_attrs = MergeFunc()); + void updateAllMergeAttributes(const DynamicSceneGraph& unmerged, + DynamicSceneGraph& merged, + const MergeFunc& merge_attrs); + void clear(); + void erase_nodes(std::vector nodes_to_erase); + std::string print() const; + private: void updateParents(std::map& prior_merges, const Merge& merge); std::map> merge_sets_; }; +struct GroupedMergeTracker { + void initializeTracker(std::string name); + void clear(); + void erase_nodes(std::vector nodes); + std::string print() const; + MergeTracker& getMergeGroup(std::string name); + + private: + std::map group_to_tracker_; +}; + } // namespace hydra diff --git a/include/hydra/backend/update_buildings_functor.h b/include/hydra/backend/update_buildings_functor.h index c9f4ae2d..58198925 100644 --- a/include/hydra/backend/update_buildings_functor.h +++ b/include/hydra/backend/update_buildings_functor.h @@ -33,8 +33,6 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include - #include "hydra/backend/update_functions.h" namespace hydra { @@ -50,11 +48,6 @@ struct UpdateBuildingsFunctor : public UpdateFunctor { void call(const DynamicSceneGraph& unmerged, SharedDsgInfo& dsg, const UpdateInfo::ConstPtr& info) const override; - - private: - inline static const auto registration_ = - config::RegistrationWithConfig( - "UpdateBuildingsFunctor"); }; void declare_config(UpdateBuildingsFunctor::Config& config); diff --git a/include/hydra/backend/update_frontiers_functor.h b/include/hydra/backend/update_frontiers_functor.h index 75c3ab6a..ead6a2fe 100644 --- a/include/hydra/backend/update_frontiers_functor.h +++ b/include/hydra/backend/update_frontiers_functor.h @@ -41,11 +41,12 @@ namespace hydra { struct UpdateFrontiersFunctor : public UpdateFunctor { struct Config { - //! Frontiers with in this distance of the robot will be checked against places for - //! potential removal - double frontier_removal_check_threshold = 4; + //! Frontiers within this distance will be checked against places for removal + double frontier_removal_check_threshold = 4.0; //! Frontiers within this distance of the robot will be removed - double frontier_removal_threshold = 1; + double frontier_removal_threshold = 1.0; + //! Distance around existing frontiers that blocks adding new frontiers + double frontier_exclusion_radius = 0.0; } const config; explicit UpdateFrontiersFunctor(const Config& config) : config(config) {} @@ -54,12 +55,18 @@ struct UpdateFrontiersFunctor : public UpdateFunctor { SharedDsgInfo&, const UpdateInfo::ConstPtr&) const override; - void cleanup(uint64_t timestamp_ns, SharedDsgInfo& dsg) const; + void cleanup(uint64_t timestamp_ns, + DynamicSceneGraph& unmerged_dsg, + SharedDsgInfo& dsg) const; private: inline static const auto registration_ = config::RegistrationWithConfig( "UpdateFrontiersFunctor"); + mutable NodeSymbol next_node_id_ = NodeSymbol('G', 0); + mutable std::set deleted_frontiers_; }; +void declare_config(UpdateFrontiersFunctor::Config& config); + } // namespace hydra diff --git a/include/hydra/backend/update_functions.h b/include/hydra/backend/update_functions.h index 36321276..610008c9 100644 --- a/include/hydra/backend/update_functions.h +++ b/include/hydra/backend/update_functions.h @@ -65,8 +65,8 @@ struct UpdateInfo { kimera_pgmo::MeshOffsetInfo mesh_offsets = {}; }; -using LayerCleanupFunc = - std::function; +using LayerCleanupFunc = std::function; using FindMergeFunc = std::function; using MergeFunc = std::function; - struct Hooks { LayerCleanupFunc cleanup; FindMergeFunc find_merges; diff --git a/include/hydra/backend/update_rooms_functor.h b/include/hydra/backend/update_rooms_functor.h index 2f73a17b..16e74474 100644 --- a/include/hydra/backend/update_rooms_functor.h +++ b/include/hydra/backend/update_rooms_functor.h @@ -33,8 +33,6 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include - #include "hydra/backend/update_functions.h" #include "hydra/common/output_sink.h" #include "hydra/rooms/room_finder.h" @@ -60,9 +58,6 @@ struct UpdateRoomsFunctor : public UpdateFunctor { std::unique_ptr room_finder; private: - inline static const auto registration_ = - config::RegistrationWithConfig( - "UpdateRoomsFunctor"); Sink::List sinks_; }; diff --git a/include/hydra/backend/update_surface_places_functor.h b/include/hydra/backend/update_surface_places_functor.h index 7c2bddb3..bb892c8b 100644 --- a/include/hydra/backend/update_surface_places_functor.h +++ b/include/hydra/backend/update_surface_places_functor.h @@ -88,10 +88,6 @@ struct Update2dPlacesFunctor : public UpdateFunctor { private: mutable NodeSymbol next_node_id_ = NodeSymbol('S', 0); - - inline static const auto registration_ = - config::RegistrationWithConfig( - "Update2dPlacesFunctor"); }; void declare_config(Update2dPlacesFunctor::Config& conf); diff --git a/include/hydra/utils/logging.h b/include/hydra/utils/logging.h index b8595f98..3dc0829d 100644 --- a/include/hydra/utils/logging.h +++ b/include/hydra/utils/logging.h @@ -44,6 +44,9 @@ struct VerbosityConfig { VerbosityConfig() = default; VerbosityConfig(int verbosity); VerbosityConfig(const std::string& prefix, int verbosity = 0); + + //! Make a copy of the verbosity config with a prefix derived from the provided name + VerbosityConfig with_name(const std::string& name) const; }; void declare_config(VerbosityConfig& config); diff --git a/src/backend/dsg_updater.cpp b/src/backend/dsg_updater.cpp index 9c45f11c..5d6f140a 100644 --- a/src/backend/dsg_updater.cpp +++ b/src/backend/dsg_updater.cpp @@ -41,26 +41,53 @@ #include #include -#include "hydra/common/global_info.h" -#include "hydra/common/launch_callbacks.h" -#include "hydra/common/pipeline_queues.h" -#include "hydra/utils/minimum_spanning_tree.h" -#include "hydra/utils/pgmo_mesh_traits.h" +#include "hydra/utils/pgmo_mesh_traits.h" // IWYU pragma: keep #include "hydra/utils/timing_utilities.h" namespace hydra { +namespace { + +void findAndApplyMerges(const VerbosityConfig& config, + const UpdateFunctor::Hooks& hooks, + const UpdateInfo::ConstPtr& info, + const DynamicSceneGraph& source, + SharedDsgInfo& target, + MergeTracker& tracker, + bool exhaustive) { + // TODO(nathan) handle given merges + auto merges = hooks.find_merges(source, info); + auto applied = tracker.applyMerges(source, merges, target, hooks.merge); + MLOG(1) << "pass 0: " << merges.size() << " merges (applied " << applied << ")"; + if (!exhaustive) { + return; + } + + applied = 0; + size_t iter = 0; + do { + merges = hooks.find_merges(*target.graph, info); + applied = tracker.applyMerges(source, merges, target, hooks.merge); + MLOG(1) << "pass " << iter << ": " << merges.size() << " merges (applied " + << applied << ")"; + ++iter; + } while (applied > 0); +} + +} // namespace using hydra::timing::ScopedTimer; -using kimera_pgmo::KimeraPgmoInterface; -using pose_graph_tools::PoseGraph; + +DsgUpdater::Config::Config() : VerbosityConfig("[dsg_updated] ") {} void declare_config(DsgUpdater::Config& config) { using namespace config; name("DsgUpdaterConfig"); + base(config); + field(config.functor_logging, "functor_logging"); field(config.enable_node_merging, "enable_node_merging"); - field(config.enable_exhaustive_merging, "enable_exhaustive_merging"); field(config.reset_dsg_on_loop_closure, "reset_dsg_on_loop_closure"); field(config.update_functors, "update_functors"); + field(config.exhaustive_functors, "exhaustive_functors"); } DsgUpdater::DsgUpdater(const Config& config, @@ -69,6 +96,7 @@ DsgUpdater::DsgUpdater(const Config& config, : config(config::checkValid(config)), source_graph_(source), target_dsg_(target) { for (const auto& [name, functor] : config.update_functors) { update_functors_.emplace(name, functor.create()); + merge_tracker.initializeTracker(name); } } @@ -116,11 +144,28 @@ void DsgUpdater::callUpdateFunctions(size_t timestamp_ns, UpdateInfo::ConstPtr i if (config.reset_dsg_on_loop_closure && info->loop_closure_detected) { resetBackendDsg(timestamp_ns); } + GraphMergeConfig merge_config; merge_config.previous_merges = &target_dsg_->merges; merge_config.update_dynamic_attributes = false; target_dsg_->graph->mergeGraph(*source_graph_, merge_config); + // Nodes occasionally get added to the backend after they've left the active window, + // which means they never get deformed or updated correctly. This forces them to be + // active for at least one update + std::vector active_nodes_to_restore; + for (auto& [layer_id, layer] : source_graph_->layers()) { + for (auto& [node_id, node] : layer->nodes()) { + auto& attrs = node->attributes(); + if (source_graph_->checkNode(node_id) == NodeStatus::NEW && !attrs.is_active) { + attrs.is_active = true; + active_nodes_to_restore.push_back(node_id); + } + } + } + + const std::set exhaustive_names(config.exhaustive_functors.begin(), + config.exhaustive_functors.end()); std::list cleanup_hooks; for (const auto& [name, functor] : update_functors_) { if (!functor) { @@ -134,30 +179,39 @@ void DsgUpdater::callUpdateFunctions(size_t timestamp_ns, UpdateInfo::ConstPtr i functor->call(*source_graph_, *target_dsg_, info); if (hooks.find_merges && enable_merging) { - // TODO(nathan) handle given merges - const auto merges = hooks.find_merges(*source_graph_, info); - const auto applied = - merge_tracker.applyMerges(*source_graph_, merges, *target_dsg_, hooks.merge); - VLOG(1) << "[Backend: " << name << "] Found " << merges.size() - << " merges (applied " << applied << ")"; - - if (config.enable_exhaustive_merging) { - size_t merge_iter = 0; - size_t num_applied = 0; - do { - const auto new_merges = hooks.find_merges(*target_dsg_->graph, info); - num_applied = merge_tracker.applyMerges( - *source_graph_, new_merges, *target_dsg_, hooks.merge); - VLOG(1) << "[Backend: " << name << "] Found " << new_merges.size() - << " merges at pass " << merge_iter << " (" << num_applied - << " applied)"; - ++merge_iter; - } while (num_applied > 0); + auto& tracker = merge_tracker.getMergeGroup(name); + findAndApplyMerges(config.functor_logging.with_name(name), + hooks, + info, + *source_graph_, + *target_dsg_, + tracker, + exhaustive_names.count(name)); + if (info->loop_closure_detected && hooks.merge) { + MLOG(3) << "updating all merge attributes for " << name; + MLOG(3) << "current tracker: " << tracker.print(); + tracker.updateAllMergeAttributes( + *source_graph_, *target_dsg_->graph, hooks.merge); } } - } - launchCallbacks(cleanup_hooks, info, target_dsg_.get()); + MLOG(2) << "all merges: " << merge_tracker.print(); + for (const auto& func : cleanup_hooks) { + func(info, *source_graph_, target_dsg_.get()); + } + + // We reset active flags for all new nodes that were inactive after one update + for (auto node_id : active_nodes_to_restore) { + auto node = source_graph_->findNode(node_id); + if (node) { + node->attributes().is_active = false; + } + } + + // clear new node status + // TODO(nathan) add API for marking new nodes + source_graph_->getNewNodes(true); + } } } // namespace hydra diff --git a/src/backend/merge_tracker.cpp b/src/backend/merge_tracker.cpp index 835ff28e..4f67880b 100644 --- a/src/backend/merge_tracker.cpp +++ b/src/backend/merge_tracker.cpp @@ -97,8 +97,51 @@ size_t MergeTracker::applyMerges(const DynamicSceneGraph& unmerged, return num_applied; } +void MergeTracker::updateAllMergeAttributes(const DynamicSceneGraph& unmerged, + DynamicSceneGraph& merged, + const MergeFunc& merge_attrs) { + for (auto& [parent, children] : merge_sets_) { + auto child_iter = children.begin(); + while (child_iter != children.end()) { + if (!unmerged.hasNode(*child_iter)) { + child_iter = children.erase(child_iter); + } else { + ++child_iter; + } + } + + std::vector nodes{parent}; + nodes.insert(nodes.end(), children.begin(), children.end()); + merged.setNodeAttributes(parent, merge_attrs(unmerged, nodes)); + } +} + +std::string MergeTracker::print() const { + std::stringstream ss; + for (const auto& [parent, children] : merge_sets_) { + ss << " - " << NodeSymbol(parent).str() << ": ["; + auto iter = children.begin(); + while (iter != children.end()) { + ss << NodeSymbol(*iter).str(); + ++iter; + if (iter != children.end()) { + ss << ", "; + } + } + ss << "]\n"; + } + + return ss.str(); +} + void MergeTracker::clear() { merge_sets_.clear(); } +void MergeTracker::erase_nodes(std::vector nodes_to_erase) { + for (auto n : nodes_to_erase) { + merge_sets_.erase(n); + } +} + void MergeTracker::updateParents(std::map& prior_merges, const Merge& merge) { // look up (and maybe initialize new parent) @@ -126,4 +169,33 @@ void MergeTracker::updateParents(std::map& prior_merges, merge_sets_.erase(from_iter); } +void GroupedMergeTracker::initializeTracker(std::string name) { + group_to_tracker_.insert({name, MergeTracker()}); +} + +void GroupedMergeTracker::clear() { + for (auto& [name, tracker] : group_to_tracker_) { + tracker.clear(); + } +} + +void GroupedMergeTracker::erase_nodes(std::vector nodes) { + for (auto& [name, tracker] : group_to_tracker_) { + tracker.erase_nodes(nodes); + } +} + +std::string GroupedMergeTracker::print() const { + std::stringstream ss; + for (auto& [name, tracker] : group_to_tracker_) { + ss << "Group '" << name << "' merges:\n" << tracker.print(); + } + + return ss.str(); +} + +MergeTracker& GroupedMergeTracker::getMergeGroup(std::string name) { + return group_to_tracker_.at(name); +} + } // namespace hydra diff --git a/src/backend/update_buildings_functor.cpp b/src/backend/update_buildings_functor.cpp index 03daab11..c73a4135 100644 --- a/src/backend/update_buildings_functor.cpp +++ b/src/backend/update_buildings_functor.cpp @@ -39,6 +39,14 @@ #include namespace hydra { +namespace { + +static const auto reg = config::RegistrationWithConfig( + "UpdateBuildingsFunctor"); + +} void declare_config(UpdateBuildingsFunctor::Config& config) { using namespace config; diff --git a/src/backend/update_frontiers_functor.cpp b/src/backend/update_frontiers_functor.cpp index 895f3c77..47334395 100644 --- a/src/backend/update_frontiers_functor.cpp +++ b/src/backend/update_frontiers_functor.cpp @@ -46,9 +46,11 @@ using timing::ScopedTimer; UpdateFunctor::Hooks UpdateFrontiersFunctor::hooks() const { auto my_hooks = UpdateFunctor::hooks(); - my_hooks.cleanup = [this](const UpdateInfo::ConstPtr& info, SharedDsgInfo* dsg) { + my_hooks.cleanup = [this](const UpdateInfo::ConstPtr& info, + DynamicSceneGraph& unmerged_dsg, + SharedDsgInfo* dsg) { if (dsg) { - cleanup(info->timestamp_ns, *dsg); + cleanup(info->timestamp_ns, unmerged_dsg, *dsg); } }; @@ -61,72 +63,226 @@ void UpdateFrontiersFunctor::call(const DynamicSceneGraph&, return; } -void UpdateFrontiersFunctor::cleanup(uint64_t timestamp_ns, SharedDsgInfo& dsg) const { +bool check_against_places(const SharedDsgInfo& dsg, + const std::shared_ptr place_finder, + const spark_dsg::PlaceNodeAttributes& attrs) { + std::vector> nearest_places; + place_finder->findRadius(attrs.position, 5, false, [&](NodeId pid, size_t, double) { + auto& pattr = dsg.graph->getNode(pid).attributes(); + nearest_places.push_back({pattr.position, pattr.distance}); + }); + + for (auto center_rad : nearest_places) { + if ((center_rad.first - attrs.position).norm() <= center_rad.second) { + return true; + } + } + return false; +} + +bool point_in_polygon(Eigen::Vector2d pt, std::vector polygon) { + if (polygon.size() < 3) { + return false; + } + + double projection_minimum = 0.0; + for (size_t idx = 0; idx < polygon.size(); ++idx) { + auto x1 = polygon.at(idx); + auto x2 = polygon.at((idx + 1) % polygon.size()); + auto u = (x2 - x1).head(2); + if (u.norm() < .00001) { + LOG(WARNING) << "short edge: " << x2 << x1; + } + Eigen::Vector2d normal(-u(1), u(0)); + auto projection = normal.dot(pt - x1.head(2)); + if (projection < projection_minimum) { + return false; + } + } + return true; +} + +bool check_against_2d_places(const SharedDsgInfo& dsg, + const std::shared_ptr place_2d_finder, + const spark_dsg::PlaceNodeAttributes& attrs) { + std::vector> nearest_places; + place_2d_finder->findRadius( + attrs.position, 20, false, [&](NodeId pid, size_t, double) { + auto& pattr = dsg.graph->getNode(pid).attributes(); + nearest_places.push_back(pattr.boundary); + }); + + for (auto boundary : nearest_places) { + if (point_in_polygon(attrs.position.head(2), boundary)) { + return true; + } + } + return false; +} + +void UpdateFrontiersFunctor::cleanup(uint64_t timestamp_ns, + DynamicSceneGraph& unmerged_dsg, + SharedDsgInfo& dsg) const { ScopedTimer spin_timer("backend/cleanup_frontiers", timestamp_ns); if (!dsg.graph->hasLayer(DsgLayers::PLACES)) { return; } std::lock_guard lock(dsg.mutex); + + for (const auto nid : deleted_frontiers_) { + dsg.graph->removeNode(nid); + unmerged_dsg.removeNode(nid); + } + const auto& places_layer = dsg.graph->getLayer(DsgLayers::PLACES); + const auto& places_2d_layer = dsg.graph->getLayer(DsgLayers::MESH_PLACES); + + const auto& prefix = GlobalInfo::instance().getRobotPrefix(); + const auto layer_key = dsg.graph->getLayerKey(DsgLayers::AGENTS); + if (!layer_key) { + LOG(ERROR) << "Update frontiers functor could not find agents layer"; + return; + } + const auto agents = dsg.graph->findLayer(layer_key->layer, prefix.key); + if (!agents || agents->numNodes() == 0) { + return; + } + NodeSymbol pgmo_key(prefix.key, agents->numNodes() - 1); + Eigen::Vector3d agent_pos = dsg.graph->getNode(pgmo_key).attributes().position; + Eigen::Quaterniond agent_quat = + dsg.graph->getNode(pgmo_key).attributes().world_R_body; + Eigen::Vector3d agent_heading = agent_quat * Eigen::Vector3d(1, 0, 0); std::unordered_set layer_nodes; + std::unordered_set frontier_nodes; for (const auto& [node_id, node] : places_layer.nodes()) { auto attrs = node->tryAttributes(); - if (!attrs || !attrs->real_place) { + if (!attrs) { continue; } + if (attrs->real_place) { + layer_nodes.insert(node_id); + } else if (!attrs->is_predicted && !attrs->need_cleanup) { + frontier_nodes.insert(node_id); + } + } - layer_nodes.insert(node_id); + std::unordered_set layer_2d_nodes; + for (const auto& [node_id, node] : places_2d_layer.nodes()) { + layer_2d_nodes.insert(node_id); } - auto place_finder = std::make_unique(places_layer, layer_nodes); + bool have_existing_frontiers = frontier_nodes.size() > 0; + + auto place_finder = std::make_shared(places_layer, layer_nodes); + auto place_2d_finder = + std::make_shared(places_2d_layer, layer_2d_nodes); + auto frontier_finder = + std::make_unique(places_layer, frontier_nodes); std::set nodes_to_remove; + std::set nodes_to_copy; + std::set nodes_to_anti; for (const auto& [node_id, node] : places_layer.nodes()) { auto attrs = node->tryAttributes(); if (!attrs || attrs->real_place) { continue; } - const auto& prefix = GlobalInfo::instance().getRobotPrefix(); - const auto layer_id = dsg.graph->getLayerKey(DsgLayers::AGENTS)->layer; - if (!dsg.graph->hasLayer(layer_id, prefix.key)) { + // Delete frontier if it's too close to an existing frontier. + if (have_existing_frontiers && attrs->need_cleanup) { + NodeId nearest_frontier; + frontier_finder->find(attrs->position, 1, false, [&](NodeId pid, size_t, double) { + nearest_frontier = pid; + }); + const auto& neighbor_attrs = + dsg.graph->getNode(nearest_frontier).attributes(); + double neighbor_distance = (neighbor_attrs.position - attrs->position).norm(); + if (neighbor_distance < config.frontier_exclusion_radius) { + nodes_to_remove.insert(node_id); + continue; + } + } + + // Delete frontiers that are inside of existing places. + bool frontier_check_3d_places = false; + bool frontier_check_2d_places = true; + bool remove = false; + + bool close_to_robot = + (agent_pos - attrs->position).norm() < config.frontier_removal_check_threshold; + + if (attrs->need_cleanup || close_to_robot) { + attrs->need_cleanup = false; + auto unmerged_node = unmerged_dsg.findNode(node_id); + if (unmerged_node) { + unmerged_node->attributes().need_cleanup = false; + } + if (frontier_check_3d_places) { + remove = check_against_places(dsg, place_finder, *attrs); + } + + if (frontier_check_2d_places) { + remove |= check_against_2d_places(dsg, place_2d_finder, *attrs); + } + } + + if (!frontier_check_2d_places && !frontier_check_3d_places) { + attrs->need_cleanup = false; + } + + if (remove) { + nodes_to_remove.insert(node_id); continue; } - const auto& agents = dsg.graph->getLayer(layer_id, prefix.key); - NodeSymbol pgmo_key(prefix.key, agents.numNodes() - 1); - Eigen::Vector3d agent_pos = dsg.graph->getNode(pgmo_key).attributes().position; // Some frontiers may end up outside any places even when the environment // is fully explored, because the places aren't infinitely dense. If the // robot gets close enough to these frontiers, we clear them. - if ((agent_pos - attrs->position).norm() < config.frontier_removal_threshold) { - nodes_to_remove.insert(node_id); + // NOTE: currently we don't delete these nodes, we actually turn them into + // anti-frontiers + auto disp_to_frontier = attrs->position - agent_pos; + double distance_to_frontier = disp_to_frontier.norm(); + bool facing_frontier = + (disp_to_frontier / distance_to_frontier).dot(agent_heading) > 0.5; + bool near_frontier = + (attrs->position - agent_pos).norm() < config.frontier_removal_threshold; + if (facing_frontier && near_frontier) { + // nodes_to_remove.insert(id_node_pair.first); + if (!attrs->active_frontier) { + nodes_to_anti.insert(node_id); + } else { + // If the frontier is active, we need to copy it or it will be deleted by the + // frontend + nodes_to_copy.insert(node_id); + } + continue; } + } - bool close_to_robot = - (agent_pos - attrs->position).norm() < config.frontier_removal_check_threshold; - if (attrs->need_cleanup || close_to_robot) { - attrs->need_cleanup = false; - std::vector> nearest_places; - place_finder->findRadius( - attrs->position, 5, false, [&](NodeId pid, size_t, double) { - auto& pattr = dsg.graph->getNode(pid).attributes(); - nearest_places.push_back({pattr.position, pattr.distance}); - }); - - for (auto center_rad : nearest_places) { - if ((center_rad.first - attrs->position).norm() <= center_rad.second) { - nodes_to_remove.insert(node_id); - break; - } - } + for (const auto nid : nodes_to_anti) { + auto node = unmerged_dsg.findNode(nid); + if (node) { + node->attributes().anti_frontier = true; } + auto& merged_attrs = dsg.graph->getNode(nid).attributes(); + merged_attrs.anti_frontier = true; } for (const auto nid : nodes_to_remove) { + deleted_frontiers_.insert(nid); dsg.graph->removeNode(nid); + unmerged_dsg.removeNode(nid); + } + + // TODO(aaron): do we need to also deal with copying the "active" frontiers to the + // unmerged graph? + for (const auto nid : nodes_to_copy) { + NodeSymbol new_node_id = next_node_id_++; + auto cloned_attrs = dsg.graph->getNode(nid).attributes().clone(); + dynamic_cast(cloned_attrs.get())->active_frontier = false; + dsg.graph->emplaceNode(DsgLayers::PLACES, new_node_id, std::move(cloned_attrs)); } } @@ -135,6 +291,7 @@ void declare_config(UpdateFrontiersFunctor::Config& config) { name("FrontierConfig"); field(config.frontier_removal_threshold, "frontier_removal_threshold"); field(config.frontier_removal_check_threshold, "frontier_removal_check_threshold"); + field(config.frontier_exclusion_radius, "frontier_exclusion_radius"); } } // namespace hydra diff --git a/src/backend/update_rooms_functor.cpp b/src/backend/update_rooms_functor.cpp index b272ee4e..4a481b53 100644 --- a/src/backend/update_rooms_functor.cpp +++ b/src/backend/update_rooms_functor.cpp @@ -41,6 +41,14 @@ #include "hydra/utils/timing_utilities.h" namespace hydra { +namespace { + +static const auto reg = + config::RegistrationWithConfig("UpdateRoomsFunctor"); + +} using timing::ScopedTimer; using SemanticLabel = SemanticNodeAttributes::Label; diff --git a/src/backend/update_surface_places_functor.cpp b/src/backend/update_surface_places_functor.cpp index bb9516a1..8a044955 100644 --- a/src/backend/update_surface_places_functor.cpp +++ b/src/backend/update_surface_places_functor.cpp @@ -47,6 +47,14 @@ #include "hydra/utils/timing_utilities.h" namespace hydra { +namespace { + +static const auto reg = config::RegistrationWithConfig( + "Update2dPlacesFunctor"); + +} using timing::ScopedTimer; @@ -98,7 +106,7 @@ Update2dPlacesFunctor::Update2dPlacesFunctor(const Config& config) UpdateFunctor::Hooks Update2dPlacesFunctor::hooks() const { auto my_hooks = UpdateFunctor::hooks(); - my_hooks.cleanup = [this](const UpdateInfo::ConstPtr&, SharedDsgInfo* dsg) { + my_hooks.cleanup = [this](const auto&, auto&, auto dsg) { if (dsg) { cleanup(*dsg); } diff --git a/src/backend/update_traversability_functor.cpp b/src/backend/update_traversability_functor.cpp index 4bf313a6..96ee9f38 100644 --- a/src/backend/update_traversability_functor.cpp +++ b/src/backend/update_traversability_functor.cpp @@ -87,9 +87,7 @@ UpdateFunctor::Hooks UpdateTraversabilityFunctor::hooks() const { const std::vector& merge_ids) { return mergeNodes(dsg, merge_ids); }; - my_hooks.cleanup = [this](const UpdateInfo::ConstPtr& info, SharedDsgInfo* dsg) { - cleanup(info, dsg); - }; + my_hooks.cleanup = [this](const auto& info, auto&, auto dsg) { cleanup(info, dsg); }; return my_hooks; } diff --git a/src/utils/logging.cpp b/src/utils/logging.cpp index 6fc8d37a..f4076b38 100644 --- a/src/utils/logging.cpp +++ b/src/utils/logging.cpp @@ -48,4 +48,10 @@ VerbosityConfig::VerbosityConfig(int verbosity) : VerbosityConfig("", verbosity) VerbosityConfig::VerbosityConfig(const std::string& prefix, int verbosity) : verbosity(verbosity), prefix(prefix) {} +VerbosityConfig VerbosityConfig::with_name(const std::string& name) const { + auto new_config = *this; + new_config.prefix = "[" + name + "] "; + return new_config; +} + } // namespace hydra