15 #include <mrpt/core/exceptions.h>
16 #include <mrpt/core/round.h>
17 #include <mrpt/version.h>
25 mrpt::system::COutputLogger::setLoggerName(
"Matcher_Point2Plane");
34 MCP_LOAD_REQ(params,
knn);
45 std::vector<size_t>& kddIdxs, std::vector<float>& kddSqrDist,
46 const float maxDistForCorrespondenceSquared)
49 if (!kddSqrDist.empty() &&
50 kddSqrDist.back() < maxDistForCorrespondenceSquared)
56 for (
size_t j = 0; j < kddSqrDist.size(); j++)
58 if (kddSqrDist[j] > maxDistForCorrespondenceSquared)
69 const mrpt::maps::CMetricMap& pcGlobalMap,
70 const mrpt::maps::CPointsMap& pcLocal,
71 const mrpt::poses::CPose3D& localPose,
MatchState& ms,
77 const auto* pcGlobalPtr =
78 dynamic_cast<const mrpt::maps::CPointsMap*
>(&pcGlobalMap);
81 "This class only supports global maps of point cloud types, but "
83 pcGlobalMap.GetRuntimeClass()->className);
84 const auto& pcGlobal = *pcGlobalPtr;
87 if (pcGlobal.empty() || pcLocal.empty())
return;
93 if (!pcGlobal.boundingBox().intersection({tl.localMin, tl.localMax}))
97 out.paired_pt2pl.reserve(
out.paired_pt2pl.size() + pcLocal.size() / 10);
101 const float maxDistForCorrespondenceSquared = mrpt::square(
searchRadius);
103 const auto& gxs = pcGlobal.getPointsBufferRef_x();
104 const auto& gys = pcGlobal.getPointsBufferRef_y();
105 const auto& gzs = pcGlobal.getPointsBufferRef_z();
107 const auto& lxs = pcLocal.getPointsBufferRef_x();
108 const auto& lys = pcLocal.getPointsBufferRef_y();
109 const auto& lzs = pcLocal.getPointsBufferRef_z();
111 std::vector<float> kddSqrDist, kddSqrDistLocal;
112 std::vector<size_t> kddIdxs, kddIdxsLocal;
114 for (
size_t i = 0; i < tl.
x_locals.size(); i++)
116 const size_t localIdx = tl.
idxs.has_value() ? (*tl.
idxs)[i] : i;
135 pcGlobal.kdTreeNClosestPoint3DIdx(
138 kddIdxs, kddSqrDist);
142 kddIdxs, kddSqrDist, maxDistForCorrespondenceSquared);
148 gxs.data(), gys.data(), gzs.data(), kddIdxs);
152 std::cout <<
"eig values: " << eig.
eigVals[0] <<
" " << eig.
eigVals[1]
154 <<
" eigvec0: " << eig.
eigVectors[0].asString() <<
"\n"
155 <<
" eigvec1: " << eig.
eigVectors[1].asString() <<
"\n"
156 <<
" eigvec2: " << eig.
eigVectors[2].asString() <<
"\n";
163 const mrpt::math::TPoint3D planeCentroid = {
166 const auto thePlane = mrpt::math::TPlane(planeCentroid, normal);
167 const double ptPlaneDist = std::abs(thePlane.distance({lx, ly, lz}));
174 pcLocal.kdTreeNClosestPoint3DIdx(
176 lxs[localIdx], lys[localIdx], lzs[localIdx],
178 knn, kddIdxsLocal, kddSqrDistLocal);
181 kddIdxsLocal, kddSqrDistLocal, maxDistForCorrespondenceSquared);
195 const auto& normalLocal = eigLocal.
eigVectors[0];
197 const float dotProd = normalLocal.x * normal.x +
198 normalLocal.y * normal.y +
199 normalLocal.z * normal.z;
206 auto& p =
out.paired_pt2pl.emplace_back();
207 p.pt_local = {lxs[localIdx], lys[localIdx], lzs[localIdx]};
208 p.pl_global.centroid = planeCentroid;
210 p.pl_global.plane = thePlane;