14 #include <mrpt/core/exceptions.h>
15 #include <mrpt/core/round.h>
16 #include <mrpt/version.h>
24 mrpt::system::COutputLogger::setLoggerName(
25 "Matcher_Points_DistanceThreshold");
29 const mrpt::containers::yaml& params)
40 const mrpt::maps::CMetricMap& pcGlobalMap,
41 const mrpt::maps::CPointsMap& pcLocal,
42 const mrpt::poses::CPose3D& localPose,
MatchState& ms,
48 const auto* pcGlobalPtr =
49 dynamic_cast<const mrpt::maps::CPointsMap*
>(&pcGlobalMap);
52 "This class only supports global maps of point cloud types, but "
54 pcGlobalMap.GetRuntimeClass()->className);
55 const auto& pcGlobal = *pcGlobalPtr;
58 if (pcGlobal.empty() || pcLocal.empty())
return;
64 if (!pcGlobal.boundingBox().intersection({tl.localMin, tl.localMax}))
68 out.paired_pt2pt.reserve(
out.paired_pt2pt.size() + pcLocal.size());
72 const float maxDistForCorrespondenceSquared = mrpt::square(
threshold);
74 const auto& gxs = pcGlobal.getPointsBufferRef_x();
75 const auto& gys = pcGlobal.getPointsBufferRef_y();
76 const auto& gzs = pcGlobal.getPointsBufferRef_z();
78 const auto& lxs = pcLocal.getPointsBufferRef_x();
79 const auto& lys = pcLocal.getPointsBufferRef_y();
80 const auto& lzs = pcLocal.getPointsBufferRef_z();
85 std::map<size_t, std::map<float, mrpt::tfest::TMatchingPair>>
86 candidateMatchesForGlobal;
88 const auto lambdaAddPair = [
this, &candidateMatchesForGlobal, &lxs, &lys,
89 &lzs, &gxs, &gys, &gzs, &ms, &globalName](
90 const size_t localIdx,
91 const size_t globalIdx,
const float errSqr) {
94 ms.globalPairedBitField.point_layers.at(globalName).at(globalIdx))
98 auto& p = candidateMatchesForGlobal[globalIdx][errSqr];
100 p.globalIdx = globalIdx;
101 p.localIdx = localIdx;
102 p.global = {gxs[globalIdx], gys[globalIdx], gzs[globalIdx]};
103 p.local = {lxs[localIdx], lys[localIdx], lzs[localIdx]};
105 p.errorSquareAfterTransformation = errSqr;
109 std::vector<size_t> neighborIndices;
110 std::vector<float> neighborSqrDists;
112 for (
size_t i = 0; i < tl.
x_locals.size(); i++)
114 const size_t localIdx = tl.
idxs.has_value() ? (*tl.
idxs)[i] : i;
117 ms.localPairedBitField.point_layers.at(localName).at(localIdx))
129 float tentativeErrSqr;
130 const size_t tentativeGlobalIdx = pcGlobal.kdTreeClosestPoint3D(
136 if (tentativeErrSqr < maxDistForCorrespondenceSquared)
137 lambdaAddPair(localIdx, tentativeGlobalIdx, tentativeErrSqr);
141 pcGlobal.kdTreeNClosestPoint3DIdx(
146 for (
size_t k = 0; k < neighborIndices.size(); k++)
148 const auto tentativeErrSqr = neighborSqrDists.at(k);
149 if (tentativeErrSqr >= maxDistForCorrespondenceSquared)
152 lambdaAddPair(localIdx, neighborIndices.at(k), tentativeErrSqr);
158 for (
const auto& kv : candidateMatchesForGlobal)
160 const auto globalIdx = kv.first;
163 ms.globalPairedBitField.point_layers.at(globalName).at(globalIdx))
166 const auto& pairs = kv.second;
167 ASSERT_(!pairs.empty());
170 const auto& bestPair = pairs.begin()->second;
172 out.paired_pt2pt.emplace_back(bestPair);
175 const auto localIdx = bestPair.localIdx;
176 ms.localPairedBitField.point_layers[localName].at(localIdx) =
true;
177 ms.globalPairedBitField.point_layers[globalName].at(globalIdx) =
true;