Generator.h

Go to the documentation of this file.

/* -------------------------------------------------------------------------
 * A repertory of multi primitive-to-primitive (MP2P) ICP algorithms in C++
 * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria
 * See LICENSE for license information.
 * ------------------------------------------------------------------------- */
#pragma once
 
#include <mp2p_icp/Parameterizable.h>
#include <mp2p_icp/metricmap.h>
#include <mrpt/containers/yaml.h>
#include <mrpt/maps/CPointsMap.h>
#include <mrpt/obs/obs_frwds.h>
#include <mrpt/rtti/CObject.h>
#include <mrpt/system/COutputLogger.h>
 
#include <regex>
#include <stdexcept>
 
namespace mp2p_icp_filters
{
struct NotImplementedError : public std::runtime_error
{
    // NotImplementedError() = default;
    template <typename T>
    NotImplementedError(T v) : std::runtime_error(v)
    {
    }
};
 
class Generator : public mrpt::rtti::CObject,  // RTTI support
                  public mrpt::system::COutputLogger,  // Logging support
                  public mp2p_icp::Parameterizable  // Dynamic parameters
{
    DEFINE_MRPT_OBJECT(Generator, mp2p_icp_filters)
 
   public:
    Generator();
 
    virtual void initialize(const mrpt::containers::yaml& cfg_block);
 
    virtual bool process(
        const mrpt::obs::CObservation& input_raw, mp2p_icp::metric_map_t& inOut,
        const std::optional<mrpt::poses::CPose3D>& robotPose = std::nullopt) const;
 
    struct Parameters
    {
        void load_from_yaml(const mrpt::containers::yaml& c, Generator& parent);
 
        std::string target_layer = mp2p_icp::metric_map_t::PT_LAYER_RAW;
 
        std::string metric_map_definition_ini_file;
 
        mrpt::containers::yaml metric_map_definition;
 
        std::string process_class_names_regex = ".*";
 
        std::string process_sensor_labels_regex = ".*";
 
        bool throw_on_unhandled_observation_class = false;
    };
 
    Parameters params_;
 
   protected:
    // To be overrided in derived classes, if implemented:
    virtual bool filterScan2D(
        const mrpt::obs::CObservation2DRangeScan& pc, mp2p_icp::metric_map_t& out,
        const std::optional<mrpt::poses::CPose3D>& robotPose) const;
    virtual bool filterScan3D(
        const mrpt::obs::CObservation3DRangeScan& pc, mp2p_icp::metric_map_t& out,
        const std::optional<mrpt::poses::CPose3D>& robotPose) const;
    virtual bool filterVelodyneScan(
        const mrpt::obs::CObservationVelodyneScan& pc, mp2p_icp::metric_map_t& out,
        const std::optional<mrpt::poses::CPose3D>& robotPose) const;
    virtual bool filterPointCloud(
        const mrpt::maps::CPointsMap& pc, const mrpt::poses::CPose3D& sensorPose,
        mp2p_icp::metric_map_t& out, const std::optional<mrpt::poses::CPose3D>& robotPose) const;
    virtual bool filterRotatingScan(
        const mrpt::obs::CObservationRotatingScan& pc, mp2p_icp::metric_map_t& out,
        const std::optional<mrpt::poses::CPose3D>& robotPose) const;
 
    bool       initialized_ = false;
    std::regex process_class_names_regex_;
    std::regex process_sensor_labels_regex_;
 
   private:
    bool implProcessDefault(
        const mrpt::obs::CObservation& o, mp2p_icp::metric_map_t& out,
        const std::optional<mrpt::poses::CPose3D>& robotPose = std::nullopt) const;
 
    bool implProcessCustomMap(
        const mrpt::obs::CObservation& o, mp2p_icp::metric_map_t& out,
        const std::optional<mrpt::poses::CPose3D>& robotPose = std::nullopt) const;
 
    void internalLoadUserPlugin(const std::string& moduleToLoad) const;
};
 
using GeneratorSet = std::vector<Generator::Ptr>;
 
bool apply_generators(
    const GeneratorSet& generators, const mrpt::obs::CObservation& obs,
    mp2p_icp::metric_map_t&                    output,
    const std::optional<mrpt::poses::CPose3D>& robotPose = std::nullopt);
 
[[nodiscard]] mp2p_icp::metric_map_t apply_generators(
    const GeneratorSet& generators, const mrpt::obs::CObservation& obs,
    const std::optional<mrpt::poses::CPose3D>& robotPose = std::nullopt);
 
bool apply_generators(
    const GeneratorSet& generators, const mrpt::obs::CSensoryFrame& sf,
    mp2p_icp::metric_map_t&                    output,
    const std::optional<mrpt::poses::CPose3D>& robotPose = std::nullopt);
 
[[nodiscard]] mp2p_icp::metric_map_t apply_generators(
    const GeneratorSet& generators, const mrpt::obs::CSensoryFrame& sf,
    const std::optional<mrpt::poses::CPose3D>& robotPose = std::nullopt);
 
[[nodiscard]] GeneratorSet generators_from_yaml(
    const mrpt::containers::yaml&       c,
    const mrpt::system::VerbosityLevel& vLevel = mrpt::system::LVL_INFO);
 
[[nodiscard]] GeneratorSet generators_from_yaml_file(
    const std::string&                  filename,
    const mrpt::system::VerbosityLevel& vLevel = mrpt::system::LVL_INFO);
 
}  // namespace mp2p_icp_filters