Main MRPT website > C++ reference for MRPT 1.5.6
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-2017, 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 
11 #include <mrpt/topography.h>
13 #include <mrpt/system/filesystem.h>
14 #include <gtest/gtest.h>
15 
16 using namespace mrpt;
17 using namespace mrpt::utils;
18 using namespace mrpt::math;
19 using namespace mrpt::poses;
20 using namespace mrpt::topography;
21 using namespace std;
22 
23 // Defined in tests/test_main.cpp
24 namespace mrpt { namespace utils {
26  }
27 }
28 
29 
30 TEST(TopographyReconstructPathFrom3RTK, sampleDataset )
31 {
33 
34  mrpt::obs::CRawlog rawlog;
35 
36  const string dataset_fil = MRPT_GLOBAL_UNITTEST_SRC_DIR + string("/share/mrpt/datasets/test_rtk_path.rawlog");
37  if (!mrpt::system::fileExists(dataset_fil))
38  {
39  cerr << "WARNING: Skipping test due to missing file: " << dataset_fil << "\n";
40  return;
41  }
42  if (!rawlog.loadFromRawLogFile(dataset_fil))
43  {
44  cerr << "WARNING: Skipping test due to error loading file: " << dataset_fil << "\n";
45  }
46  else
47  {
49 
50  // -------------------------------------------
51  // Run path reconstruction:
52  // -------------------------------------------
54  robot_path,
55  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 -0.045293
67  //1226225380.000000 377.095830 233.343569 9.724171 0.177037 -0.073565 -0.019024
68  const mrpt::system::TTimeStamp t1 = mrpt::system::time_tToTimestamp( 1226225355.000000 );
69  const mrpt::system::TTimeStamp t2 = mrpt::system::time_tToTimestamp( 1226225380.000000 );
70  const CPose3D pose_GT_1(279.696, 216.623, 9.21315, 0.195764, -0.0319733, -0.0420478 );
71  const CPose3D pose_GT_2(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 
89  EXPECT_NEAR( (p1vec - p1vec_gt).array().abs().sum(), 0, 1e-3);
90  EXPECT_NEAR( (p2vec - p2vec_gt).array().abs().sum(), 0, 1e-3);
91  }
92 }
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:30
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
std::string MRPT_GLOBAL_UNITTEST_SRC_DIR
TEST(TopographyReconstructPathFrom3RTK, sampleDataset)
bool BASE_IMPEXP fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
Definition: filesystem.cpp:124
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
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:204
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
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: obs/CRawlog.h:52
GLsizei const GLchar ** string
Definition: glext.h:3919
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
This class stores a time-stamped trajectory in SE(3) (CPose3D poses).
This namespace provides topography helper functions, coordinate transformations.
Definition: conversions.h:25
mrpt::system::TTimeStamp BASE_IMPEXP 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:48
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:376
size_t size() const
Returns the number of actions / observations object in the sequence.
Definition: CRawlog.cpp:83
void TOPO_IMPEXP 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=NULL)
Reconstruct the path of a vehicle equipped with 3 RTK GPSs.



Page generated by Doxygen 1.8.14 for MRPT 1.5.6 Git: 4c65e8431 Tue Apr 24 08:18:17 2018 +0200 at lun oct 28 01:35:26 CET 2019