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