15 #include <mrpt/core/exceptions.h>
16 #include <mrpt/core/round.h>
17 #include <mrpt/version.h>
25 mrpt::system::COutputLogger::setLoggerName(
"Matcher_Point2Line");
33 MCP_LOAD_REQ(params,
knn);
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_pt2pl.reserve(
out.paired_pt2pl.size() + pcLocal.size() / 10);
72 const float maxDistForCorrespondenceSquared =
75 const auto& gxs = pcGlobal.getPointsBufferRef_x();
76 const auto& gys = pcGlobal.getPointsBufferRef_y();
77 const auto& gzs = pcGlobal.getPointsBufferRef_z();
79 const auto& lxs = pcLocal.getPointsBufferRef_x();
80 const auto& lys = pcLocal.getPointsBufferRef_y();
81 const auto& lzs = pcLocal.getPointsBufferRef_z();
83 std::vector<float> kddSqrDist;
84 std::vector<size_t> kddIdxs;
86 for (
size_t i = 0; i < tl.
x_locals.size(); i++)
88 const size_t localIdx = tl.
idxs.has_value() ? (*tl.
idxs)[i] : i;
107 pcGlobal.kdTreeNClosestPoint3DIdx(
110 kddIdxs, kddSqrDist);
115 if (!kddSqrDist.empty() &&
116 kddSqrDist.back() < maxDistForCorrespondenceSquared)
122 for (
size_t j = 0; j < kddSqrDist.size(); j++)
124 if (kddSqrDist[j] > maxDistForCorrespondenceSquared)
127 kddSqrDist.resize(j);
137 gxs.data(), gys.data(), gzs.data(), kddIdxs);
141 std::cout <<
"eig values: " << eig.
eigVals[0] <<
" " << eig.
eigVals[1]
143 <<
" eigvec0: " << eig.
eigVectors[0].asString() <<
"\n"
144 <<
" eigvec1: " << eig.
eigVectors[1].asString() <<
"\n"
145 <<
" eigvec2: " << eig.
eigVectors[2].asString() <<
"\n";
152 auto& p =
out.paired_pt2ln.emplace_back();
153 p.pt_local = {lxs[localIdx], lys[localIdx], lzs[localIdx]};
156 p.ln_global.pBase = {
158 p.ln_global.director = normal.unitarize();