Class Matcher_Points_Base

Nested Relationships

Nested Types

Inheritance Relationships

Base Type

Derived Types

Class Documentation

class Matcher_Points_Base : public mp2p_icp::Matcher

Pointcloud matcher auxiliary class for iterating over point layers.

The common to all matchers working on points in the local input metric map. See derived classes for possible generated pairings.

Subclassed by mp2p_icp::Matcher_Adaptive, mp2p_icp::Matcher_Point2Line, mp2p_icp::Matcher_Point2Plane, mp2p_icp::Matcher_Points_DistanceThreshold, mp2p_icp::Matcher_Points_InlierRatio

Public Functions

Matcher_Points_Base() = default
virtual void initialize(const mrpt::containers::yaml &params) override

Common parameters to all derived classes:

  • maxLocalPointsPerLayer: Maximum number of local points to consider for the “local” point cloud, per point layer. “0” means “all” (no decimation) [Default=0].

  • localPointsSampleSeed: Only if maxLocalPointsPerLayer!=0, and the number of points in the local map is larger than that number, a seed for the RNG used to pick random point indices. 0 (default) means to use a time-based seed.

  • pointLayerMatches: Optional map of layer names to relative weights. Refer to example YAML files.

  • allowMatchAlreadyMatchedPoints: Optional (Default=false). Whether to find matches for local points which were already paired by other matcher in an earlier stage in the matchers pipeline.

  • bounding_box_intersection_check_epsilon: Optional (Default=0.20). The additional “margin” in all axes (x,y,z) that bounding box is enlarged for checking the feasibility of pairings to exist.

Public Members

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

Weights for each potential Local->Global point layer matching. If empty, the output Pairings::point_weights will left empty (=all points have equal weight).

Note

Note: this field can be loaded from a configuration file via initializeLayerWeights().

Note

Map is: w[“globalLayer”][“localLayer”]=weight;

uint64_t maxLocalPointsPerLayer_ = 0
uint64_t localPointsSampleSeed_ = 0
bool allowMatchAlreadyMatchedPoints_ = false

Whether to allow matching local points that have been already matched by a former Matcher instance in the pipeline.

bool allowMatchAlreadyMatchedGlobalPoints_ = false

If false, global map points can be paired exclusively against one local point.

std::optional<std::size_t> kdtree_leaf_max_points_

Maximum number of points per tree node. Not set: nanoflann default.

double bounding_box_intersection_check_epsilon_ = 0.20

The additional “margin” in all axes (x,y,z) that bounding box is enlarged for checking the feasibility of pairings to exist.

Public Static Functions

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 Functions

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 final override
Returns:

true if the mather is actually invoked, false if disabled.

struct TransformedLocalPointCloud

the output of transform_local_to_global()

Public Members

mrpt::math::TPoint3Df localMin = {fMax, fMax, fMax}
mrpt::math::TPoint3Df localMax = {-fMax, -fMax, -fMax}
std::optional<std::vector<std::size_t>> idxs

Reordering indexes, used only if we had to pick random indexes

mrpt::aligned_std_vector<float> x_locals

Transformed local points: all, or a random subset

mrpt::aligned_std_vector<float> y_locals
mrpt::aligned_std_vector<float> z_locals