16 #include <mrpt/containers/yaml.h>
17 #include <mrpt/core/exceptions.h>
18 #include <mrpt/maps/CSimplePointsMap.h>
25 int main([[maybe_unused]]
int argc, [[maybe_unused]]
char** argv)
29 const auto inFile =
std::string(
"happy_buddha_decim.xyz.gz");
32 mrpt::maps::CSimplePointsMap::Ptr pts =
35 mrpt::containers::yaml params;
37 params[
"ncols"] = 100;
45 params[
"sigma"] = 0.1;
53 const auto viewPose = mrpt::poses::CPose3D(-0.2, 0, 0, 0, 0, 0);
54 pts->changeCoordinatesReference(viewPose);
59 std::tuple<mrpt::poses::CPose3D, mrpt::poses::CPose3D, double>>
62 {{.0, .0, .0, .0, .0, .0}, {.0, .0, .0, .0, .0, .0}, 1.0},
64 {{1.0, 1.0, .0, .0, .0, .0}, {1.0, 1.0, .0, .0, .0, .0}, 1.0},
66 {{.1, .1, .2, .0, .0, .0}, {.101, .1, .2, .0, .0, .0}, 0.93},
70 for (
const auto& p : testPairs)
72 const mrpt::poses::CPose3D& relPoseGT = std::get<0>(p);
73 const mrpt::poses::CPose3D& relPoseTest = std::get<1>(p);
74 const double expectedVal = std::get<2>(p);
76 auto p1 = mrpt::maps::CSimplePointsMap::Create();
77 p1->insertAnotherMap(pts.get(), mrpt::poses::CPose3D::Identity());
79 auto p2 = mrpt::maps::CSimplePointsMap::Create();
80 p2->insertAnotherMap(pts.get(), relPoseGT);
88 const double quality =
q.evaluate(pcG, pcL, relPoseTest, {});
90 if (std::abs(quality - expectedVal) > 0.1)
92 std::cerr <<
"Failed for test case:\n"
93 <<
" relPoseGT : " << relPoseGT <<
"\n"
94 <<
" relPoseTest : " << relPoseTest <<
"\n"
95 <<
" expectedVal : " << expectedVal <<
"\n"
96 <<
" quality : " << quality <<
"\n";
98 throw std::runtime_error(
"test failed (see cerr above)");
102 catch (std::exception& e)
104 std::cerr << mrpt::exception_to_str(e) <<
"\n";