Function getMergedAttributes

clio/src/object_update_functor.cpp:208–236

}

NodeAttributes::Ptr getMergedAttributes(const DynamicSceneGraph& graph,
                                        const std::vector<NodeId>& nodes) {
  if (nodes.empty()) {
    return nullptr;
  }

  auto iter = nodes.begin();
  CHECK(graph.hasNode(*iter));
  const auto& node = graph.getNode(*iter);
  ++iter;

  auto attrs_ptr = node.attributes().clone();
  auto& attrs = *CHECK_NOTNULL(dynamic_cast<KhronosObjectAttributes*>(attrs_ptr.get()));
  attrs.semantic_feature = attrs.semantic_feature.rowwise().mean().eval();

  while (iter != nodes.end()) {
    const auto& other = graph.getNode(*iter);
    const auto& other_attrs = other.attributes<KhronosObjectAttributes>();
    attrs.position += other_attrs.position;
    attrs.semantic_feature += other_attrs.semantic_feature.rowwise().mean();
    // TODO(nathan) update khronos to add the attribute merging somewhere convenient
    mergeObjectAttributes(other_attrs, attrs);
    ++iter;
  }

  attrs.position /= nodes.size();
  attrs.semantic_feature /= nodes.size();
  return attrs_ptr;
}

std::optional<std::pair<NodeId, bool>> getBestParent(const DynamicSceneGraph& graph,
                                                     const std::vector<NodeId>& nodes) {