MRPT  2.0.4
MonteCarloLocalization_App_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/poses/Lie/SE.h>
13 #include <mrpt/system/filesystem.h>
14 #include <test_mrpt_common.h>
15 #include <iostream>
16 
17 using config_changer_t = std::function<void(mrpt::config::CConfigFileBase&)>;
18 using post_tester_t =
19  std::function<void(mrpt::apps::MonteCarloLocalization_Base&)>;
20 
22 
24  const std::string& ini_filename, const std::string& rawlog_filename,
25  const std::string& map_filename, config_changer_t cfg_changer,
26  post_tester_t post_tester)
27 {
28  using namespace std::string_literals;
29 
30  const auto ini_fil = mrpt::UNITTEST_BASEDIR +
31  "/share/mrpt/config_files/pf-localization/"s +
32  ini_filename;
34 
35  const auto rawlog_fil =
36  mrpt::UNITTEST_BASEDIR + "/share/mrpt/datasets/"s + rawlog_filename;
37  ASSERT_FILE_EXISTS_(rawlog_fil);
38 
39  const auto map_fil =
40  mrpt::UNITTEST_BASEDIR + "/share/mrpt/datasets/"s + map_filename;
41  ASSERT_FILE_EXISTS_(map_fil);
42 
43  try
44  {
45  MCL app;
47 
48  const char* argv[] = {"pf-localization-slam", ini_fil.c_str(),
49  rawlog_fil.c_str()};
50  const int argc = sizeof(argv) / sizeof(argv[0]);
51 
52  app.initialize(argc, argv);
53 
54  app.params.write(
55  MCL::sect, "logOutput_dir",
56  mrpt::system::getTempFileName() + "_dir"s);
57  app.params.write(MCL::sect, "SHOW_PROGRESS_3D_REAL_TIME", false);
58 
59  app.params.write(MCL::sect, "map_file", map_fil);
60 
61  app.fill_out_estimated_path = true;
62  app.allow_quit_on_esc_key = false;
63 
64 #if !MRPT_HAS_OPENCV
65  app.params.write(MCL::sect, "3DSceneFrequency", -1);
66  app.params.write(MCL::sect, "LOG_FREQUENCY", 0);
67 #endif
68 
69  cfg_changer(app.params);
70  app.run();
71 
72  // Check results:
73  post_tester(app);
74  }
75  catch (const std::exception& e)
76  {
77  std::cerr << mrpt::exception_to_str(e);
78  GTEST_FAIL();
79  }
80 }
81 
82 static bool tester_result_ok = true;
83 
86  EXPECT_EQ(o.out_estimated_path.size(), 37U);
87  if (o.out_estimated_path.empty()) return;
88 
89  const mrpt::math::TPose3D p = o.out_estimated_path.rbegin()->second;
90  const auto p_gt = mrpt::math::TPose3D::FromString(
91  "[15.89 -10.0 0.000000 4.8 0.000000 0.000000]");
92 
93  const auto p_err = mrpt::poses::CPose3D(p_gt - p);
94  const double err = mrpt::poses::Lie::SE<3>::log(p_err).norm();
95 
96  if (err < 0.5)
97  {
98  tester_result_ok = true;
99  }
100  else
101  {
102  tester_result_ok = false;
103  std::cerr << "Final pose mismatch (will retry N times):\n"
104  "Expected: "
105  << p_gt.asString()
106  << "\n"
107  "Actual : "
108  << p.asString() << "\n";
109  }
110  };
111 
112 TEST(MonteCarloLocalization_Rawlog, RunForSampleDataset_2D)
113 {
114  using namespace std::string_literals;
115  for (int tries = 0; tries < 5; tries++)
116  {
118  "localization_demo.ini", "localization_demo.rawlog",
119  "localization_demo.simplemap.gz",
121  // Use 2D:
122  cfg.write(MCL::sect, "use_3D_poses", false);
123  },
125 
126  if (tester_result_ok) break;
127  }
128 }
129 
130 TEST(MonteCarloLocalization_Rawlog, RunForSampleDataset_3D)
131 {
132  using namespace std::string_literals;
133  for (int tries = 0; tries < 5; tries++)
134  {
136  "localization_demo.ini", "localization_demo.rawlog",
137  "localization_demo.simplemap.gz",
139  // Use 3D:
140  cfg.write(MCL::sect, "use_3D_poses", true);
141  // 3D requires init in a fixed volume:
142  cfg.write(MCL::sect, "init_PDF_mode", 1);
143  },
145 
146  if (tester_result_ok) break;
147  }
148 }
mrpt::config::CConfigFileMemory params
Populated in initialize().
void run()
Runs with the current parameter set.
void asString(std::string &s) const
Returns a human-readable textual representation of the object (eg: "[x y z yaw pitch roll]"...
Definition: TPose3D.cpp:36
bool allow_quit_on_esc_key
If true, will watch the keyboard and quit when ESC is pushed.
std::function< void(mrpt::apps::ICP_SLAM_App_Base &)> post_tester_t
void setMinLoggingLevel(const VerbosityLevel level)
Set the minimum logging level for which the incoming logs are going to be taken into account...
bool fill_out_estimated_path
Whether to populate out_estimated_path.
This class allows loading and storing values and vectors of different types from a configuration text...
std::string getTempFileName()
Returns the name of a proposed temporary file name.
Definition: filesystem.cpp:283
MonteCarlo (Particle filter) localization wrapper class, reading from a rawlog dataset.
void initialize(int argc, const char **argv)
Initializes the application from CLI parameters.
const std::string ini_fil
void generic_pf_test(const std::string &ini_filename, const std::string &rawlog_filename, const std::string &map_filename, config_changer_t cfg_changer, post_tester_t post_tester)
static bool tester_result_ok
void write(const std::string &section, const std::string &name, enum_t value, const int name_padding_width=-1, const int value_padding_width=-1, const std::string &comment=std::string())
Traits for SE(n), rigid-body transformations in R^n space.
Definition: SE.h:30
const char * argv[]
static TPose3D FromString(const std::string &s)
Definition: TPose3D.h:200
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)
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
std::string exception_to_str(const std::exception &e)
Builds a nice textual representation of a nested exception, which if generated using MRPT macros (THR...
Definition: exceptions.cpp:59
MonteCarlo (Particle filter) localization wrapper class for CLI or custom applications: virtual base ...
const int argc
#define ASSERT_FILE_EXISTS_(FIL)
Definition: filesystem.h:22
static constexpr auto sect
Default name of the main configuration section in INI files for this app.
std::function< void(mrpt::config::CConfigFileBase &)> config_changer_t
TEST(MonteCarloLocalization_Rawlog, RunForSampleDataset_2D)
static auto tester_for_localization_demo



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020