Method updateActiveParents

clio/src/object_update_functor.cpp:340–391

}

void ObjectUpdateFunctor::updateActiveParents(DynamicSceneGraph& graph) const {
  std::vector<NodeId> place_ids;
  const auto& places = graph.getLayer(DsgLayers::PLACES);
  for (const auto& id_node_pair : graph.getLayer(DsgLayers::PLACES).nodes()) {
    place_ids.push_back(id_node_pair.first);
  }

  // TODO(nathan) drop this once edges start behaving
  for (const auto& id_node_pair : graph.getLayer(DsgLayers::OBJECTS).nodes()) {
    active_.insert(id_node_pair.first);
  }

  hydra::NearestNodeFinder places_finder(places, place_ids);
  auto iter = active_.begin();
  while (iter != active_.end()) {
    const auto& node = graph.getNode(*iter);
    const auto parent_id = node.getParent();
    if (parent_id) {
      const bool is_active = places.getNode(*parent_id).attributes().is_active;
      if (is_active) {
        ++iter;
      } else {
        iter = active_.erase(iter);
      }

      continue;
    }

    bool found = false;
    places_finder.find(node.attributes().position,
                       1,
                       false,
                       [&](NodeId place_id, size_t, double distance) {
                         if (config.neighbor_max_distance > 0.0 &&
                             distance >= config.neighbor_max_distance) {
                           LOG(WARNING)
                               << "Discarding nearest neighbor '"
                               << NodeSymbol(place_id).getLabel() << "' for node '"
                               << NodeSymbol(node.id).getLabel() << "' with distance "
                               << distance << " >= " << config.neighbor_max_distance;
                         }

                         graph.insertEdge(place_id, node.id);
                         found = true;
                       });
    if (found) {
      iter = active_.erase(iter);
    } else {
      ++iter;
    }
  }
}

}  // namespace clio