Main MRPT website > C++ reference for MRPT 1.9.9
path_from_rtk_gps_unittest.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include <mrpt/topography.h>
12 #include <mrpt/system/filesystem.h>
13 #include <gtest/gtest.h>
14 
15 using namespace mrpt;
16 using namespace mrpt::math;
17 using namespace mrpt::poses;
18 using namespace mrpt::topography;
19 using namespace std;
20 
21 // Defined in tests/test_main.cpp
22 namespace mrpt
23 {
25 }
26 
27 TEST(TopographyReconstructPathFrom3RTK, sampleDataset)
28 {
30 
31  mrpt::obs::CRawlog rawlog;
32 
33  const string dataset_fil =
35  string("/share/mrpt/datasets/test_rtk_path.rawlog");
36  if (!mrpt::system::fileExists(dataset_fil))
37  {
38  cerr << "WARNING: Skipping test due to missing file: " << dataset_fil
39  << "\n";
40  return;
41  }
42  if (!rawlog.loadFromRawLogFile(dataset_fil))
43  {
44  cerr << "WARNING: Skipping test due to error loading file: "
45  << dataset_fil << "\n";
46  }
47  else
48  {
50 
51  // -------------------------------------------
52  // Run path reconstruction:
53  // -------------------------------------------
55  robot_path, rawlog,
56  0, // first entry
57  rawlog.size() - 1, // last entry
58  false, // Isn't a GUI
59  false, // disableGPSInterp
60  1, // path_smooth_filter_size
61  &rtk_path_info);
62 
63  EXPECT_EQ(robot_path.size(), 75u);
64 
65  // Expected values:
66  // 1226225355.000000 279.705647 216.651473 8.517821 0.194222 -0.083873
67  // -0.045293
68  // 1226225380.000000 377.095830 233.343569 9.724171 0.177037 -0.073565
69  // -0.019024
70  const mrpt::system::TTimeStamp t1 =
71  mrpt::system::time_tToTimestamp(1226225355.000000);
72  const mrpt::system::TTimeStamp t2 =
73  mrpt::system::time_tToTimestamp(1226225380.000000);
74  const CPose3D pose_GT_1(
75  279.696, 216.623, 9.21315, 0.195764, -0.0319733, -0.0420478);
76  const CPose3D pose_GT_2(
77  377.087, 233.311, 10.474, 0.178932, -0.0212096, -0.0154982);
78 
79  CPose3D pose1, pose2;
80  bool valid;
81  robot_path.interpolate(t1, pose1, valid);
82  EXPECT_TRUE(valid);
83 
84  robot_path.interpolate(t2, pose2, valid);
85  EXPECT_TRUE(valid);
86 
87  CVectorDouble p1vec(12), p2vec(12);
88  pose1.getAs12Vector(p1vec);
89  pose2.getAs12Vector(p2vec);
90 
91  CVectorDouble p1vec_gt(12), p2vec_gt(12);
92  pose_GT_1.getAs12Vector(p1vec_gt);
93  pose_GT_2.getAs12Vector(p2vec_gt);
94 
95  EXPECT_NEAR((p1vec - p1vec_gt).array().abs().sum(), 0, 1e-3);
96  EXPECT_NEAR((p2vec - p2vec_gt).array().abs().sum(), 0, 1e-3);
97  }
98 }
TEST(TopographyReconstructPathFrom3RTK, sampleDataset)
bool fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
Definition: filesystem.cpp:127
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:44
STL namespace.
Used to return optional information from mrpt::topography::path_from_rtk_gps.
pose_t & interpolate(mrpt::system::TTimeStamp 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...
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:190
This base provides a set of functions for maths stuff.
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
This class stores a rawlog (robotic datasets) in one of two possible formats:
Definition: CRawlog.h:68
GLsizei const GLchar ** string
Definition: glext.h:4101
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
std::string MRPT_GLOBAL_UNITTEST_SRC_DIR
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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:88
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:31
This class stores a time-stamped trajectory in SE(3) (CPose3D poses).
This namespace provides topography helper functions, coordinate transformations.
Definition: conversions.h:22
mrpt::system::TTimeStamp time_tToTimestamp(const double t)
Transform from standard "time_t" (actually a double number, it can contain fractions of seconds) to T...
Definition: datetime.cpp:50
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:505
size_t size() const
Returns the number of actions / observations object in the sequence.
Definition: CRawlog.cpp:88



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019