MRPT  1.9.9
graph_slam_levmarq_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 
11 
12 #include <gtest/gtest.h>
14 #include <mrpt/system/filesystem.h>
15 #include <test_mrpt_common.h>
16 
17 using namespace mrpt;
18 using namespace mrpt::random;
19 using namespace mrpt::poses;
20 using namespace mrpt::graphs;
21 using namespace mrpt::math;
22 using namespace std;
23 
24 // Define in/out files for testing:
25 using in_out_filenames = std::set<std::tuple<std::string, std::string>>;
26 const std::map<std::string, in_out_filenames> inout_graph_files{
27  {"GraphTester2D",
28  {{"graphslam_SE2_in.graph", "graphslam_SE2_out_good.graph"},
29  {"graphslam_SE2_in2.graph", "graphslam_SE2_out_good2.graph"},
30  {"graphslam_SE2_in3.graph", "graphslam_SE2_out_good3.graph"}}},
31  {"GraphTester2DInf",
32  {{"graphslam_SE2_in.graph", "graphslam_SE2_out_good.graph"},
33  {"graphslam_SE2pdf_in.graph", "graphslam_SE2pdf_out_good.graph"}}},
34  {"GraphTester3D",
35  {{"graphslam_SE3_in_torus3D-first100.graph",
36  "graphslam_SE3_out_good_torus3D-first100.graph"},
37  {"graphslam_SE2_in2.graph", "graphslam_SE2_out_good2.graph"}}}};
38 
39 template <class my_graph_t>
40 class GraphTester : public GraphSlamLevMarqTest<my_graph_t>,
41  public ::testing::Test
42 {
43  protected:
44  void SetUp() override {}
45  void TearDown() override {}
46  void test_ring_path(const char* className)
47  {
48  // This is the initial input graph (make a copy for later use):
49  my_graph_t graph;
51 
52  const my_graph_t graph_initial = graph;
53 
54  // ----------------------------
55  // Run graph slam:
56  // ----------------------------
58  // params["verbose"] = 1;
59  params["max_iterations"] = 100;
60 
62 
64  graph, levmarq_info, nullptr, params);
65 
66  const double err_init = graph_initial.chi2();
67  const double err_end = graph.chi2();
68  std::cout << "err_init: " << err_init << std::endl;
69  std::cout << "err_end: " << err_end << std::endl;
70  // graph_initial.saveToTextFile(
71  // string("in_") + string(className) + string(".graph"));
72 
73  // Do some basic checks on the results:
74  EXPECT_GE(levmarq_info.num_iters, 2U);
75  EXPECT_LE(levmarq_info.final_total_sq_error, 5e-2);
76  EXPECT_LT(err_end, err_init);
77 
78  } // end test_ring_path
79 
81  const my_graph_t& g1, const my_graph_t& g2,
82  const double eps_node_pos = 1e-3, const double eps_edges = 1e-3)
83  {
84  EXPECT_EQ(g1.edges.size(), g2.edges.size());
85  EXPECT_EQ(g1.nodes.size(), g2.nodes.size());
86  EXPECT_EQ(g1.root, g2.root);
87 
88  if (g1.edges.size() != g2.edges.size() ||
89  g1.nodes.size() != g2.nodes.size())
90  return;
91 
92  // Check that the edge values are OK:
93  {
94  typename my_graph_t::const_iterator it1, it2;
95  for (it1 = g1.edges.begin(), it2 = g2.edges.begin();
96  it1 != g1.edges.end(); ++it1, ++it2)
97  {
98  EXPECT_EQ(it1->first, it2->first);
100  0,
101  (it1->second.getPoseMean().asVectorVal() -
102  it2->second.getPoseMean().asVectorVal())
103  .sum_abs(),
104  eps_edges);
105  }
106  }
107 
108  // Check nodes:
109  {
110  auto itn1 = g1.nodes.cbegin(), itn2 = g2.nodes.cbegin();
111  for (; itn1 != g1.nodes.cend(); ++itn1, ++itn2)
112  {
113  EXPECT_EQ(itn1->first, itn2->first);
114  EXPECT_NEAR(
115  0,
116  (itn1->second.asVectorVal() - itn2->second.asVectorVal())
117  .sum_abs(),
118  eps_node_pos)
119  << "Poses of keyframe #" << itn1->first
120  << " do not match:" << std::endl
121  << "- Expected: " << itn2->second << std::endl
122  << "- Got : " << itn1->second << std::endl;
123  }
124  }
125  }
126 
128  {
129  my_graph_t graph;
131  // text write:
132  std::stringstream ss;
133  graph.writeAsText(ss);
134  // read:
135  my_graph_t read_graph;
136  ss.seekg(0); // rewind
137  read_graph.readAsText(ss);
138 
139  compare_two_graphs(graph, read_graph);
140  }
141 
143  {
144  my_graph_t graph;
146  // binary write:
148  auto arch = mrpt::serialization::archiveFrom(mem);
149  arch << graph;
150  // read:
151  my_graph_t read_graph;
152  mem.Seek(0);
153  arch >> read_graph;
154 
155  compare_two_graphs(graph, read_graph);
156  }
157 
159  {
160  auto files_it = inout_graph_files.find(type);
161  if (files_it == inout_graph_files.end())
162  return; // No tests for this type
163 
164  const string prefix = UNITTEST_BASEDIR + string("/tests/");
165  for (const auto& tst : files_it->second)
166  {
167  std::cout << "Testing graph type `" << type << "`, in_file=`"
168  << std::get<0>(tst) << "`" << std::endl;
169 
170  const string in_f = prefix + std::get<0>(tst);
171  ASSERT_FILE_EXISTS_(in_f);
172  const string good_f = prefix + std::get<1>(tst);
173  ASSERT_FILE_EXISTS_(good_f);
174 
175  my_graph_t graph, graph_good;
176  graph.loadFromTextFile(in_f);
177  graph_good.loadFromTextFile(good_f);
178  ASSERT_(graph.nodeCount() > 1);
179  ASSERT_EQ(graph.nodeCount(), graph_good.nodeCount());
180  ASSERT_EQ(graph.edgeCount(), graph_good.edgeCount());
181 
182  // Optimize:
183  const my_graph_t graph_initial = graph;
185  params["max_iterations"] = 100;
186 
188 
190  graph, levmarq_info, nullptr, params);
191 
192  /* DEBUG */
193  const double err_init = graph_initial.chi2();
194  const double err_end = graph.chi2();
195  const double err_good = graph_good.chi2();
196  std::cout << "err_init: " << err_init << std::endl;
197  std::cout << "err_end: " << err_end << std::endl;
198  std::cout << "err_good: " << err_good << std::endl;
199 
200  // Do some basic checks on the results:
201  EXPECT_GE(levmarq_info.num_iters, 2U);
202  EXPECT_LE(levmarq_info.final_total_sq_error, 0.2);
203  EXPECT_LT(err_end, err_init);
204 
205  // Compare to good solution:
206  compare_two_graphs(graph, graph_good);
207  }
208  }
209 };
210 
215 
216 #define GRAPHS_TESTS(_TYPE) \
217  TEST_F(_TYPE, OptimizeSampleRingPath) \
218  { \
219  for (int seed = 1; seed <= 3; seed++) \
220  { \
221  getRandomGenerator().randomize(seed); \
222  test_ring_path(#_TYPE); \
223  } \
224  } \
225  TEST_F(_TYPE, BinarySerialization) \
226  { \
227  getRandomGenerator().randomize(123); \
228  test_graph_bin_serialization(); \
229  } \
230  TEST_F(_TYPE, WriteReadTextFile) \
231  { \
232  getRandomGenerator().randomize(123); \
233  test_graph_text_serialization(); \
234  } \
235  TEST_F(_TYPE, OptimizeCompareKnownSolution) \
236  { \
237  test_optimize_compare_known_solution(#_TYPE); \
238  }
239 
A namespace of pseudo-random numbers generators of diferent distributions.
EXPECT_LT(out.final_rmse, 3.0)
#define GRAPHS_TESTS(_TYPE)
void optimize_graph_spa_levmarq(GRAPH_T &graph, TResultInfoSpaLevMarq &out_info, const std::set< mrpt::graphs::TNodeID > *in_nodes_to_optimize=nullptr, const mrpt::system::TParametersDouble &extra_params=mrpt::system::TParametersDouble(), FEEDBACK_CALLABLE functor_feedback=FEEDBACK_CALLABLE())
Optimize a graph of pose constraints using the Sparse Pose Adjustment (SPA) sparse representation and...
Definition: levmarq.h:79
Abstract graph and tree data structures, plus generic graph algorithms.
std::set< std::tuple< std::string, std::string > > in_out_filenames
size_t num_iters
The number of LM iterations executed.
void test_ring_path(const char *className)
STL namespace.
CArchiveStreamBase< STREAM > archiveFrom(STREAM &s)
Helper function to create a templatized wrapper CArchive object for a: MRPT&#39;s CStream, std::istream, std::ostream, std::stringstream.
Definition: CArchive.h:591
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
This base provides a set of functions for maths stuff.
const std::map< std::string, in_out_filenames > inout_graph_files
This CStream derived class allow using a memory buffer as a CStream.
uint64_t Seek(int64_t Offset, CStream::TSeekOrigin Origin=sFromBeginning) override
Introduces a pure virtual method for moving to a specified position in the streamed resource...
GLsizei const GLchar ** string
Definition: glext.h:4116
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
EXPECT_EQ(out.image_pair_was_used.size(), NUM_IMGS)
Output information for mrpt::graphslam::optimize_graph_spa_levmarq()
static void create_ring_path(my_graph_t &graph, size_t N_VERTEX=50, double DIST_THRES=7, double NODES_XY_MAX=20)
EXPECT_NEAR(out.cam_params.rightCameraPose.x, 0.1194, 0.005)
void compare_two_graphs(const my_graph_t &g1, const my_graph_t &g2, const double eps_node_pos=1e-3, const double eps_edges=1e-3)
#define ASSERT_FILE_EXISTS_(FIL)
Definition: filesystem.h:22
GLenum const GLfloat * params
Definition: glext.h:3538
GLuint GLuint GLsizei GLenum type
Definition: glext.h:3532
void test_optimize_compare_known_solution(const char *type)
double final_total_sq_error
The sum of all the squared errors for every constraint involved in the problem.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 6e6d28d09 Wed Nov 13 18:58:42 2019 +0100 at miƩ nov 13 19:00:09 CET 2019