Program Listing for File BackEndBase.h

↰ Return to documentation for file (include/mola_kernel/interfaces/BackEndBase.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/WorldModel.h>
#include <mola_kernel/Yaml.h>
#include <mola_kernel/interfaces/ExecutableBase.h>
#include <mrpt/core/Clock.h>
#include <mrpt/core/WorkerThreadsPool.h>
#include <mrpt/img/TCamera.h>  // TODO: Remove after unused below
#include <mrpt/math/CMatrixFixed.h>
#include <mrpt/obs/CSensoryFrame.h>

#include <future>
#include <optional>

namespace mola
{
class BackEndBase : public ExecutableBase
{
    DEFINE_VIRTUAL_MRPT_OBJECT(BackEndBase)

   public:
    BackEndBase();
    virtual ~BackEndBase() = default;

    void initialize(const Yaml& cfg) override final;

   protected:
    virtual void initialize_backend(const Yaml& cfg) = 0;

   public:
    struct ProposeKF_Input
    {
        mrpt::Clock::time_point timestamp{};

        std::optional<mrpt::obs::CSensoryFrame> observations{std::nullopt};
    };

    struct ProposeKF_Output
    {
        bool                       success{false};
        std::optional<mola::id_t>  new_kf_id{std::nullopt};
        std::optional<std::string> error_msg{std::nullopt};
    };

    std::future<ProposeKF_Output> addKeyFrame(const ProposeKF_Input& i)
    {
        return slam_be_threadpool_.enqueue(
            &BackEndBase::doAddKeyFrame, this, i);
    }

    struct AddFactor_Output
    {
        bool                       success{false};
        std::optional<mola::fid_t> new_factor_id{std::nullopt};
        std::optional<std::string> error_msg{std::nullopt};
    };

    std::future<AddFactor_Output> addFactor(Factor& f)
    {
        return slam_be_threadpool_.enqueue(&BackEndBase::doAddFactor, this, f);
    }

    struct AdvertiseUpdatedLocalization_Input
    {
        mrpt::Clock::time_point timestamp{};

        mola::id_t reference_kf{mola::INVALID_ID};

        mrpt::math::TPose3D                        pose;
        std::optional<mrpt::math::CMatrixDouble66> cov{std::nullopt};
    };
    std::future<void> advertiseUpdatedLocalization(
        const AdvertiseUpdatedLocalization_Input& l)
    {
        const auto copyOfL = l;
        return slam_be_threadpool_.enqueue(
            [this, copyOfL]() { doAdvertiseUpdatedLocalization(copyOfL); });
    }

    virtual void onSmartFactorChanged(
        [[maybe_unused]] mola::fid_t             id,
        [[maybe_unused]] const mola::FactorBase* f)
    {
    }

    virtual mola::id_t temp_createStereoCamera(
        [[maybe_unused]] const mrpt::img::TCamera& left,
        [[maybe_unused]] const mrpt::img::TCamera& right,
        [[maybe_unused]] const double              baseline)
    {
        THROW_EXCEPTION("Not implemented in selected back-end!");
    }

    virtual mola::id_t temp_createLandmark(
        [[maybe_unused]]  //
        const mrpt::math::TPoint3D& init_value)
    {
        THROW_EXCEPTION("Not implemented in selected back-end!");
    }

    virtual void lock_slam() {}
    virtual void unlock_slam() {}

    virtual ProposeKF_Output doAddKeyFrame(const ProposeKF_Input& i) = 0;
    virtual AddFactor_Output doAddFactor(Factor& f)                  = 0;
    virtual void             doAdvertiseUpdatedLocalization(
                    const AdvertiseUpdatedLocalization_Input& l) = 0;
   protected:
    WorldModel::Ptr worldmodel_;

    mrpt::WorkerThreadsPool slam_be_threadpool_{
        2, mrpt::WorkerThreadsPool::POLICY_FIFO, "slam_backend"};
};

}  // namespace mola