Matcher.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/Pairings.h>
#include <mp2p_icp/Parameterizable.h>
#include <mp2p_icp/metricmap.h>
#include <mp2p_icp/pointcloud_bitfield.h>
#include <mrpt/containers/yaml.h>
#include <mrpt/rtti/CObject.h>
#include <mrpt/system/COutputLogger.h>
#include <mrpt/version.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,
                public mp2p_icp::Parameterizable
{
#if MRPT_VERSION < 0x020e00
    DEFINE_VIRTUAL_MRPT_OBJECT(Matcher)
#else
    DEFINE_VIRTUAL_MRPT_OBJECT(Matcher, mp2p_icp)
#endif
 
   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;  
    bool     enabled          = true;
 
   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