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