Main MRPT website > C++ reference for MRPT 1.5.7
graph_slam_levmarq_unittest.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
11 
12 #include <gtest/gtest.h>
14 #include <mrpt/system/filesystem.h>
15 #include <set>
16 #include <map>
17 
18 // Defined in tests/test_main.cpp
19 namespace mrpt
20 {
21 namespace utils
22 {
24 }
25 }
26 
27 using namespace mrpt;
28 using namespace mrpt::random;
29 using namespace mrpt::poses;
30 using namespace mrpt::graphs;
31 using namespace mrpt::math;
32 using namespace mrpt::utils;
33 using namespace std;
34 
35 // Define in/out files for testing:
36 using in_out_filenames = std::set<std::pair<std::string, std::string>>;
37 const std::map<std::string, in_out_filenames> inout_graph_files{
38  { "GraphTester2D",
39  { { "graphslam_SE2_in.graph", "graphslam_SE2_out_good.graph" },
40  { "graphslam_SE2_in2.graph", "graphslam_SE2_out_good2.graph" },
41  { "graphslam_SE2_in3.graph", "graphslam_SE2_out_good3.graph" } }
42  } ,
43  { "GraphTester2DInf",
44  { { "graphslam_SE2_in.graph", "graphslam_SE2_out_good.graph" },
45  { "graphslam_SE2_in2.graph", "graphslam_SE2_out_good2.graph" },
46  { "graphslam_SE2_in3.graph", "graphslam_SE2_out_good3.graph" },
47  { "graphslam_SE2pdf_in.graph", "graphslam_SE2pdf_out_good.graph" } } } };
48 
49 template <class my_graph_t>
50 class GraphTester : public GraphSlamLevMarqTest<my_graph_t>,
51  public ::testing::Test
52 {
53  protected:
54  virtual void SetUp() {}
55  virtual void TearDown() {}
57  {
58  // This is the initial input graph (make a copy for later use):
59  my_graph_t graph;
61 
62  const my_graph_t graph_initial = graph;
63 
64  // ----------------------------
65  // Run graph slam:
66  // ----------------------------
68  //params["verbose"] = 1;
69  params["max_iterations"] = 100;
70 
72 
74  graph, levmarq_info, nullptr, params);
75 
76  const double err_init = graph_initial.chi2();
77  const double err_end = graph.chi2();
78 
79  // Do some basic checks on the results:
80  EXPECT_GE(levmarq_info.num_iters, 2U);
81  EXPECT_LE(levmarq_info.final_total_sq_error, 5e-2);
82  EXPECT_LT(err_end, err_init);
83 
84  } // end test_ring_path
85 
87  const my_graph_t& g1, const my_graph_t& g2,
88  const double eps_node_pos = 1e-3, const double eps_edges = 1e-3)
89  {
90  EXPECT_EQ(g1.edges.size(), g2.edges.size());
91  EXPECT_EQ(g1.nodes.size(), g2.nodes.size());
92  EXPECT_EQ(g1.root, g2.root);
93 
94  if (g1.edges.size() != g2.edges.size() ||
95  g1.nodes.size() != g2.nodes.size())
96  return;
97 
98  // Check that the edge values are OK:
99  {
100  typename my_graph_t::const_iterator it1, it2;
101  for (it1 = g1.edges.begin(), it2 = g2.edges.begin();
102  it1 != g1.edges.end(); ++it1, ++it2)
103  {
104  EXPECT_EQ(it1->first, it2->first);
105  EXPECT_NEAR(
106  0,
107  (it1->second.getPoseMean().getAsVectorVal() -
108  it2->second.getPoseMean().getAsVectorVal())
109  .array()
110  .abs()
111  .maxCoeff(),
112  eps_edges);
113  }
114  }
115 
116  // Check nodes:
117  {
118  auto itn1 = g1.nodes.cbegin(), itn2 = g2.nodes.cbegin();
119  for (; itn1 != g1.nodes.cend(); ++itn1, ++itn2)
120  {
121  EXPECT_EQ(itn1->first, itn2->first);
122  EXPECT_NEAR(
123  0,
124  (itn1->second.getAsVectorVal() -
125  itn2->second.getAsVectorVal())
126  .array()
127  .abs()
128  .maxCoeff(),
129  eps_node_pos)
130  << "Poses of keyframe #" << itn1->first << " do not match:" << std::endl
131  << "- Expected: " << itn2->second
132  << std::endl << "- Got : " << itn1->second << std::endl;
133  }
134  }
135  }
136 
138  {
139  my_graph_t graph;
141  // text write:
142  std::stringstream ss;
143  graph.writeAsText(ss);
144  // read:
145  my_graph_t read_graph;
146  ss.seekg(0); // rewind
147  read_graph.readAsText(ss);
148 
149  compare_two_graphs(graph, read_graph);
150  }
151 
153  {
154  my_graph_t graph;
156  // binary write:
157  CMemoryStream mem;
158  mem << graph;
159  // read:
160  my_graph_t read_graph;
161  mem.Seek(0);
162  mem >> read_graph;
163 
164  compare_two_graphs(graph, read_graph);
165  }
166 
168  {
169  auto files_it = inout_graph_files.find(type);
170  if (files_it == inout_graph_files.end())
171  return; // No tests for this type
172 
173  const string prefix = MRPT_GLOBAL_UNITTEST_SRC_DIR + string("/tests/");
174 
175  for (const auto& tst : files_it->second)
176  {
177  std::cout << "Testing graph type `" << type << "`, in_file=`"
178  << tst.first << "`" << std::endl;
179 
180  const string in_f = prefix + tst.first;
181  ASSERT_FILE_EXISTS_(in_f);
182  const string good_f = prefix + tst.second;
183  ASSERT_FILE_EXISTS_(good_f);
184 
185  my_graph_t graph, graph_good;
186  graph.loadFromTextFile(in_f);
187  graph_good.loadFromTextFile(good_f);
188  ASSERT_(graph.nodeCount() > 1);
189  ASSERT_EQ(graph.nodeCount(), graph_good.nodeCount());
190  ASSERT_EQ(graph.edgeCount(), graph_good.edgeCount());
191 
192  // Optimize:
193  const my_graph_t graph_initial = graph;
195  //params["verbose"] = 1;
196  params["max_iterations"] = 100;
197 
199 
201  graph, levmarq_info, nullptr, params);
202 
203  const double err_init = graph_initial.chi2();
204  const double err_end = graph.chi2();
205  const double err_good = graph_good.chi2();
206  /* DEBUG */
207 #if 1
208  std::cout << "err_init: " << err_init << std::endl;
209  std::cout << "err_end: " << err_end << std::endl;
210  std::cout << "err_good: " << err_good << std::endl;
211  graph.saveToTextFile("out.graph");
212 #endif
213  // Do some basic checks on the results:
214  EXPECT_GE(levmarq_info.num_iters, 2U);
215  EXPECT_LE(levmarq_info.final_total_sq_error, 0.2);
216  EXPECT_LT(err_end, err_init);
217 
218  // Compare to good solution:
219  compare_two_graphs(graph, graph_good);
220  }
221  }
222 };
223 
228 
229 #define GRAPHS_TESTS(_TYPE) \
230  TEST_F(_TYPE, OptimizeSampleRingPath) \
231  { \
232  for (int seed = 1; seed < 3; seed++) \
233  { \
234  randomGenerator.randomize(seed); \
235  test_ring_path(); \
236  } \
237  } \
238  TEST_F(_TYPE, BinarySerialization) \
239  { \
240  randomGenerator.randomize(123); \
241  test_graph_bin_serialization(); \
242  } \
243  TEST_F(_TYPE, WriteReadTextFile) \
244  { \
245  randomGenerator.randomize(123); \
246  test_graph_text_serialization(); \
247  } \
248  TEST_F(_TYPE, OptimizeCompareKnownSolution) \
249  { \
250  test_optimize_compare_known_solution(#_TYPE); \
251  }
252 
253 MRPT_TODO("Re-enable tests after https://github.com/MRPT/mrpt/issues/770");
254 
256 //GRAPHS_TESTS(GraphTester3D)
258 //GRAPHS_TESTS(GraphTester3DInf)
259 
A namespace of pseudo-random numbers genrators of diferent distributions.
void optimize_graph_spa_levmarq(GRAPH_T &graph, TResultInfoSpaLevMarq &out_info, const std::set< mrpt::utils::TNodeID > *in_nodes_to_optimize=NULL, const mrpt::utils::TParametersDouble &extra_params=mrpt::utils::TParametersDouble(), typename graphslam_traits< GRAPH_T >::TFunctorFeedback functor_feedback=NULL)
Optimize a graph of pose constraints using the Sparse Pose Adjustment (SPA) sparse representation and...
Definition: levmarq.h:56
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
std::string MRPT_GLOBAL_UNITTEST_SRC_DIR
#define GRAPHS_TESTS(_TYPE)
Abstract graph and tree data structures, plus generic graph algorithms.
size_t num_iters
The number of LM iterations executed.
STL namespace.
const Scalar * const_iterator
Definition: eigen_plugins.h:24
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
std::set< std::pair< std::string, std::string > > in_out_filenames
const std::map< std::string, in_out_filenames > inout_graph_files
This CStream derived class allow using a memory buffer as a CStream.
Definition: CMemoryStream.h:26
MRPT_TODO("Re-enable tests after https://github.com/MRPT/mrpt/issues/770")
GLsizei const GLchar ** string
Definition: glext.h:3919
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Output information for mrpt::graphslam::optimize_graph_spa_levmarq()
#define ASSERT_(f)
#define ASSERT_FILE_EXISTS_(FIL)
uint64_t Seek(int64_t Offset, CStream::TSeekOrigin Origin=sFromBeginning) MRPT_OVERRIDE
Introduces a pure virtual method for moving to a specified position in the streamed resource...
static void create_ring_path(my_graph_t &graph, size_t N_VERTEX=50, double DIST_THRES=7, double NODES_XY_MAX=20)
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)
GLenum const GLfloat * params
Definition: glext.h:3514
GLuint GLuint GLsizei GLenum type
Definition: glext.h:3512
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.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019