MRPT  2.0.1
path_from_rtk_gps_unittest.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, 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>
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 {
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
33  << "\n";
34  return;
35  }
36  if (!rawlog.loadFromRawLogFile(dataset_fil))
37  {
38  cerr << "WARNING: Skipping test due to error loading file: "
39  << dataset_fil << "\n";
40  }
41  else
42  {
44 
45  // -------------------------------------------
46  // Run path reconstruction:
47  // -------------------------------------------
49  robot_path, rawlog,
50  0, // first entry
51  rawlog.size() - 1, // last entry
52  false, // Isn't a GUI
53  false, // disableGPSInterp
54  1, // path_smooth_filter_size
55  &rtk_path_info);
56 
57  EXPECT_EQ(robot_path.size(), 75u);
58 
59  // Expected values:
60  // 1226225355.000000 279.705647 216.651473 8.517821 0.194222 -0.083873
61  // -0.045293
62  // 1226225380.000000 377.095830 233.343569 9.724171 0.177037 -0.073565
63  // -0.019024
64  const mrpt::Clock::time_point t1(
65  std::chrono::seconds(1226225355 + 11644473600));
66  const mrpt::Clock::time_point t2(
67  std::chrono::seconds(1226225380 + 11644473600));
68  const CPose3D pose_GT_1(
69  279.696, 216.623, 9.21315, 0.195764, -0.0319733, -0.0420478);
70  const CPose3D pose_GT_2(
71  377.087, 233.311, 10.474, 0.178932, -0.0212096, -0.0154982);
72 
73  CPose3D pose1, pose2;
74  bool valid;
75  robot_path.interpolate(t1, pose1, valid);
76  EXPECT_TRUE(valid);
77 
78  robot_path.interpolate(t2, pose2, valid);
79  EXPECT_TRUE(valid);
80 
81  CVectorDouble p1vec(12), p2vec(12);
82  pose1.getAs12Vector(p1vec);
83  pose2.getAs12Vector(p2vec);
84 
85  CVectorDouble p1vec_gt(12), p2vec_gt(12);
86  pose_GT_1.getAs12Vector(p1vec_gt);
87  pose_GT_2.getAs12Vector(p2vec_gt);
88  EXPECT_NEAR((p1vec - p1vec_gt).sum_abs(), 0, 1e-3);
89  EXPECT_NEAR((p2vec - p2vec_gt).sum_abs(), 0, 1e-3);
90  }
91 }
EXPECT_TRUE(mrpt::system::fileExists(ini_fil))
std::chrono::time_point< Clock > time_point
Definition: Clock.h:25
TEST(TopographyReconstructPathFrom3RTK, sampleDataset)
bool fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
Definition: filesystem.cpp:128
STL namespace.
Used to return optional information from mrpt::topography::path_from_rtk_gps.
bool loadFromRawLogFile(const std::string &fileName, bool non_obs_objects_are_legal=false)
Load the contents from a file containing one of these possibilities:
Definition: CRawlog.cpp:166
This base provides a set of functions for maths stuff.
This class stores a rawlog (robotic datasets) in one of two possible formats:
Definition: CRawlog.h:65
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
pose_t & interpolate(const mrpt::Clock::time_point &t, pose_t &out_interp, bool &out_valid_interp) const
Returns the pose at a given time, or interpolates using splines if there is not an exact match...
void path_from_rtk_gps(mrpt::poses::CPose3DInterpolator &robot_path, const mrpt::obs::CRawlog &rawlog, size_t rawlog_first, size_t rawlog_last, bool isGUI=false, bool disableGPSInterp=false, int path_smooth_filter_size=2, TPathFromRTKInfo *outInfo=nullptr)
Reconstruct the path of a vehicle equipped with 3 RTK GPSs.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
EXPECT_EQ(out.image_pair_was_used.size(), NUM_IMGS)
This class stores a time-stamped trajectory in SE(3) (CPose3D poses).
EXPECT_NEAR(out.cam_params.rightCameraPose.x, 0.1194, 0.005)
This namespace provides topography helper functions, coordinate transformations.
Definition: conversions.h:21
void getAs12Vector(ARRAYORVECTOR &vec12) const
Get the pose representation as an array with these 12 elements: [r11 r21 r31 r12 r22 r32 r13 r23 r33 ...
Definition: CPose3D.h:523
size_t size() const
Returns the number of actions / observations object in the sequence.
Definition: CRawlog.cpp:64



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020