Main MRPT website > C++ reference for MRPT 1.5.6
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-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 
11 #include <mrpt/graphslam/types.h>
12 #include <mrpt/graphslam/levmarq.h>
13 #include <mrpt/graphs.h>
14 #include <mrpt/random.h>
16 
17 using namespace mrpt;
18 using namespace mrpt::random;
19 using namespace mrpt::utils;
20 using namespace mrpt::poses;
21 using namespace mrpt::graphs;
22 using namespace mrpt::math;
23 using namespace std;
24 
25 template <class my_graph_t>
27 {
28 public:
29  // adds a new edge to the graph. The edge is annotated with the relative position of the two nodes
30  static void addEdge(TNodeID from, TNodeID to, const typename my_graph_t::global_poses_t &real_poses,my_graph_t &graph)
31  {
32  typename my_graph_t::edge_t RelativePose = real_poses.find(to)->second - real_poses.find(from)->second;
33  graph.insertEdge(from,to, RelativePose );
34  }
35 
36  // The graph: nodes + edges:
37  static void create_ring_path(
38  my_graph_t & graph,
39  size_t N_VERTEX = 50,
40  double DIST_THRES = 7,
41  double NODES_XY_MAX = 20)
42  {
43  // The global poses of each node (without covariance):
44  typename my_graph_t::global_poses_t real_node_poses;
45 
46  // ----------------------------
47  // Create a random graph:
48  // ----------------------------
49  // Level of noise in nodes initial positions:
50  const double STD_NOISE_NODE_XYZ = 0.5;
51  const double STD_NOISE_NODE_ANG = DEG2RAD(5);
52 
53  // Level of noise in edges:
54  const double STD_NOISE_EDGE_XYZ = 0; //0.01;
55  const double STD_NOISE_EDGE_ANG = 0; //DEG2RAD(0.1);
56 
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(
63  R*cos(ang*j),
64  R*sin(ang*j),
65  ang);
66 
67  // Save real pose:
68  real_node_poses[j] = p;
69 
70  // Copy the nodes to the graph, and add some noise:
71  graph.nodes[j] = p;
72  }
73 
74 
75  // Add some edges
76  for (TNodeID i=0;i<N_VERTEX;i++)
77  {
78  for (TNodeID j=i+1;j<N_VERTEX;j++)
79  {
80  if ( real_node_poses[i].distanceTo(real_node_poses[j]) < DIST_THRES )
81  addEdge(i,j,real_node_poses,graph);
82  }
83  }
84 
85  // Cross-links:
86  addEdge(0,N_VERTEX/2,real_node_poses,graph);
87 
88  // The root node (the origin of coordinates):
89  graph.root = TNodeID(0);
90 
91  // This is the ground truth graph (make a copy for later use):
92  const my_graph_t graph_GT = graph;
93 
94  // Add noise to edges & nodes:
95  for (typename my_graph_t::edges_map_t::iterator itEdge=graph.edges.begin();itEdge!=graph.edges.end();++itEdge)
96  itEdge->second += typename my_graph_t::edge_t( CPose3D(
97  randomGenerator.drawGaussian1D(0,STD_NOISE_EDGE_XYZ),
98  randomGenerator.drawGaussian1D(0,STD_NOISE_EDGE_XYZ),
99  randomGenerator.drawGaussian1D(0,STD_NOISE_EDGE_XYZ),
100  randomGenerator.drawGaussian1D(0,STD_NOISE_EDGE_ANG),
101  randomGenerator.drawGaussian1D(0,STD_NOISE_EDGE_ANG),
102  randomGenerator.drawGaussian1D(0,STD_NOISE_EDGE_ANG) ) );
103 
104  for (typename my_graph_t::global_poses_t::iterator itNode=graph.nodes.begin();itNode!=graph.nodes.end();++itNode)
105  if (itNode->first!=graph.root)
106  itNode->second += typename my_graph_t::edge_t::type_value( CPose3D(
107  randomGenerator.drawGaussian1D(0,STD_NOISE_NODE_XYZ),
108  randomGenerator.drawGaussian1D(0,STD_NOISE_NODE_XYZ),
109  randomGenerator.drawGaussian1D(0,STD_NOISE_NODE_XYZ),
110  randomGenerator.drawGaussian1D(0,STD_NOISE_NODE_ANG),
111  randomGenerator.drawGaussian1D(0,STD_NOISE_NODE_ANG),
112  randomGenerator.drawGaussian1D(0,STD_NOISE_NODE_ANG) ) );
113  }
114 };
115 
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
Scalar * iterator
Definition: eigen_plugins.h:23
#define M_PI
Definition: bits.h:78
double drawGaussian1D(const double mean, const double std)
Generate a normally distributed pseudo-random number.
uint64_t TNodeID
The type for node IDs in graphs of different types.
Definition: types_simple.h:45
GLfloat GLfloat p
Definition: glew.h:10113
#define DEG2RAD
static void addEdge(TNodeID from, TNodeID to, const typename my_graph_t::global_poses_t &real_poses, my_graph_t &graph)
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
static void create_ring_path(my_graph_t &graph, size_t N_VERTEX=50, double DIST_THRES=7, double NODES_XY_MAX=20)



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018