MRPT  2.0.5
RBPF_SLAM_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 = std::function<void(mrpt::apps::RBPF_SLAM_App_Base&)>;
19 
21  const std::string& ini_filename, const std::string& rawlog_filename,
22  config_changer_t cfg_changer, post_tester_t post_tester)
23 {
24  using namespace std::string_literals;
25 
26  const auto ini_fil = mrpt::UNITTEST_BASEDIR +
27  "/share/mrpt/config_files/rbpf-slam/"s + ini_filename;
29 
30  const auto rawlog_fil =
31  mrpt::UNITTEST_BASEDIR + "/share/mrpt/datasets/"s + rawlog_filename;
33 
34  try
35  {
38 
39  const char* argv[] = {"rbpf-slam", ini_fil.c_str(), rawlog_fil.c_str()};
40  const int argc = sizeof(argv) / sizeof(argv[0]);
41 
42  app.initialize(argc, argv);
43 
44  app.params.write(
45  "MappingApplication", "logOutput_dir",
46  mrpt::system::getTempFileName() + "_dir"s);
47  app.params.write(
48  "MappingApplication", "SHOW_PROGRESS_IN_WINDOW", false);
49 
50 #if !MRPT_HAS_OPENCV
51  app.params.write("MappingApplication", "SAVE_3D_SCENE", false);
52  app.params.write("MappingApplication", "LOG_FREQUENCY", 0);
53 #endif
54 
55  cfg_changer(app.params);
56  app.run();
57 
58  // Check results:
59  post_tester(app);
60  }
61  catch (const std::exception& e)
62  {
63  std::cerr << mrpt::exception_to_str(e);
64  GTEST_FAIL();
65  }
66 }
67 
69  EXPECT_EQ(o.out_estimated_path.size(), 224U);
70  const auto p = mrpt::poses::CPose3D(o.out_estimated_path.rbegin()->second);
71  const auto p_gt = mrpt::poses::CPose3D::FromString(
72  "[3.4548 -18.0399 0.000000 -86.48 0.000000 0.000000]");
73 
75  << "actual pose =" << p.asString()
76  << "\nexpected pose=" << p_gt.asString();
77 };
78 
80  EXPECT_EQ(o.out_estimated_path.size(), 99U);
81  const auto p = mrpt::poses::CPose3D(o.out_estimated_path.rbegin()->second);
82  const auto p_gt = mrpt::poses::CPose3D::FromString(
83  "[1.938686 3.352273 0.000000 114.993417 0.000000 0.000000]");
84 
86  << "actual pose =" << p.asString()
87  << "\nexpected pose=" << p_gt.asString();
88 
89  MRPT_TODO("Stricter unit tests: check for estimated landmark positions");
90 };
91 
92 TEST(RBPF_SLAM_App, MapFromRawlog_Lidar2D_optimal_sampling)
93 {
94  using namespace std::string_literals;
95 
97  "gridmapping_optimal_sampling.ini",
98  "2006-01ENE-21-SENA_Telecom Faculty_one_loop_only.rawlog",
100 }
101 
102 TEST(RBPF_SLAM_App, MapFromRawlog_Lidar2D_gridICP)
103 {
104  using namespace std::string_literals;
105 
107  "gridmapping_RBPF_grid_ICPbased_malaga.ini",
108  "2006-01ENE-21-SENA_Telecom Faculty_one_loop_only.rawlog",
110 }
111 
112 TEST(RBPF_SLAM_App, MapFromRawlog_Lidar2D_pointsICP)
113 {
114  using namespace std::string_literals;
115 
117  "gridmapping_RBPF_ICPbased_malaga.ini",
118  "2006-01ENE-21-SENA_Telecom Faculty_one_loop_only.rawlog",
120 }
121 
122 TEST(RBPF_SLAM_App, MapFromRawlog_ROSLAM_MC)
123 {
124  using namespace std::string_literals;
125 
127  "RO-SLAM_simulatedData_MC.ini", "RO-SLAM_demo.rawlog",
129 }
130 
131 TEST(RBPF_SLAM_App, MapFromRawlog_ROSLAM_SOG)
132 {
133  using namespace std::string_literals;
134 
136  "RO-SLAM_simulatedData_SOG.ini", "RO-SLAM_demo.rawlog",
138 }
EXPECT_LT(out.final_rmse, 3.0)
Instance of RBPF_SLAM_App_Base to run mapping from an offline dataset file.
Definition: RBPF_SLAM_App.h:81
void run()
Runs with the current parameter set.
EXPECT_TRUE(mrpt::system::fileExists(ini_fil))
static CPose3D FromString(const std::string &s)
Definition: CPose3D.h:644
TEST(RBPF_SLAM_App, MapFromRawlog_Lidar2D_optimal_sampling)
std::function< void(mrpt::apps::ICP_SLAM_App_Base &)> post_tester_t
bool fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
Definition: filesystem.cpp:128
void setMinLoggingLevel(const VerbosityLevel level)
Set the minimum logging level for which the incoming logs are going to be taken into account...
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
void initialize(int argc, const char **argv)
Initializes the application from CLI parameters.
static auto tester_for_2006_01_21
const std::string ini_fil
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())
RBPF-SLAM virtual base class for application wrappers.
Definition: RBPF_SLAM_App.h:31
Traits for SE(n), rigid-body transformations in R^n space.
Definition: SE.h:30
void asString(std::string &s) const
Returns a human-readable textual representation of the object (eg: "[x y z yaw pitch roll]"...
Definition: CPose3D.h:618
const char * argv[]
mrpt::config::CConfigFileMemory params
Populated in initialize().
Definition: RBPF_SLAM_App.h:62
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)
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
MRPT_TODO("toPointCloud / calibration")
const int argc
static auto tester_for_ROSLAM_demo
std::function< void(mrpt::config::CConfigFileBase &)> config_changer_t
CONTAINER::Scalar norm(const CONTAINER &v)
void generic_rbpf_slam_test(const std::string &ini_filename, const std::string &rawlog_filename, config_changer_t cfg_changer, post_tester_t post_tester)



Page generated by Doxygen 1.8.14 for MRPT 2.0.5 Git: e84511500 Thu Jul 2 23:53:34 2020 +0200 at vie jul 3 00:00:10 CEST 2020