Generator.h

Go to the documentation of this file.

/* -------------------------------------------------------------------------
 * A repertory of multi primitive-to-primitive (MP2P) ICP algorithms in C++
 * Copyright (C) 2018-2021 Jose Luis Blanco, University of Almeria
 * See LICENSE for license information.
 * ------------------------------------------------------------------------- */
#pragma once

#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 <cstdint>
#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
{
    DEFINE_MRPT_OBJECT(Generator, mp2p_icp_filters)

   public:
    Generator();

    virtual void initialize(const mrpt::containers::yaml& cfg_block);

    virtual void process(
        const mrpt::obs::CObservation& input_raw,
        mp2p_icp::metric_map_t&        inOut) const;

    struct Parameters
    {
        void load_from_yaml(const mrpt::containers::yaml& c);

        std::string target_pointcloud_layer =
            mp2p_icp::metric_map_t::PT_LAYER_RAW;

        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;
    virtual bool filterScan3D(
        const mrpt::obs::CObservation3DRangeScan& pc,
        mp2p_icp::metric_map_t&                   out) const;
    virtual bool filterVelodyneScan(
        const mrpt::obs::CObservationVelodyneScan& pc,
        mp2p_icp::metric_map_t&                    out) const;
    virtual bool filterPointCloud(
        const mrpt::maps::CPointsMap& pc, mp2p_icp::metric_map_t& out) const;
    virtual bool filterRotatingScan(
        const mrpt::obs::CObservationRotatingScan& pc,
        mp2p_icp::metric_map_t&                    out) const;

    bool       initialized_ = false;
    std::regex process_class_names_regex_;
    std::regex process_sensor_labels_regex_;
};

using GeneratorSet = std::vector<Generator::Ptr>;

void apply_generators(
    const GeneratorSet& generators, const mrpt::obs::CObservation& obs,
    mp2p_icp::metric_map_t& output);

mp2p_icp::metric_map_t apply_generators(
    const GeneratorSet& generators, const mrpt::obs::CObservation& obs);

void apply_generators(
    const GeneratorSet& generators, const mrpt::obs::CSensoryFrame& sf,
    mp2p_icp::metric_map_t& output);

mp2p_icp::metric_map_t apply_generators(
    const GeneratorSet& generators, const mrpt::obs::CSensoryFrame& sf);

GeneratorSet generators_from_yaml(
    const mrpt::containers::yaml&       c,
    const mrpt::system::VerbosityLevel& vLevel = mrpt::system::LVL_INFO);

GeneratorSet generators_from_yaml_file(
    const std::string&                  filename,
    const mrpt::system::VerbosityLevel& vLevel = mrpt::system::LVL_INFO);

}  // namespace mp2p_icp_filters