Method FrontendModule

src/frontend/frontend_module.cpp:54–92

using LabelClusters = MeshSegmenter::LabelClusters;

FrontendModule::FrontendModule(const RobotPrefixConfig& prefix,
                               const FrontendConfig& config,
                               const SharedDsgInfo::Ptr& dsg,
                               const SharedModuleState::Ptr& state)
    : queue_(std::make_shared<FrontendInputQueue>()),
      prefix_(prefix),
      config_(config),
      dsg_(dsg),
      state_(state) {
  config_.pgmo_config.robot_id = prefix_.id;
  label_map_.reset(new kimera::SemanticLabel2Color(config_.semantic_label_file));

  // add placeholder mesh to allow mesh edges to be added
  dsg_->graph->initMesh();
  dsg_->graph->createDynamicLayer(DsgLayers::AGENTS, prefix_.key);

  const auto mesh_resolution = config_.pgmo_config.mesh_resolution;
  mesh_compression_.reset(new kimera_pgmo::DeltaCompression(mesh_resolution));

  CHECK(mesh_frontend_.initialize(config_.pgmo_config));
  segmenter_.reset(
      new MeshSegmenter(config_.object_config, dsg_->graph->getMeshVertices()));

  if (config_.should_log) {
    VLOG(1) << "[Hydra Frontend] logging to " << (config_.log_path + "/frontend");
    frontend_graph_logger_.setOutputPath(config_.log_path + "/frontend");
    frontend_graph_logger_.setLayerName(DsgLayers::OBJECTS, "objects");
    frontend_graph_logger_.setLayerName(DsgLayers::PLACES, "places");
  }

  input_callbacks_.push_back(
      std::bind(&FrontendModule::updateMeshAndObjects, this, std::placeholders::_1));
  input_callbacks_.push_back(
      std::bind(&FrontendModule::updateDeformationGraph, this, std::placeholders::_1));
  input_callbacks_.push_back(
      std::bind(&FrontendModule::updatePoseGraph, this, std::placeholders::_1));
  input_callbacks_.push_back(
      std::bind(&FrontendModule::updatePlaces, this, std::placeholders::_1));
}

FrontendModule::~FrontendModule() { stop(); }