MRPT  1.9.9
ICP_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-2019, 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>
11 #include <mrpt/apps/ICP_SLAM_App.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::ICP_SLAM_App_Base&)>;
19 
20 template <class SLAM_CLASS>
22  const std::string& ini_filename, const std::string& rawlog_filename,
23  config_changer_t cfg_changer, post_tester_t post_tester)
24 {
25  using namespace std::string_literals;
26 
27  const auto ini_fil = mrpt::UNITTEST_BASEDIR +
28  "/share/mrpt/config_files/icp-slam/"s + ini_filename;
30 
31  const auto rawlog_fil =
32  mrpt::UNITTEST_BASEDIR + "/share/mrpt/datasets/"s + rawlog_filename;
34 
35  try
36  {
37  SLAM_CLASS app;
38  app.setMinLoggingLevel(mrpt::system::LVL_WARN);
39 
40  const char* argv[] = {"icp-slam", ini_fil.c_str(), rawlog_fil.c_str()};
41  const int argc = sizeof(argv) / sizeof(argv[0]);
42 
43  app.initialize(argc, argv);
44 
45  app.params.write(
46  "MappingApplication", "logOutput_dir",
47  mrpt::system::getTempFileName() + "_dir"s);
48  app.params.write(
49  "MappingApplication", "SHOW_PROGRESS_3D_REAL_TIME", false);
50 
51 #if !MRPT_HAS_OPENCV
52  app.params.write("MappingApplication", "SAVE_3D_SCENE", false);
53  app.params.write("MappingApplication", "LOG_FREQUENCY", 0);
54 #endif
55 
56  cfg_changer(app.params);
57  app.run();
58 
59  // Check results:
60  post_tester(app);
61  }
62  catch (const std::exception& e)
63  {
64  std::cerr << mrpt::exception_to_str(e);
65  GTEST_FAIL();
66  }
67 }
68 
70  EXPECT_EQ(o.out_estimated_path.size(), 224U);
71  const auto p = mrpt::poses::CPose3D(o.out_estimated_path.rbegin()->second);
72  const auto p_gt = mrpt::poses::CPose3D::FromString(
73  "[3.4548 -18.0399 0.000000 -86.48 0.000000 0.000000]");
74 
76  << "actual pose =" << p.asString()
77  << "\nexpected pose=" << p_gt.asString();
78 };
79 
80 TEST(ICP_SLAM_App, MapFromRawlog_PointMap)
81 {
82  using namespace std::string_literals;
83 
84  generic_icp_slam_test<mrpt::apps::ICP_SLAM_App_Rawlog>(
85  "icp-slam_demo_classic.ini",
86  "2006-01ENE-21-SENA_Telecom Faculty_one_loop_only.rawlog",
88 }
89 
90 TEST(ICP_SLAM_App, MapFromRawlog_Grid)
91 {
92  using namespace std::string_literals;
93 
94  generic_icp_slam_test<mrpt::apps::ICP_SLAM_App_Rawlog>(
95  "icp-slam_demo_classic_gridmatch.ini",
96  "2006-01ENE-21-SENA_Telecom Faculty_one_loop_only.rawlog",
98 }
99 
100 TEST(ICP_SLAM_App, MapFromRawlog_LM)
101 {
102  using namespace std::string_literals;
103 
104  generic_icp_slam_test<mrpt::apps::ICP_SLAM_App_Rawlog>(
105  "icp-slam_demo_LM.ini",
106  "2006-01ENE-21-SENA_Telecom Faculty_one_loop_only.rawlog",
108 }
EXPECT_LT(out.final_rmse, 3.0)
EXPECT_TRUE(mrpt::system::fileExists(ini_fil))
static CPose3D FromString(const std::string &s)
Definition: CPose3D.h:644
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
ICP-SLAM virtual base class for application wrappers.
Definition: ICP_SLAM_App.h:31
TEST(ICP_SLAM_App, MapFromRawlog_PointMap)
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
const std::string ini_fil
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[]
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
const int argc
static auto tester_for_2006_01_21
std::function< void(mrpt::config::CConfigFileBase &)> config_changer_t
void generic_icp_slam_test(const std::string &ini_filename, const std::string &rawlog_filename, config_changer_t cfg_changer, post_tester_t post_tester)
CONTAINER::Scalar norm(const CONTAINER &v)



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 24b95e159 Thu Jan 23 01:15:46 2020 +0100 at jue ene 23 01:30:10 CET 2020