MRPT  2.0.1
graph_slam_levmarq_test_common.h
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 <mrpt/graphs.h>
11 #include <mrpt/graphslam/levmarq.h>
12 #include <mrpt/graphslam/types.h>
13 #include <mrpt/io/CMemoryStream.h>
14 #include <mrpt/random.h>
15 
16 using namespace mrpt;
17 using namespace mrpt::random;
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>
25 {
26  public:
27  // adds a new edge to the graph. The edge is annotated with the relative
28  // position of the two nodes
29  static void addEdge(
30  TNodeID from, TNodeID to,
31  const typename my_graph_t::global_poses_t& real_poses,
32  my_graph_t& graph)
33  {
34  auto RelativePose =
35  real_poses.find(to)->second - real_poses.find(from)->second;
36 
37  // Add information matrix if present:
38  if constexpr (my_graph_t::edge_t::is_PDF())
39  {
40  const auto N = my_graph_t::edge_t::state_length;
42  auto mat = getRandomGenerator()
45  N, 2.0 /*std*/, 1.0 /*diagonal offset*/);
46  graph.insertEdge(
47  from, to, typename my_graph_t::edge_t(RelativePose, mat));
48  }
49  else
50  {
51  graph.insertEdge(from, to, RelativePose);
52  }
53  }
54 
55  // The graph: nodes + edges:
56  static void create_ring_path(
57  my_graph_t& graph, size_t N_VERTEX = 50, double DIST_THRES = 7,
58  double NODES_XY_MAX = 20)
59  {
60  // The global poses of each node (without covariance):
61  typename my_graph_t::global_poses_t real_node_poses;
62 
63  // ----------------------------
64  // Create a random graph:
65  // ----------------------------
66  // Level of noise in nodes initial positions:
67  const double STD_NOISE_NODE_XYZ = 0.5;
68  const double STD_NOISE_NODE_ANG = 5.0_deg;
69 
70  // Level of noise in edges:
71  const double STD_NOISE_EDGE_XYZ = 0; // 0.01;
72  const double STD_NOISE_EDGE_ANG = 0; // 0.1_deg;
73 
74  for (TNodeID j = 0; j < N_VERTEX; j++)
75  {
76  static double ang = 2 * M_PI / N_VERTEX;
77  const double R = NODES_XY_MAX + 2 * (j % 2 ? 1 : -1);
78  CPose2D p(R * cos(ang * j), R * sin(ang * j), ang);
79 
80  // Save real pose:
81  real_node_poses[j] = p;
82 
83  // Copy the nodes to the graph, and add some noise:
84  graph.nodes[j] = p;
85  }
86 
87  // Add some edges
88  for (TNodeID i = 0; i < N_VERTEX; i++)
89  {
90  for (TNodeID j = i + 1; j < N_VERTEX; j++)
91  {
92  if (real_node_poses[i].distanceTo(real_node_poses[j]) <
93  DIST_THRES)
94  addEdge(i, j, real_node_poses, graph);
95  }
96  }
97 
98  // Cross-links:
99  addEdge(0, N_VERTEX / 2, real_node_poses, graph);
100 
101  // The root node (the origin of coordinates):
102  graph.root = TNodeID(0);
103 
104  // This is the ground truth graph (make a copy for later use):
105  const my_graph_t graph_GT = graph;
106 
107  // Add noise to edges & nodes:
108  for (auto& edge : graph.edges)
109  {
110  // edge_t::type_value: for edge_t a plain pose, type_value is the
111  // edge_t itself. For poses+uncertainty, it is the underlying pose
112  // type.
113  if constexpr (my_graph_t::edge_t::type_value::size() == 6)
114  {
115  const auto noise = CPose3D(
116  getRandomGenerator().drawGaussian1D(0, STD_NOISE_EDGE_XYZ),
117  getRandomGenerator().drawGaussian1D(0, STD_NOISE_EDGE_XYZ),
118  getRandomGenerator().drawGaussian1D(0, STD_NOISE_EDGE_XYZ),
119  getRandomGenerator().drawGaussian1D(0, STD_NOISE_EDGE_ANG),
120  getRandomGenerator().drawGaussian1D(0, STD_NOISE_EDGE_ANG),
121  getRandomGenerator().drawGaussian1D(0, STD_NOISE_EDGE_ANG));
122 
123  if constexpr (my_graph_t::edge_t::is_PDF())
124  {
125  const auto N = my_graph_t::edge_t::state_length;
127  InfMat mat;
128  mat.setIdentity();
129  edge.second += typename my_graph_t::edge_t(noise, mat);
130  }
131  else
132  {
133  edge.second += typename my_graph_t::edge_t(noise);
134  }
135  }
136  else
137  {
138  const auto noise = CPose2D(
139  getRandomGenerator().drawGaussian1D(0, STD_NOISE_EDGE_XYZ),
140  getRandomGenerator().drawGaussian1D(0, STD_NOISE_EDGE_XYZ),
141  getRandomGenerator().drawGaussian1D(0, STD_NOISE_EDGE_ANG));
142  if constexpr (my_graph_t::edge_t::is_PDF())
143  {
144  const auto N = my_graph_t::edge_t::state_length;
146  InfMat mat;
147  mat.setIdentity();
148  edge.second += typename my_graph_t::edge_t(noise, mat);
149  }
150  else
151  {
152  edge.second += typename my_graph_t::edge_t(noise);
153  }
154  }
155  }
156 
157  for (auto itNode = graph.nodes.begin(); itNode != graph.nodes.end();
158  ++itNode)
159  if (itNode->first != graph.root)
160  itNode
161  ->second += typename my_graph_t::edge_t::type_value(CPose3D(
162  getRandomGenerator().drawGaussian1D(0, STD_NOISE_NODE_XYZ),
163  getRandomGenerator().drawGaussian1D(0, STD_NOISE_NODE_XYZ),
164  getRandomGenerator().drawGaussian1D(0, STD_NOISE_NODE_XYZ),
165  getRandomGenerator().drawGaussian1D(0, STD_NOISE_NODE_ANG),
166  getRandomGenerator().drawGaussian1D(0, STD_NOISE_NODE_ANG),
167  getRandomGenerator().drawGaussian1D(
168  0, STD_NOISE_NODE_ANG)));
169  }
170 };
A namespace of pseudo-random numbers generators of diferent distributions.
MATRIX drawDefinitePositiveMatrix(const size_t dim, const double std_scale=1.0, const double diagonal_epsilon=1e-8)
Generates a random definite-positive matrix of the given size, using the formula C = v*v^t + epsilon*...
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
void addEdge(TNodeID from, TNodeID to, const std::map< TNodeID, CPose2D > &real_poses, CNetworkOfPoses2D &graph_links)
size_t size(const MATRIXLIKE &m, const int dim)
Abstract graph and tree data structures, plus generic graph algorithms.
STL namespace.
const double STD_NOISE_NODE_XYZ
This base provides a set of functions for maths stuff.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
static void addEdge(TNodeID from, TNodeID to, const typename my_graph_t::global_poses_t &real_poses, my_graph_t &graph)
const double STD_NOISE_EDGE_ANG
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
uint64_t TNodeID
A generic numeric type for unique IDs of nodes or entities.
Definition: TNodeID.h:16
static void create_ring_path(my_graph_t &graph, size_t N_VERTEX=50, double DIST_THRES=7, double NODES_XY_MAX=20)
const double STD_NOISE_NODE_ANG
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
const double STD_NOISE_EDGE_XYZ



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