Debian Codegraph @

libs/topography/src/path_from_rtk_gps_unittest.cpp

raw
1 /* +------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | https://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2026, Individual contributors, see AUTHORS file |
6 | See: https://www.mrpt.org/Authors - All rights reserved. |
7 | Released under BSD License. See: https://www.mrpt.org/License |
8 +------------------------------------------------------------------------+ */
9
10 #include <gtest/gtest.h>
11 #include <mrpt/poses/CPose3DInterpolator.h>
12 #include <mrpt/system/filesystem.h>
13 #include <mrpt/topography.h>
14 #include <test_mrpt_common.h>
15
16 using namespace mrpt;
17 using namespace mrpt::math;
18 using namespace mrpt::poses;
19 using namespace mrpt::topography;
20 using namespace std;
21
22 TEST(TopographyReconstructPathFrom3RTK, sampleDataset)
23 {
24 mrpt::poses::CPose3DInterpolator robot_path;
25
26 mrpt::obs::CRawlog rawlog;
27
28 const string dataset_fil =
29 UNITTEST_BASEDIR() + string("/share/mrpt/datasets/test_rtk_path.rawlog");
30 if (!mrpt::system::fileExists(dataset_fil))
31 {
32 cerr << "WARNING: Skipping test due to missing file: " << dataset_fil << "\n";
33 return;
34 }
35 if (!rawlog.loadFromRawLogFile(dataset_fil))
36 {
37 cerr << "WARNING: Skipping test due to error loading file: " << dataset_fil << "\n";
38 }
39 else
40 {
41 mrpt::topography::TPathFromRTKInfo rtk_path_info;
42
43 // -------------------------------------------
44 // Run path reconstruction:
45 // -------------------------------------------
46 mrpt::topography::path_from_rtk_gps(
47 robot_path, rawlog,
48 0, // first entry
49 rawlog.size() - 1, // last entry
50 false, // Isn't a GUI
51 false, // disableGPSInterp
52 1, // path_smooth_filter_size
53 &rtk_path_info);
54
55 EXPECT_EQ(robot_path.size(), 75u);
56
57 // clang-format off
58 // Expected values:
59 // 1226225355.000000 279.696222 216.622980 9.213152 0.195764 -0.031973 -0.042048
60 // 1226225380.000000 377.086918 233.311009 10.474043 0.178932 -0.025085 -0.013734
61 // clang-format on
62 const auto t1 = mrpt::Clock::fromDouble(1226225355.0);
63 const auto t2 = mrpt::Clock::fromDouble(1226225380.0);
64 const CPose3D pose_GT_1(279.696222, 216.622980, 9.213152, 0.195764, -0.031973, -0.042048);
65 const CPose3D pose_GT_2(377.086918, 233.311009, 10.474043, 0.178932, -0.025085, -0.013734);
66
67 CPose3D pose1, pose2;
68 bool valid;
69 robot_path.interpolate(t1, pose1, valid);
70 EXPECT_TRUE(valid);
71
72 robot_path.interpolate(t2, pose2, valid);
73 EXPECT_TRUE(valid);
74
75 CVectorDouble p1vec(12), p2vec(12);
76 pose1.getAs12Vector(p1vec);
77 pose2.getAs12Vector(p2vec);
78
79 CVectorDouble p1vec_gt(12), p2vec_gt(12);
80 pose_GT_1.getAs12Vector(p1vec_gt);
81 pose_GT_2.getAs12Vector(p2vec_gt);
82 EXPECT_NEAR((p1vec - p1vec_gt).sum_abs(), 0, 1e-3);
83 EXPECT_NEAR((p2vec - p2vec_gt).sum_abs(), 0, 1e-3);
84 }
85 }