Main MRPT website > C++ reference for MRPT 1.9.9
graph_slam_levmarq_test_common.h
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 
10 #include <mrpt/graphslam/types.h>
11 #include <mrpt/graphslam/levmarq.h>
12 #include <mrpt/graphs.h>
13 #include <mrpt/random.h>
14 #include <mrpt/io/CMemoryStream.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  typename my_graph_t::edge_t RelativePose =
35  real_poses.find(to)->second - real_poses.find(from)->second;
36  graph.insertEdge(from, to, RelativePose);
37  }
38 
39  // The graph: nodes + edges:
40  static void create_ring_path(
41  my_graph_t& graph, size_t N_VERTEX = 50, double DIST_THRES = 7,
42  double NODES_XY_MAX = 20)
43  {
44  // The global poses of each node (without covariance):
45  typename my_graph_t::global_poses_t real_node_poses;
46 
47  // ----------------------------
48  // Create a random graph:
49  // ----------------------------
50  // Level of noise in nodes initial positions:
51  const double STD_NOISE_NODE_XYZ = 0.5;
52  const double STD_NOISE_NODE_ANG = DEG2RAD(5);
53 
54  // Level of noise in edges:
55  const double STD_NOISE_EDGE_XYZ = 0; // 0.01;
56  const double STD_NOISE_EDGE_ANG = 0; // DEG2RAD(0.1);
57 
58  for (TNodeID j = 0; j < N_VERTEX; j++)
59  {
60  static double ang = 2 * M_PI / N_VERTEX;
61  const double R = NODES_XY_MAX + 2 * (j % 2 ? 1 : -1);
62  CPose2D p(R * cos(ang * j), R * sin(ang * j), ang);
63 
64  // Save real pose:
65  real_node_poses[j] = p;
66 
67  // Copy the nodes to the graph, and add some noise:
68  graph.nodes[j] = p;
69  }
70 
71  // Add some edges
72  for (TNodeID i = 0; i < N_VERTEX; i++)
73  {
74  for (TNodeID j = i + 1; j < N_VERTEX; j++)
75  {
76  if (real_node_poses[i].distanceTo(real_node_poses[j]) <
77  DIST_THRES)
78  addEdge(i, j, real_node_poses, graph);
79  }
80  }
81 
82  // Cross-links:
83  addEdge(0, N_VERTEX / 2, real_node_poses, graph);
84 
85  // The root node (the origin of coordinates):
86  graph.root = TNodeID(0);
87 
88  // This is the ground truth graph (make a copy for later use):
89  const my_graph_t graph_GT = graph;
90 
91  // Add noise to edges & nodes:
92  for (typename my_graph_t::edges_map_t::iterator itEdge =
93  graph.edges.begin();
94  itEdge != graph.edges.end(); ++itEdge)
95  itEdge->second += typename my_graph_t::edge_t(
96  CPose3D(
97  getRandomGenerator().drawGaussian1D(0, STD_NOISE_EDGE_XYZ),
98  getRandomGenerator().drawGaussian1D(0, STD_NOISE_EDGE_XYZ),
99  getRandomGenerator().drawGaussian1D(0, STD_NOISE_EDGE_XYZ),
100  getRandomGenerator().drawGaussian1D(0, STD_NOISE_EDGE_ANG),
101  getRandomGenerator().drawGaussian1D(0, STD_NOISE_EDGE_ANG),
102  getRandomGenerator().drawGaussian1D(
103  0, STD_NOISE_EDGE_ANG)));
104 
105  for (typename my_graph_t::global_poses_t::iterator itNode =
106  graph.nodes.begin();
107  itNode != graph.nodes.end(); ++itNode)
108  if (itNode->first != graph.root)
109  itNode->second += typename my_graph_t::edge_t::type_value(
110  CPose3D(
111  getRandomGenerator().drawGaussian1D(
112  0, STD_NOISE_NODE_XYZ),
113  getRandomGenerator().drawGaussian1D(
114  0, STD_NOISE_NODE_XYZ),
115  getRandomGenerator().drawGaussian1D(
116  0, STD_NOISE_NODE_XYZ),
117  getRandomGenerator().drawGaussian1D(
118  0, STD_NOISE_NODE_ANG),
119  getRandomGenerator().drawGaussian1D(
120  0, STD_NOISE_NODE_ANG),
121  getRandomGenerator().drawGaussian1D(
122  0, STD_NOISE_NODE_ANG)));
123  }
124 };
A namespace of pseudo-random numbers generators of diferent distributions.
Scalar * iterator
Definition: eigen_plugins.h:26
void addEdge(TNodeID from, TNodeID to, const mrpt::aligned_std_map< TNodeID, CPose2D > &real_poses, CNetworkOfPoses2D &graph_links)
double DEG2RAD(const double x)
Degrees to radians.
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:40
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
uint64_t TNodeID
A generic numeric type for unique IDs of nodes or entities.
Definition: TNodeID.h:17
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.
GLfloat GLfloat p
Definition: glext.h:6305
const double STD_NOISE_EDGE_XYZ



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019