MRPT  2.0.1
CGridMapAligner_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/config.h>
13 #include <mrpt/math/TPose2D.h>
14 #include <mrpt/math/wrap2pi.h>
15 #include <mrpt/system/filesystem.h>
16 #include <test_mrpt_common.h>
17 
18 #if MRPT_HAS_OPENCV
19 TEST(CGridMapAligner, alignGridMaps)
20 #else
21 TEST(CGridMapAligner, DISABLED_alignGridMaps)
22 #endif
23 {
24  const std::string ini_fil =
25  mrpt::UNITTEST_BASEDIR +
26  std::string(
27  "/share/mrpt/config_files/grid-matching/gridmatch_example.ini");
29 
30  const std::string gridmap1_fil =
31  mrpt::UNITTEST_BASEDIR +
32  std::string("/share/mrpt/datasets/malaga-cs-fac-building.simplemap.gz");
34 
35  try
36  {
37  // Since this uses RANSAC, there is a small chance of random failure:
38  for (int tries = 0; tries < 3; tries++)
39  {
42 
43  const char* argv[] = {
44  "grid-matching", "--config", ini_fil.c_str(),
45  "--map1", gridmap1_fil.c_str(), "--detect-test",
46  "--nologo", "--nosave", "--noicp"};
47  const int argc = sizeof(argv) / sizeof(argv[0]);
48 
49  app.initialize(argc, argv);
50  app.run();
51 
52  // Check result:
53  if (std::abs(app.estimateMean.x() - app.GT_Ax) < 0.1 &&
54  std::abs(app.estimateMean.y() - app.GT_Ay) < 0.1 &&
56  app.estimateMean.phi(), app.GT_Aphi_rad) < 0.05)
57  {
58  // good result.
59  return;
60  }
61  std::cout << "Attempt #" << tries
62  << " failed:\n"
63  "Estimation: "
64  << app.estimateMean.asString()
65  << "\n"
66  "GT: "
68  app.GT_Ax, app.GT_Ay, app.GT_Aphi_rad)
69  .asString();
70  }
71  GTEST_FAIL() << "Ground truth not reached after several attempts.";
72  }
73  catch (const std::exception& e)
74  {
75  std::cerr << mrpt::exception_to_str(e);
76  GTEST_FAIL();
77  }
78 }
void asString(std::string &s) const
Returns a human-readable textual representation of the object (eg: "[x y yaw]", yaw in degrees) ...
Definition: CPose2D.cpp:445
void asString(std::string &s) const
Returns a human-readable textual representation of the object (eg: "[x y yaw]", yaw in degrees) ...
Definition: TPose2D.cpp:24
EXPECT_TRUE(mrpt::system::fileExists(ini_fil))
mrpt::poses::CPose2D estimateMean
A class for aligning two multi-metric maps (with an occupancy grid maps and a points map...
bool fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
Definition: filesystem.cpp:128
T angDistance(T from, T to)
Computes the shortest angular increment (or distance) between two planar orientations, such that it is constrained to [-pi,pi] and is correct for any combination of angles (e.g.
Definition: wrap2pi.h:95
void setMinLoggingLevel(const VerbosityLevel level)
Set the minimum logging level for which the incoming logs are going to be taken into account...
TEST(ICP_SLAM_App, MapFromRawlog_PointMap)
void initialize(int argc, const char **argv)
Initializes the application from CLI parameters.
double phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
const std::string ini_fil
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
grid-matching application wrapper class.
void run()
Runs with the current parameter set.
const char * argv[]
const std::string gridmap1_fil
Lightweight 2D pose.
Definition: TPose2D.h:22
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



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020