| 62 | } |
| 63 | |
| 64 | void RegionUpdateFunctor::updateGraphBatch(DynamicSceneGraph& graph, |
| 65 | const Clusters& clusters) const { |
| 66 | VLOG(2) << "Got " << clusters.size() << " cluster(s)"; |
| 67 | |
| 68 | std::vector<NodeId> prev_regions; |
| 69 | for (const auto& id_node_pair : graph.getLayer(DsgLayers::ROOMS).nodes()) { |
| 70 | prev_regions.push_back(id_node_pair.first); |
| 71 | } |
| 72 | |
| 73 | for (const auto node : prev_regions) { |
| 74 | graph.removeNode(node); |
| 75 | } |
| 76 | |
| 77 | std::set<NodeId> new_nodes; |
| 78 | for (size_t i = 0; i < clusters.size(); ++i) { |
| 79 | NodeSymbol new_node_id(region_id_.category(), i); |
| 80 | auto attrs = std::make_unique<SemanticNodeAttributes>(); |
| 81 | attrs->semantic_label = 0; |
| 82 | attrs->name = clusters[i]->best_task_name; |
| 83 | attrs->semantic_feature = clusters[i]->feature; |
| 84 | attrs->semantic_label = clusters[i]->best_task_index; |
| 85 | graph.emplaceNode(DsgLayers::ROOMS, new_node_id, std::move(attrs)); |
| 86 | |
| 87 | for (const auto node_id : clusters[i]->nodes) { |
| 88 | graph.insertEdge(new_node_id, node_id); |
| 89 | } |
| 90 | |
| 91 | new_nodes.insert(new_node_id); |
| 92 | } |
| 93 | |
| 94 | const auto& places = graph.getLayer(DsgLayers::PLACES); |
| 95 | for (auto&& [node_id, node] : graph.getLayer(DsgLayers::ROOMS).nodes()) { |
| 96 | const std::unordered_set<NodeId> to_use(node->children().begin(), |
| 97 | node->children().end()); |
| 98 | node->attributes().position = hydra::getRoomPosition(places, to_use); |
| 99 | } |
| 100 | |
| 101 | hydra::addEdgesToRoomLayer(graph, new_nodes); |
| 102 | } |
| 103 | |
| 104 | } // namespace clio |