Main MRPT website > C++ reference for MRPT 1.9.9
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 
15 using namespace mrpt;
16 using namespace mrpt::random;
17 using namespace mrpt::poses;
18 using namespace mrpt::graphs;
19 using namespace mrpt::math;
20 using namespace std;
21 
22 template <class my_graph_t>
23 class GraphSlamLevMarqTester : public GraphSlamLevMarqTest<my_graph_t>,
24  public ::testing::Test
25 {
26  protected:
27  virtual void SetUp() {}
28  virtual void TearDown() {}
30  {
31  // This is the initial input graph (make a copy for later use):
32  my_graph_t graph;
34 
35  const my_graph_t graph_initial = graph;
36 
37  // ----------------------------
38  // Run graph slam:
39  // ----------------------------
41  // params["verbose"] = 1;
42  // params["profiler"] = 1;
43  params["max_iterations"] = 1000;
44 
46 
48  graph, levmarq_info, nullptr, params);
49 
50  // Do some basic checks on the results:
51  EXPECT_GE(levmarq_info.num_iters, 10U);
52  EXPECT_LE(levmarq_info.final_total_sq_error, 1e-2);
53 
54  } // end test_ring_path
55 
57  {
58  my_graph_t graph;
60 
61  // binary dump:
63  auto arch = mrpt::serialization::archiveFrom(mem);
64  arch << graph;
65 
66  {
67  my_graph_t read_graph;
68  mem.Seek(0);
69  arch >> read_graph;
70 
71  EXPECT_EQ(read_graph.edges.size(), graph.edges.size());
72  EXPECT_EQ(read_graph.nodes.size(), graph.nodes.size());
73 
74  // Also check that the edge values are OK:
75  typename my_graph_t::const_iterator it1, it2;
76  for (it1 = read_graph.edges.begin(), it2 = graph.edges.begin();
77  it1 != read_graph.edges.end(); ++it1, ++it2)
78  {
79  EXPECT_EQ(it1->first, it2->first);
80  EXPECT_NEAR(
81  0, (it1->second.getPoseMean().getAsVectorVal() -
82  it2->second.getPoseMean().getAsVectorVal())
83  .array()
84  .abs()
85  .sum(),
86  1e-9);
87  }
88  }
89  }
90 };
91 
94 
95 TEST_F(GraphSlamLevMarqTester2D, OptimizeSampleRingPath)
96 {
97  for (int seed = 1; seed < 5; seed++)
98  {
100  test_ring_path();
101  }
102 }
103 TEST_F(GraphSlamLevMarqTester2D, BinarySerialization)
104 {
106  test_graph_bin_serialization();
107 }
108 
109 TEST_F(GraphSlamLevMarqTester3D, OptimizeSampleRingPath)
110 {
111  for (int seed = 1; seed < 5; seed++)
112  {
114  test_ring_path();
115  }
116 }
117 TEST_F(GraphSlamLevMarqTester3D, BinarySerialization)
118 {
120  test_graph_bin_serialization();
121 }
mrpt::io::CMemoryStream::Seek
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.
Definition: CMemoryStream.cpp:120
const_iterator
const Scalar * const_iterator
Definition: eigen_plugins.h:27
GraphSlamLevMarqTester::TearDown
virtual void TearDown()
Definition: graph_slam_levmarq_unittest.cpp:28
GraphSlamLevMarqTester::test_ring_path
void test_ring_path()
Definition: graph_slam_levmarq_unittest.cpp:29
mrpt::graphslam::TResultInfoSpaLevMarq
Output information for mrpt::graphslam::optimize_graph_spa_levmarq()
Definition: graphslam/include/mrpt/graphslam/types.h:67
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:30
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:25
mrpt::graphslam::TResultInfoSpaLevMarq::num_iters
size_t num_iters
The number of LM iterations executed.
Definition: graphslam/include/mrpt/graphslam/types.h:70
GraphSlamLevMarqTest::create_ring_path
static void create_ring_path(my_graph_t &graph, size_t N_VERTEX=50, double DIST_THRES=7, double NODES_XY_MAX=20)
Definition: graph_slam_levmarq_test_common.h:40
mrpt::random::CRandomGenerator::randomize
void randomize(const uint32_t seed)
Initialize the PRNG from the given random seed.
Definition: RandomGenerator.cpp:32
mrpt::graphslam::TResultInfoSpaLevMarq::final_total_sq_error
double final_total_sq_error
The sum of all the squared errors for every constraint involved in the problem.
Definition: graphslam/include/mrpt/graphslam/types.h:73
mrpt::graphslam::optimize_graph_spa_levmarq
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(), typename graphslam_traits< GRAPH_T >::TFunctorFeedback functor_feedback=typename graphslam_traits< GRAPH_T >::TFunctorFeedback())
Optimize a graph of pose constraints using the Sparse Pose Adjustment (SPA) sparse representation and...
Definition: levmarq.h:80
mrpt::system::TParameters< double >
graph_slam_levmarq_test_common.h
TEST_F
TEST_F(GraphSlamLevMarqTester2D, OptimizeSampleRingPath)
Definition: graph_slam_levmarq_unittest.cpp:95
mrpt::graphs
Abstract graph and tree data structures, plus generic graph algorithms.
Definition: CAStarAlgorithm.h:18
mrpt::io::CMemoryStream
This CStream derived class allow using a memory buffer as a CStream.
Definition: io/CMemoryStream.h:26
mrpt::serialization::archiveFrom
CArchiveStreamBase< STREAM > archiveFrom(STREAM &s)
Helper function to create a templatized wrapper CArchive object for a: MRPT's CStream,...
Definition: CArchive.h:561
GraphSlamLevMarqTester
Definition: graph_slam_levmarq_unittest.cpp:23
mrpt::random::getRandomGenerator
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:19
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:13
mrpt::random
A namespace of pseudo-random numbers generators of diferent distributions.
Definition: random_shuffle.h:17
CArchive.h
GraphSlamLevMarqTester::SetUp
virtual void SetUp()
Definition: graph_slam_levmarq_unittest.cpp:27
GraphSlamLevMarqTest
Definition: graph_slam_levmarq_test_common.h:24
GraphSlamLevMarqTester::test_graph_bin_serialization
void test_graph_bin_serialization()
Definition: graph_slam_levmarq_unittest.cpp:56
params
GLenum const GLfloat * params
Definition: glext.h:3534



Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST