MRPT  2.0.3
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 }
filesystem.h
generic_rbpf_slam_test
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)
Definition: RBPF_SLAM_App_unittest.cpp:20
mrpt::apps::RBPF_SLAM_App_Base::run
void run()
Runs with the current parameter set.
Definition: RBPF_SLAM_App.cpp:73
SE.h
mrpt::config::CConfigFileBase::write
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())
Definition: config/CConfigFileBase.h:107
EXPECT_TRUE
EXPECT_TRUE(mrpt::system::fileExists(ini_fil))
post_tester_t
std::function< void(mrpt::apps::ICP_SLAM_App_Base &)> post_tester_t
Definition: ICP_SLAM_App_unittest.cpp:18
MRPT_TODO
MRPT_TODO("toPointCloud / calibration")
RBPF_SLAM_App.h
mrpt::apps::RBPF_SLAM_App_Rawlog
Instance of RBPF_SLAM_App_Base to run mapping from an offline dataset file.
Definition: RBPF_SLAM_App.h:81
tester_for_2006_01_21
static auto tester_for_2006_01_21
Definition: RBPF_SLAM_App_unittest.cpp:68
tester_for_ROSLAM_demo
static auto tester_for_ROSLAM_demo
Definition: RBPF_SLAM_App_unittest.cpp:79
ini_fil
const std::string ini_fil
Definition: RawlogGrabberApp_unittest.cpp:26
config_changer_t
std::function< void(mrpt::config::CConfigFileBase &)> config_changer_t
Definition: ICP_SLAM_App_unittest.cpp:17
mrpt::system::getTempFileName
std::string getTempFileName()
Returns the name of a proposed temporary file name.
Definition: filesystem.cpp:283
mrpt::system::fileExists
bool fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
Definition: filesystem.cpp:128
mrpt::math::norm
CONTAINER::Scalar norm(const CONTAINER &v)
Definition: ops_containers.h:133
TEST
TEST(RBPF_SLAM_App, MapFromRawlog_Lidar2D_optimal_sampling)
Definition: RBPF_SLAM_App_unittest.cpp:92
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
mrpt::apps::RBPF_SLAM_App_Base::params
mrpt::config::CConfigFileMemory params
Populated in initialize().
Definition: RBPF_SLAM_App.h:62
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::apps::RBPF_SLAM_App_Base::initialize
void initialize(int argc, const char **argv)
Initializes the application from CLI parameters.
Definition: RBPF_SLAM_App.cpp:46
mrpt::system::COutputLogger::setMinLoggingLevel
void setMinLoggingLevel(const VerbosityLevel level)
Set the minimum logging level for which the incoming logs are going to be taken into account.
Definition: COutputLogger.cpp:144
mrpt::system::LVL_WARN
@ LVL_WARN
Definition: system/COutputLogger.h:32
argv
const char * argv[]
Definition: RawlogGrabberApp_unittest.cpp:40
EXPECT_EQ
EXPECT_EQ(out.image_pair_was_used.size(), NUM_IMGS)
EXPECT_LT
EXPECT_LT(out.final_rmse, 3.0)
mrpt::poses::Lie::SE
Traits for SE(n), rigid-body transformations in R^n space.
Definition: SE.h:30
mrpt::apps::RBPF_SLAM_App_Base
RBPF-SLAM virtual base class for application wrappers.
Definition: RBPF_SLAM_App.h:31
mrpt::poses::CPose3D::FromString
static CPose3D FromString(const std::string &s)
Definition: CPose3D.h:644
argc
const int argc
Definition: RawlogGrabberApp_unittest.cpp:41
mrpt::exception_to_str
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



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 15:49:54 UTC 2020