| 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 |
} |