Main MRPT website > C++ reference for MRPT 1.5.5
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-2017, 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 
10 
12 
13 #include <gtest/gtest.h>
14 
15 using namespace mrpt;
16 using namespace mrpt::random;
17 using namespace mrpt::utils;
18 using namespace mrpt::poses;
19 using namespace mrpt::graphs;
20 using namespace mrpt::math;
21 using namespace std;
22 
23 template <class my_graph_t>
24 class GraphSlamLevMarqTester : public GraphSlamLevMarqTest<my_graph_t>, public ::testing::Test
25 {
26 protected:
27  virtual void SetUp()
28  {
29  }
30  virtual void TearDown() { }
31 
33  {
34  // This is the initial input graph (make a copy for later use):
35  my_graph_t graph;
37 
38  const my_graph_t graph_initial = graph;
39 
40  // ----------------------------
41  // Run graph slam:
42  // ----------------------------
44  //params["verbose"] = 1;
45  //params["profiler"] = 1;
46  params["max_iterations"] = 1000;
47 
49 
51  graph,
52  levmarq_info,
53  NULL,
54  params
55  );
56 
57  // Do some basic checks on the results:
58  EXPECT_GE(levmarq_info.num_iters, 10U);
59  EXPECT_LE(levmarq_info.final_total_sq_error, 1e-2);
60 
61  } // end test_ring_path
62 
64  {
65  my_graph_t graph;
67 
68  // binary dump:
70  mem << graph;
71 
72  {
73  my_graph_t read_graph;
74  mem.Seek(0);
75  mem >> read_graph;
76 
77  EXPECT_EQ( read_graph.edges.size(), graph.edges.size() );
78  EXPECT_EQ( read_graph.nodes.size(), graph.nodes.size() );
79 
80  // Also check that the edge values are OK:
81  typename my_graph_t::const_iterator it1, it2;
82  for (it1=read_graph.edges.begin(), it2=graph.edges.begin(); it1!=read_graph.edges.end(); ++it1, ++it2)
83  {
84  EXPECT_EQ(it1->first, it2->first);
85  EXPECT_NEAR(0, (it1->second.getPoseMean().getAsVectorVal() - it2->second.getPoseMean().getAsVectorVal() ).array().abs().sum(), 1e-9 );
86  }
87  }
88  }
89 
90 };
91 
92 
95 
96 TEST_F(GraphSlamLevMarqTester2D, OptimizeSampleRingPath)
97 {
98  for (int seed=1;seed<5;seed++)
99  {
101  test_ring_path();
102  }
103 }
104 TEST_F(GraphSlamLevMarqTester2D, BinarySerialization)
105 {
107  test_graph_bin_serialization();
108 }
109 
110 TEST_F(GraphSlamLevMarqTester3D, OptimizeSampleRingPath)
111 {
112  for (int seed=1;seed<5;seed++)
113  {
115  test_ring_path();
116  }
117 }
118 TEST_F(GraphSlamLevMarqTester3D, BinarySerialization)
119 {
121  test_graph_bin_serialization();
122 }
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
Abstract graph and tree data structures, plus generic graph algorithms.
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
void randomize(const uint32_t seed)
Initialize the PRNG from the given random seed.
size_t num_iters
The number of LM iterations executed.
STL namespace.
const Scalar * const_iterator
Definition: eigen_plugins.h:24
TEST_F(GraphSlamLevMarqTester2D, OptimizeSampleRingPath)
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
This CStream derived class allow using a memory buffer as a CStream.
Definition: CMemoryStream.h:26
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()
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)
GraphSlamLevMarqTester< CNetworkOfPoses2D > GraphSlamLevMarqTester2D
GLenum const GLfloat * params
Definition: glext.h:3514
GraphSlamLevMarqTester< CNetworkOfPoses3D > GraphSlamLevMarqTester3D
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.5 Git: e06b63dbf Fri Dec 1 14:41:11 2017 +0100 at lun oct 28 01:31:35 CET 2019