14 #include <mrpt/maps/COccupancyGridMap3D.h>
15 #include <mrpt/maps/CSimplePointsMap.h>
16 #include <mrpt/version.h>
24 mrpt::system::COutputLogger::setLoggerName(
"QualityEvaluator_Voxels");
28 const mrpt::containers::yaml& params)
36 static double loss(
double x,
double y)
50 return 1.0 - 2 *
x - 2 * y + 4 *
x * y;
55 const mrpt::poses::CPose3D& localPose,
56 [[maybe_unused]]
const Pairings& pairingsFromICP)
const
62 mrpt::maps::COccupancyGridMap3D voxelsGlo({-r, -r, -r}, {r, r, r}, r);
63 mrpt::maps::COccupancyGridMap3D voxelsLoc({-r, -r, -r}, {r, r, r}, r);
66 auto& io = voxelsGlo.insertionOptions;
73 auto itG = pcGlobal.
layers.find(ly);
74 auto itL = pcLocal.
layers.find(ly);
75 if (itG == pcGlobal.
layers.end() || itL == pcLocal.
layers.end())
78 "Layer `%s` not found in both global/local layers.",
84 std::dynamic_pointer_cast<mrpt::maps::CPointsMap>(itG->second);
86 std::dynamic_pointer_cast<mrpt::maps::CPointsMap>(itL->second);
90 mrpt::maps::CSimplePointsMap localTransformed;
91 localTransformed.insertAnotherMap(ptsL.get(), localPose);
94 MRPT_TODO(
"Check against current size too, for many layers");
96 const auto bb = ptsG->boundingBox();
97 voxelsGlo.resizeGrid(bb.min, bb.max);
100 const auto bb = localTransformed.boundingBox();
101 voxelsLoc.resizeGrid(bb.min, bb.max);
104 voxelsGlo.insertPointCloud({0, 0, 0}, *ptsG);
105 voxelsLoc.insertPointCloud(
106 mrpt::math::TPoint3D(localPose.asTPose()), localTransformed);
111 const auto& g = voxelsGlo.m_grid;
112 const auto& l = voxelsLoc.m_grid;
116 size_t dist_cells = 0;
118 for (
size_t igx = 0; igx < g.getSizeX(); igx++)
120 const int ilx = l.x2idx(g.idx2x(igx));
121 if (ilx < 0 ||
static_cast<size_t>(ilx) >= l.getSizeX())
continue;
123 for (
size_t igy = 0; igy < g.getSizeY(); igy++)
125 const int ily = l.y2idx(g.idx2y(igy));
126 if (ily < 0 ||
static_cast<size_t>(ily) >= l.getSizeY())
continue;
128 for (
size_t igz = 0; igz < g.getSizeZ(); igz++)
130 const int ilz = l.z2idx(g.idx2z(igz));
131 if (ilz < 0 ||
static_cast<size_t>(ilz) >= l.getSizeZ())
136 const auto gCell = *g.cellByIndex(igx, igy, igz);
137 const auto lCell = *l.cellByIndex(ilx, ily, ilz);
139 const float gProb = voxelsGlo.l2p(gCell);
140 const float lProb = voxelsLoc.l2p(lCell);
142 if (std::abs(gProb - 0.5) < 0.01 ||
143 std::abs(lProb - 0.5) < 0.01)
146 const double d =
loss(lProb, gProb);
162 MRPT_LOG_WARN_STREAM(
163 "dist: " << dist <<
" dist_cells: " << dist_cells
164 <<
" quality: " << quality);