Main MRPT website > C++ reference for MRPT 1.5.7
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 
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 mrpt::utils;
21 using namespace std;
22 
23 // SFINAE seems to be badly supported in MSVC 2015...sigh. Just do non-templatized overloading:
24 template <class edge_t> struct EdgeAdder;
25 
26 template <> struct EdgeAdder<mrpt::poses::CPose2D>
27 {
28  template <class my_graph_t>
29  static void add(
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  graph.insertEdge(from, to, RelativePose);
37  }
38 
39  static void addNoise(mrpt::poses::CPose2D& p, const mrpt::poses::CPose3D & noise)
40  {
41  p += mrpt::poses::CPose2D(noise);
42  }
43 };
44 template <> struct EdgeAdder<mrpt::poses::CPose3D>
45 {
46  template <class my_graph_t>
47  static void add(
48  TNodeID from, TNodeID to,
49  const typename my_graph_t::global_poses_t& real_poses,
50  my_graph_t& graph)
51  {
52  auto RelativePose =
53  real_poses.find(to)->second - real_poses.find(from)->second;
54  graph.insertEdge(from, to, RelativePose);
55  }
56  static void addNoise(mrpt::poses::CPose3D& p, const mrpt::poses::CPose3D & noise)
57  {
58  p += noise;
59  }
60 };
62 {
63  template <class my_graph_t>
64  static void add(
65  TNodeID from, TNodeID to,
66  const typename my_graph_t::global_poses_t& real_poses,
67  my_graph_t& graph)
68  {
69  auto RelativePose =
70  real_poses.find(to)->second - real_poses.find(from)->second;
71  // Add information matrix :
72  const auto N = my_graph_t::edge_t::state_length;
73  auto mat = randomGenerator
75  N, 2.0 /*std*/, 1.0 /*diagonal offset*/);
76  graph.insertEdge(
77  from, to, typename my_graph_t::edge_t(RelativePose, mat));
78  }
80  {
81  p.mean += mrpt::poses::CPose2D(noise);
82  }
83 };
85 {
86  template <class my_graph_t>
87  static void add(
88  TNodeID from, TNodeID to,
89  const typename my_graph_t::global_poses_t& real_poses,
90  my_graph_t& graph)
91  {
92  auto RelativePose =
93  real_poses.find(to)->second - real_poses.find(from)->second;
94  // Add information matrix :
95  const auto N = my_graph_t::edge_t::state_length;
96  auto mat = randomGenerator
98  N, 2.0 /*std*/, 1.0 /*diagonal offset*/);
99  graph.insertEdge(
100  from, to, typename my_graph_t::edge_t(RelativePose, mat));
101  }
103  {
104  p.mean += noise;
105  }
106 };
107 
108 
109 template <class my_graph_t>
111 {
112 public:
113 
114  // The graph: nodes + edges:
115  static void create_ring_path(
116  my_graph_t& graph, size_t N_VERTEX = 50, double DIST_THRES = 7,
117  double NODES_XY_MAX = 20)
118  {
119  // The global poses of each node (without covariance):
120  typename my_graph_t::global_poses_t real_node_poses;
121 
122  // ----------------------------
123  // Create a random graph:
124  // ----------------------------
125  // Level of noise in nodes initial positions:
126  const double STD_NOISE_NODE_XYZ = 0.5;
127  const double STD_NOISE_NODE_ANG = DEG2RAD(5);
128 
129  // Level of noise in edges:
130  const double STD_NOISE_EDGE_XYZ = 1e-3;
131  const double STD_NOISE_EDGE_ANG = DEG2RAD(0.01);
132 
133  for (TNodeID j = 0; j < N_VERTEX; j++)
134  {
135  static double ang = 2 * M_PI / N_VERTEX;
136  const double R = NODES_XY_MAX + 2 * (j % 2 ? 1 : -1);
137  CPose2D p(R * cos(ang * j), R * sin(ang * j), ang);
138 
139  // Save real pose:
140  real_node_poses[j] = p;
141 
142  // Copy the nodes to the graph, and add some noise:
143  graph.nodes[j] = p;
144 
145  const auto noise = CPose3D(
146  randomGenerator.drawGaussian1D(0, STD_NOISE_NODE_XYZ),
147  randomGenerator.drawGaussian1D(0, STD_NOISE_NODE_XYZ),
148  randomGenerator.drawGaussian1D(0, STD_NOISE_NODE_XYZ),
149  randomGenerator.drawGaussian1D(0, STD_NOISE_NODE_ANG),
150  randomGenerator.drawGaussian1D(0, STD_NOISE_NODE_ANG),
151  randomGenerator.drawGaussian1D(0, STD_NOISE_NODE_ANG));
153 
154  }
155 
156  // Add some edges
157  for (TNodeID i = 0; i < N_VERTEX; i++)
158  {
159  for (TNodeID j = i + 1; j < N_VERTEX; j++)
160  {
161  if (real_node_poses[i].distanceTo(real_node_poses[j]) <
162  DIST_THRES)
163  EdgeAdder<typename my_graph_t::edge_underlying_t>::template add<my_graph_t>(i, j, real_node_poses, graph);
164  }
165  }
166 
167  // Cross-links:
168  EdgeAdder<typename my_graph_t::edge_underlying_t>::template add<my_graph_t>(0, N_VERTEX / 2, real_node_poses, graph);
169 
170  // The root node (the origin of coordinates):
171  graph.root = TNodeID(0);
172 
173  // Add noise to edges & nodes:
174  for (auto& edge : graph.edges)
175  {
176  const auto noise = CPose3D(
177  randomGenerator.drawGaussian1D(0, STD_NOISE_EDGE_XYZ),
178  randomGenerator.drawGaussian1D(0, STD_NOISE_EDGE_XYZ),
179  randomGenerator.drawGaussian1D(0, STD_NOISE_EDGE_XYZ),
180  randomGenerator.drawGaussian1D(0, STD_NOISE_EDGE_ANG),
181  randomGenerator.drawGaussian1D(0, STD_NOISE_EDGE_ANG),
182  randomGenerator.drawGaussian1D(0, STD_NOISE_EDGE_ANG));
184  }
185 
186  }
187 };
A namespace of pseudo-random numbers genrators of diferent distributions.
static void addNoise(mrpt::poses::CPose3D &p, const mrpt::poses::CPose3D &noise)
static void add(TNodeID from, TNodeID to, const typename my_graph_t::global_poses_t &real_poses, my_graph_t &graph)
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
static void addNoise(mrpt::poses::CPose2D &p, const mrpt::poses::CPose3D &noise)
static void addNoise(mrpt::poses::CPosePDFGaussianInf &p, const mrpt::poses::CPose3D &noise)
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.
STL namespace.
#define M_PI
Definition: bits.h:78
double drawGaussian1D(const double mean, const double std)
Generate a normally distributed pseudo-random number.
static void add(TNodeID from, TNodeID to, const typename my_graph_t::global_poses_t &real_poses, my_graph_t &graph)
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
uint64_t TNodeID
The type for node IDs in graphs of different types.
Definition: types_simple.h:45
#define DEG2RAD
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
mrpt::math::CMatrixDouble 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*...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A Probability Density function (PDF) of a 2D pose as a Gaussian with a mean and the inverse of the c...
static void add(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
Declares a class that represents a Probability Density function (PDF) of a 3D pose as a Gaussian des...
static void addNoise(mrpt::poses::CPose3DPDFGaussianInf &p, const mrpt::poses::CPose3D &noise)
static void create_ring_path(my_graph_t &graph, size_t N_VERTEX=50, double DIST_THRES=7, double NODES_XY_MAX=20)
GLfloat GLfloat p
Definition: glext.h:5587
static void add(TNodeID from, TNodeID to, const typename my_graph_t::global_poses_t &real_poses, my_graph_t &graph)



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019