| 162 | } |
| 163 | |
| 164 | std::set<size_t> ObjectUpdateFunctor::addSegmentEdges(DynamicSceneGraph& graph) const { |
| 165 | const auto& segments = graph.getLayer(DsgLayers::SEGMENTS); |
| 166 | |
| 167 | std::set<size_t> active_components; |
| 168 | for (auto&& [node_id, node] : segments.nodes()) { |
| 169 | auto& attrs = node->attributes<KhronosObjectAttributes>(); |
| 170 | if (ignored_.count(node_id)) { |
| 171 | continue; |
| 172 | } |
| 173 | |
| 174 | if (node_to_component_.count(node_id)) { |
| 175 | // only examine new nodes |
| 176 | continue; |
| 177 | } |
| 178 | |
| 179 | const Eigen::VectorXf feature = attrs.semantic_feature.rowwise().mean(); |
| 180 | const auto result = tasks_->getBestScore(*metric_, feature); |
| 181 | if (result.score < config.min_segment_score) { |
| 182 | VLOG(1) << "Skipping segment with score: " << result.score; |
| 183 | ignored_.insert(node_id); |
| 184 | attrs.is_active = false; |
| 185 | continue; |
| 186 | } |
| 187 | |
| 188 | // TODO(nathan) do something smarter than pairwise iteration |
| 189 | for (auto&& [other_id, other_node] : segments.nodes()) { |
| 190 | if (other_id == node_id) { |
| 191 | continue; |
| 192 | } |
| 193 | |
| 194 | const auto& other_attrs = other_node->attributes<KhronosObjectAttributes>(); |
| 195 | if (edge_checker_->call(attrs, other_attrs)) { |
| 196 | graph.insertEdge(node_id, other_id); |
| 197 | const auto iter = node_to_component_.find(other_id); |
| 198 | if (iter != node_to_component_.end()) { |
| 199 | active_components.insert(iter->second); |
| 200 | } |
| 201 | } |
| 202 | } |
| 203 | } |
| 204 | |
| 205 | return active_components; |
| 206 | } |
| 207 | |
| 208 | NodeAttributes::Ptr getMergedAttributes(const DynamicSceneGraph& graph, |
| 209 | const std::vector<NodeId>& nodes) { |