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-2024 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