Program Listing for File WorldModel.h

↰ Return to documentation for file (include/mola_kernel/WorldModel.h)

/* -------------------------------------------------------------------------
 *   A Modular Optimization framework for Localization and mApping  (MOLA)
 * Copyright (C) 2018-2025 Jose Luis Blanco, University of Almeria
 * See LICENSE for license information.
 * ------------------------------------------------------------------------- */
#pragma once

#include <mola_kernel/Entity.h>
#include <mola_kernel/Factor.h>
#include <mola_kernel/FastAllocator.h>
#include <mola_kernel/Yaml.h>
#include <mola_kernel/id.h>
#include <mola_kernel/interfaces/ExecutableBase.h>
#include <mrpt/serialization/CSerializable.h>

#include <map>
#include <shared_mutex>

namespace mola
{
using entity_connected_factors_t = mola::fast_map<id_t, mola::fast_set<fid_t>>;

class WorldModelData : public mrpt::serialization::CSerializable
{
  DEFINE_SERIALIZABLE(WorldModelData, mola)
 public:
  struct EntitiesContainer;
  struct FactorsContainer;

  std::string map_name_;

  std::unique_ptr<EntitiesContainer> entities_;
  entity_connected_factors_t         entity_connected_factors_;
  std::shared_mutex                  entities_mtx_;

  std::unique_ptr<FactorsContainer> factors_;
  std::shared_mutex                 factors_mtx_;

  mutable mola::fast_map<id_t, mrpt::Clock::time_point> entity_last_access_;
  std::shared_mutex                                     entity_last_access_mtx_;
};

class WorldModel : public ExecutableBase
{
  DEFINE_MRPT_OBJECT(WorldModel, mola)

 public:
  WorldModel();

  // Virtual interface of any ExecutableBase. See base docs:
  void initialize(const Yaml&) override final;
  void spinOnce() override;

  int launchOrderPriority() const override { return 10; }

  struct Parameters
  {
    double age_to_unload_keyframes{15.0};
  };

  Parameters params_;

  void map_load_from(mrpt::serialization::CArchive& in);

  void map_load_from(const std::string& fileName);

  void map_save_to(mrpt::serialization::CArchive& out) const;

  void map_save_to(const std::string& fileName) const;

  std::string map_base_directory() const { return map_base_dir_; }

  void entities_lock_for_read() { data_.entities_mtx_.lock_shared(); }
  void entities_unlock_for_read() { data_.entities_mtx_.unlock_shared(); }
  void entities_lock_for_write() { data_.entities_mtx_.lock(); }
  void entities_unlock_for_write() { data_.entities_mtx_.unlock(); }

  void factors_lock_for_read() { data_.factors_mtx_.lock_shared(); }
  void factors_unlock_for_read() { data_.factors_mtx_.unlock_shared(); }
  void factors_lock_for_write() { data_.factors_mtx_.lock(); }
  void factors_unlock_for_write() { data_.factors_mtx_.unlock(); }

  const Entity& entity_by_id(const id_t id) const;
  Entity&       entity_by_id(const id_t id);

  const Factor& factor_by_id(const fid_t id) const;
  Factor&       factor_by_id(const fid_t id);

  id_t  entity_emplace_back(Entity&& e);
  fid_t factor_emplace_back(Factor&& f);

  id_t  entity_push_back(const Entity& e);
  fid_t factor_push_back(const Factor& f);

  std::vector<id_t>  entity_all_ids() const;
  std::vector<fid_t> factor_all_ids() const;

  annotations_data_t&       entity_annotations_by_id(const id_t id);
  const annotations_data_t& entity_annotations_by_id(const id_t id) const;

  std::set<id_t> entity_neighbors(const id_t id) const;

 private:
  WorldModelData data_;

  std::string map_base_dir_;

  std::vector<id_t> findEntitiesToSwapOff();

  void internal_update_neighbors(const FactorBase& f);
};

}  // namespace mola