14 #include <mrpt/gui/CDisplayWindow.h>
15 #include <mrpt/img/TPixelCoord.h>
16 #include <mrpt/io/vector_loadsave.h>
19 QualityEvaluator_RangeImageSimilarity, QualityEvaluator,
mp2p_icp)
24 const mrpt::containers::yaml& params)
34 MCP_LOAD_OPT(params,
sigma);
43 const mrpt::poses::CPose3D& localPose,
44 [[maybe_unused]]
const Pairings& pairingsFromICP)
const
58 auto s1 =
scores(I11, I21);
59 auto s2 =
scores(I12, I22);
61 const size_t nScores = s1.size() + s2.size();
64 for (
double v : s1) sum += v;
65 for (
double v : s2) sum += v;
67 const double finalScore = nScores > 0 ? sum / nScores : .0;
72 mrpt::gui::CDisplayWindow win_11(
"I11", 400, 300);
73 mrpt::gui::CDisplayWindow win_12(
"I12", 400, 300);
74 mrpt::gui::CDisplayWindow win_21(
"I21", 400, 300);
75 mrpt::gui::CDisplayWindow win_22(
"I22", 400, 300);
77 win_12.setPos(410 + 5, 5);
78 win_21.setPos(5, 310 + 5);
79 win_22.setPos(410 + 5, 310 + 5);
80 mrpt::img::CImage im11, im12, im21, im22;
81 im11.setFromMatrix(I11,
false);
82 win_11.showImage(im11);
83 im12.setFromMatrix(I12,
false);
84 win_12.showImage(im12);
85 im21.setFromMatrix(I21,
false);
86 win_21.showImage(im21);
87 im22.setFromMatrix(I22,
false);
88 win_22.showImage(im22);
93 static std::atomic_int iv = 0;
95 I11.saveToTextFile(mrpt::format(
"I11_%05i.txt", i));
96 I22.saveToTextFile(mrpt::format(
"I22_%05i.txt", i));
97 I12.saveToTextFile(mrpt::format(
"I12_%05i.txt", i));
98 I21.saveToTextFile(mrpt::format(
"I21_%05i.txt", i));
100 mrpt::io::vectorToTextFile(s1, mrpt::format(
"I1_scores_%05i.txt", i));
101 mrpt::io::vectorToTextFile(s2, mrpt::format(
"I2_scores_%05i.txt", i));
110 const mrpt::math::TPoint3D&
P,
const mrpt::img::TCamera& params,
111 double& pixel_x,
double& pixel_y)
127 const double x = -
P.y /
P.x;
128 const double y = -
P.z /
P.x;
130 pixel_x = params.cx() + params.fx() *
x;
131 pixel_y = params.cy() + params.fy() * y;
135 const mrpt::maps::CPointsMap& pts,
136 const std::optional<mrpt::poses::CPose3D>& relativePose)
const
140 mrpt::math::CMatrixDouble I(rc.nrows, rc.ncols);
143 const auto& xs = pts.getPointsBufferRef_x();
144 const auto& ys = pts.getPointsBufferRef_y();
145 const auto& zs = pts.getPointsBufferRef_z();
147 const auto nPoints = xs.size();
148 size_t nValidPoints = 0;
149 for (
size_t i = 0; i < nPoints; i++)
151 mrpt::math::TPoint3D p(xs[i], ys[i], zs[i]);
152 if (relativePose) p = relativePose->composePoint(p);
156 int pixx =
static_cast<int>(px);
157 int pixy =
static_cast<int>(py);
160 if (pixx < 0 || pixy < 0 || pixx >=
int(rc.ncols) ||
161 pixy >= int(rc.nrows))
164 const double newRange = p.norm();
165 double& storedRange = I(pixy, pixx);
167 if (storedRange == 0 || newRange < storedRange) storedRange = newRange;
177 static double phi(
double x) {
return std::erf(
x / std::sqrt(2)); }
181 const double x = std::abs(DeltaRange / sigma);
186 const mrpt::math::CMatrixDouble& m1,
187 const mrpt::math::CMatrixDouble& m2)
const
189 ASSERT_EQUAL_(m1.rows(), m2.rows());
190 ASSERT_EQUAL_(m1.cols(), m2.cols());
192 std::vector<double>
scores;
193 scores.reserve(m1.rows() * m1.cols() / 4);
195 const size_t N = m1.rows() * m1.cols();
196 for (
size_t i = 0; i < N; i++)
198 const double r1 = m1.data()[i];
199 const double r2 = m2.data()[i];
201 if (r1 == 0 && r2 == 0)
continue;
204 if (r1 == 0 || r2 == 0)