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,
79 const mrpt::maps::NearestNeighborsCapable& nnGlobal =
82 out.potential_pairings += pcLocal.size();
85 if (pcGlobalMap.isEmpty() || pcLocal.empty())
return;
91 if (!pcGlobalMap.boundingBox().intersection(
92 {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& lxs = pcLocal.getPointsBufferRef_x();
104 const auto& lys = pcLocal.getPointsBufferRef_y();
105 const auto& lzs = pcLocal.getPointsBufferRef_z();
107 std::vector<float> kddSqrDist, kddSqrDistLocal;
108 std::vector<size_t> kddIdxs, kddIdxsLocal;
109 std::vector<mrpt::math::TPoint3Df> kddPts, kddPtsLocal;
110 std::vector<float> kddXs, kddYs, kddZs;
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;
131 nnGlobal.nn_multiple_search(
133 knn, kddPts, kddSqrDist, kddIdxs);
137 kddIdxs, kddSqrDist, maxDistForCorrespondenceSquared);
145 kddXs.data(), kddYs.data(), kddZs.data(), std::nullopt,
150 std::cout <<
"eig values: " << eig.
eigVals[0] <<
" " << eig.
eigVals[1]
152 <<
" eigvec0: " << eig.
eigVectors[0].asString() <<
"\n"
153 <<
" eigvec1: " << eig.
eigVectors[1].asString() <<
"\n"
154 <<
" eigvec2: " << eig.
eigVectors[2].asString() <<
"\n";
161 const mrpt::math::TPoint3D planeCentroid = {
164 const auto thePlane = mrpt::math::TPlane(planeCentroid, normal);
165 const double ptPlaneDist = std::abs(thePlane.distance({lx, ly, lz}));
172 pcLocal.kdTreeNClosestPoint3DIdx(
174 lxs[localIdx], lys[localIdx], lzs[localIdx],
176 knn, kddIdxsLocal, kddSqrDistLocal);
179 kddIdxsLocal, kddSqrDistLocal, maxDistForCorrespondenceSquared);
193 const auto& normalLocal = eigLocal.
eigVectors[0];
195 const float dotProd = normalLocal.x * normal.x +
196 normalLocal.y * normal.y +
197 normalLocal.z * normal.z;
204 auto& p =
out.paired_pt2pl.emplace_back();
205 p.pt_local = {lxs[localIdx], lys[localIdx], lzs[localIdx]};
206 p.pl_global.centroid = planeCentroid;
208 p.pl_global.plane = thePlane;