Matcher.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/Pairings.h>
#include <mp2p_icp/metricmap.h>
#include <mrpt/containers/yaml.h>
#include <mrpt/rtti/CObject.h>
#include <mrpt/system/COutputLogger.h>

namespace mp2p_icp
{
struct MatchContext
{
    MatchContext() = default;

    uint32_t icpIteration = 0;
};

struct MatchState
{
    MatchState(const metric_map_t& pcGlobal, const metric_map_t& pcLocal)
        : pcGlobal_(pcGlobal), pcLocal_(pcLocal)
    {
        initialize();
    }

    pointcloud_bitfield_t localPairedBitField;

    pointcloud_bitfield_t globalPairedBitField;

    void initialize()
    {
        localPairedBitField.initialize_from(pcLocal_);
        globalPairedBitField.initialize_from(pcGlobal_);
    }

   private:
    const metric_map_t& pcGlobal_;
    const metric_map_t& pcLocal_;
};

class Matcher : public mrpt::system::COutputLogger, public mrpt::rtti::CObject
{
    DEFINE_VIRTUAL_MRPT_OBJECT(Matcher)

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

    virtual bool match(
        const metric_map_t& pcGlobal, const metric_map_t& pcLocal,
        const mrpt::poses::CPose3D& localPose, const MatchContext& mc,
        MatchState& ms, Pairings& out) const;

    uint32_t runFromIteration = 0;
    uint32_t runUpToIteration = 0;  

   protected:
    virtual bool impl_match(
        const metric_map_t& pcGlobal, const metric_map_t& pcLocal,
        const mrpt::poses::CPose3D& localPose, const MatchContext& mc,
        MatchState& ms, Pairings& out) const = 0;
};

using matcher_list_t = std::vector<mp2p_icp::Matcher::Ptr>;

Pairings run_matchers(
    const matcher_list_t& matchers, const metric_map_t& pcGlobal,
    const metric_map_t& pcLocal, const mrpt::poses::CPose3D& local_wrt_global,
    const MatchContext&                   mc,
    const mrpt::optional_ref<MatchState>& userProvidedMS = std::nullopt);

}  // namespace mp2p_icp