Matcher_Points_Base.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/Matcher.h>
#include <mrpt/math/TPoint3D.h>

#include <cstdlib>
#include <limits>  // std::numeric_limits
#include <optional>
#include <vector>

namespace mp2p_icp
{
class Matcher_Points_Base : public Matcher
{
   public:
    Matcher_Points_Base() = default;

    std::map<std::string, std::map<std::string, double>> weight_pt2pt_layers;

    uint64_t maxLocalPointsPerLayer_ = 0;
    uint64_t localPointsSampleSeed_  = 0;

    bool allowMatchAlreadyMatchedPoints_ = false;

    bool allowMatchAlreadyMatchedGlobalPoints_ = false;

    std::optional<std::size_t> kdtree_leaf_max_points_;

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

    struct TransformedLocalPointCloud
    {
       public:
        mrpt::math::TPoint3Df localMin{fMax, fMax, fMax};
        mrpt::math::TPoint3Df localMax{-fMax, -fMax, -fMax};

        std::optional<std::vector<std::size_t>> idxs;

        mrpt::aligned_std_vector<float> x_locals, y_locals, z_locals;

       private:
        static constexpr auto fMax = std::numeric_limits<float>::max();
    };

    static TransformedLocalPointCloud transform_local_to_global(
        const mrpt::maps::CPointsMap& pcLocal,
        const mrpt::poses::CPose3D&   localPose,
        const std::size_t             maxLocalPoints        = 0,
        const uint64_t                localPointsSampleSeed = 0);

   protected:
    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 override final;

   private:
    virtual void implMatchOneLayer(
        const mrpt::maps::CMetricMap& pcGlobal,
        const mrpt::maps::CPointsMap& pcLocal,
        const mrpt::poses::CPose3D& localPose, MatchState& ms,
        const layer_name_t& globalName, const layer_name_t& localName,
        Pairings& out) const = 0;
};

}  // namespace mp2p_icp