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-2025 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 <mrpt/version.h>

#include <future>
#include <optional>

namespace mola
{
class BackEndBase : public ExecutableBase
{
#if MRPT_VERSION < 0x020e00
  DEFINE_VIRTUAL_MRPT_OBJECT(BackEndBase)
#else
  DEFINE_VIRTUAL_MRPT_OBJECT(BackEndBase, mola)
#endif

 public:
  BackEndBase();

  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