14 #include <mrpt/core/exceptions.h>
15 #include <mrpt/core/round.h>
16 #include <mrpt/version.h>
24 mrpt::system::COutputLogger::setLoggerName(
"Matcher_Points_InlierRatio");
28 const mrpt::containers::yaml& params)
36 const mrpt::maps::CMetricMap& pcGlobalMap,
37 const mrpt::maps::CPointsMap& pcLocal,
38 const mrpt::poses::CPose3D& localPose,
MatchState& ms,
47 const auto* pcGlobalPtr =
48 dynamic_cast<const mrpt::maps::CPointsMap*
>(&pcGlobalMap);
51 "This class only supports global maps of point cloud types, but "
53 pcGlobalMap.GetRuntimeClass()->className);
54 const auto& pcGlobal = *pcGlobalPtr;
57 if (pcGlobal.empty() || pcLocal.empty())
return;
63 if (!pcGlobal.boundingBox().intersection({tl.localMin, tl.localMax}))
68 const auto& gxs = pcGlobal.getPointsBufferRef_x();
69 const auto& gys = pcGlobal.getPointsBufferRef_y();
70 const auto& gzs = pcGlobal.getPointsBufferRef_z();
72 const auto& lxs = pcLocal.getPointsBufferRef_x();
73 const auto& lys = pcLocal.getPointsBufferRef_y();
74 const auto& lzs = pcLocal.getPointsBufferRef_z();
76 std::multimap<double, mrpt::tfest::TMatchingPair> sortedPairings;
78 for (
size_t i = 0; i < tl.
x_locals.size(); i++)
80 size_t localIdx = tl.
idxs.has_value() ? (*tl.
idxs)[i] : i;
95 float tentativeErrSqr;
96 const size_t tentativeGlobalIdx = pcGlobal.kdTreeClosestPoint3D(
101 mrpt::tfest::TMatchingPair p;
102 p.globalIdx = tentativeGlobalIdx;
103 p.localIdx = localIdx;
105 gxs[tentativeGlobalIdx], gys[tentativeGlobalIdx],
106 gzs[tentativeGlobalIdx]};
107 p.local = {lxs[localIdx], lys[localIdx], lzs[localIdx]};
109 p.errorSquareAfterTransformation = tentativeErrSqr;
112 sortedPairings.emplace_hint(
113 sortedPairings.begin(), tentativeErrSqr, p);
119 const size_t nTotal = sortedPairings.size();
122 const auto nKeep = mrpt::round(
double(nTotal) *
inliersRatio);
125 auto itEnd = sortedPairings.begin();
126 std::advance(itEnd, nKeep);
128 for (
auto it = sortedPairings.begin(); it != itEnd; ++it)
130 const auto localIdx = it->second.localIdx;
131 const auto globalIdx = it->second.globalIdx;
138 out.paired_pt2pt.push_back(it->second);