MRPT  2.0.4
TUserOptionsChecker_impl.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 // implementation of the TUserOptionsChecker class template methods
11 namespace mrpt::graphslam::apps
12 {
13 template <class GRAPH_t>
15  : sep_header(40, '='), sep_subheader(20, '-')
16 {
17 }
18 
19 template <class GRAPH_t>
21 {
22  using namespace std;
23 
24  // release the instances holding the descriptions of the available
25  // deciders/optimizers
26  for (auto it = regs_descriptions.begin(); it != regs_descriptions.end();
27  ++it)
28  {
29  delete *it;
30  }
31  for (auto it = optimizers_descriptions.begin();
32  it != optimizers_descriptions.end(); ++it)
33  {
34  delete *it;
35  }
36 }
37 
38 template <class GRAPH_t>
40 {
42 
43  using namespace mrpt::graphslam::deciders;
44  using namespace mrpt::graphslam::optimizers;
45  using namespace mrpt::graphs;
46  using namespace mrpt::poses;
47 
48  // node registration deciders
49  node_regs_map["CEmptyNRD"] =
50  &createNodeRegistrationDecider<CEmptyNRD<GRAPH_t>>;
51  node_regs_map["CFixedIntervalsNRD"] =
52  &createNodeRegistrationDecider<CFixedIntervalsNRD<GRAPH_t>>;
53  // edge registration deciders
54  edge_regs_map["CEmptyERD"] =
55  &createEdgeRegistrationDecider<CEmptyERD<GRAPH_t>>;
56  // optimizers
57  optimizers_map["CLevMarqGSO"] =
58  &createGraphSlamOptimizer<CLevMarqGSO<GRAPH_t>>;
59  optimizers_map["CEmptyGSO"] =
60  &createGraphSlamOptimizer<CLevMarqGSO<GRAPH_t>>;
61 
62  // create the decider optimizer, specific to the GRAPH_T template type
63  this->_createDeciderOptimizerMappings();
64 
65  MRPT_END
66 }
67 
68 template <class GRAPH_t>
70 {
71 }
72 
73 // deciders/optpimizers specific to the 2D SLAM cases
74 template <>
75 inline void TUserOptionsChecker<
76  mrpt::graphs::CNetworkOfPoses2DInf>::_createDeciderOptimizerMappings()
77 {
78  using namespace mrpt::graphs;
79 
80  node_regs_map["CICPCriteriaNRD"] =
81  &createNodeRegistrationDecider<CICPCriteriaNRD<CNetworkOfPoses2DInf>>;
82  edge_regs_map["CICPCriteriaERD"] =
83  &createEdgeRegistrationDecider<CICPCriteriaERD<CNetworkOfPoses2DInf>>;
84  edge_regs_map["CLoopCloserERD"] =
85  &createEdgeRegistrationDecider<CLoopCloserERD<CNetworkOfPoses2DInf>>;
86 }
87 template <>
88 inline void TUserOptionsChecker<
89  mrpt::graphs::CNetworkOfPoses2DInf_NA>::_createDeciderOptimizerMappings()
90 {
91  using namespace mrpt::graphs;
92 
93  node_regs_map["CICPCriteriaNRD"] = &createNodeRegistrationDecider<
95  edge_regs_map["CICPCriteriaERD"] = &createEdgeRegistrationDecider<
97  edge_regs_map["CLoopCloserERD"] =
98  &createEdgeRegistrationDecider<CLoopCloserERD<CNetworkOfPoses2DInf_NA>>;
99 }
100 
101 // deciders/optpimizers specific to the 3D SLAM cases
102 template <>
103 inline void TUserOptionsChecker<
104  mrpt::graphs::CNetworkOfPoses3DInf>::_createDeciderOptimizerMappings()
105 {
106 }
107 
108 template <class GRAPH_t>
110  std::string reg_type /*="all"*/) const
111 {
112  MRPT_START
113  using namespace std;
114  using namespace mrpt;
115 
117  (system::strCmpI(reg_type, "node") ||
118  system::strCmpI(reg_type, "edge") || system::strCmpI(reg_type, "all")),
119  format(
120  "Registrar string '%s' does not match a known registrar name.\n"
121  "Specify 'node' 'edge' or 'all'",
122  reg_type.c_str()));
123 
124  if (system::strCmpI(reg_type, "node") || system::strCmpI(reg_type, "edge"))
125  {
126  cout << endl
127  << "Available " << system::upperCase(reg_type)
128  << " Registration Deciders: " << endl;
129  cout << sep_header << endl;
130 
131  for (auto dec_it = regs_descriptions.begin();
132  dec_it != regs_descriptions.end(); ++dec_it)
133  {
134  TRegistrationDeciderProps* dec = *dec_it;
135  if (system::strCmpI(dec->type, reg_type))
136  {
137  cout << dec->name << endl;
138  cout << sep_subheader << endl;
139  cout << "\t- "
140  << "Description: " << dec->description << endl;
141  cout << "\t- "
142  << "Rawlog Format: " << dec->rawlog_format << endl;
143  cout << "\t- "
144  << "Observations that can be used: " << endl;
145  cout << "\t- "
146  << "Multi-robot SLAM capable decider: "
147  << (dec->is_mr_slam_class ? "TRUE" : "FALSE") << endl;
148  cout << "\t- "
149  << "SLAM Type: " << endl;
150  if (dec->is_slam_2d)
151  {
152  cout << "\t\t+ "
153  << "2D" << endl;
154  }
155  if (dec->is_slam_3d)
156  {
157  cout << "\t\t+ "
158  << "3D" << endl;
159  }
160  cout << endl;
161  for (auto obs_it = dec->observations_used.begin();
162  obs_it != dec->observations_used.end(); ++obs_it)
163  {
164  cout << "\t\t+ " << *obs_it << endl;
165  }
166  }
167  }
168  }
169  else
170  { // print both
171  dumpRegistrarsToConsole("node");
172  dumpRegistrarsToConsole("edge");
173  }
174 
175  MRPT_END
176 }
177 
178 template <class GRAPH_t>
180 {
181  MRPT_START
182 
183  using namespace std;
184 
185  cout << endl << "Available GraphSlam Optimizer classes: " << endl;
186  cout << sep_header << endl;
187 
188  for (auto opt_it = optimizers_descriptions.begin();
189  opt_it != optimizers_descriptions.end(); ++opt_it)
190  {
191  TOptimizerProps* opt = *opt_it;
192  cout << opt->name << endl;
193  cout << sep_subheader << endl;
194  cout << "\t- "
195  << "Description: " << opt->description << endl;
196 
197  cout << "\t- "
198  << "Multi-robot SLAM capable optimizer: "
199  << (opt->is_mr_slam_class ? "TRUE" : "FALSE");
200  cout << "\t- "
201  << "SLAM Type: " << endl;
202  if (opt->is_slam_2d)
203  {
204  cout << "\t\t+ "
205  << "2D" << endl;
206  }
207  if (opt->is_slam_3d)
208  {
209  cout << "\t\t+ "
210  << "3D" << endl;
211  }
212  }
213  MRPT_END
214 }
215 
216 template <class GRAPH_t>
218  std::string given_reg, std::string reg_type) const
219 {
220  MRPT_START
221 
222  using namespace std;
223  using namespace mrpt;
224  using namespace mrpt::poses;
225 
227  (system::strCmpI(reg_type, "node") ||
228  system::strCmpI(reg_type, "edge")),
229  format(
230  "Registrar string \"%s\" does not match a known registrar name.\n"
231  "Specify 'node' or 'edge' ",
232  reg_type.c_str()));
233  bool found = false;
234 
235  for (auto dec_it = regs_descriptions.begin();
236  dec_it != regs_descriptions.end(); ++dec_it)
237  {
238  TRegistrationDeciderProps* dec = *dec_it;
239 
240  // TODO - check this
241  // if decider is not suitable for the selected SLAM type, ignore.
242  pose_t p;
243  if ((!dec->is_slam_2d && IS_CLASS(p, CPose2D)) ||
244  (!dec->is_slam_3d && IS_CLASS(p, CPose3D)))
245  {
246  continue;
247  }
248 
249  if (system::strCmpI(dec->type, reg_type))
250  {
251  if (system::strCmpI(dec->name, given_reg))
252  {
253  found = true;
254  return found;
255  }
256  }
257  }
258 
259  return found;
260  MRPT_END
261 }
262 
263 template <class GRAPH_t>
265  std::string given_opt) const
266 {
267  MRPT_START
268  using namespace std;
269  using namespace mrpt;
270  using namespace mrpt::poses;
271 
272  bool found = false;
273 
274  for (auto opt_it = optimizers_descriptions.begin();
275  opt_it != optimizers_descriptions.end(); ++opt_it)
276  {
277  TOptimizerProps* opt = *opt_it;
278 
279  // if optimizer is not suitable for the selected SLAM type, ignore.
280  pose_t p;
281  if ((!opt->is_slam_2d && IS_CLASS(p, CPose2D)) &&
282  (!opt->is_slam_3d && IS_CLASS(p, CPose3D)))
283  {
284  continue;
285  }
286 
287  if (system::strCmpI(opt->name, given_opt))
288  {
289  found = true;
290  return found;
291  }
292  }
293 
294  return found;
295  MRPT_END
296 }
297 
298 template <class GRAPH_t>
300 {
301  MRPT_START
302 
303  // reset the vectors - in case they contain any elements
304  regs_descriptions.clear();
305  optimizers_descriptions.clear();
306 
307  // registering the available deciders
308  { // CFixedIntervalsNRD
309  auto* dec = new TRegistrationDeciderProps;
310  dec->name = "CFixedIntervalsNRD";
311  dec->description =
312  "Register a new node if the distance from the previous node "
313  "surpasses a predefined distance threshold. Uses odometry "
314  "information for estimating the robot movement";
315  dec->type = "Node";
316  dec->rawlog_format = "Both";
317  dec->observations_used.emplace_back(
318  "CActionRobotMovement2D - Format #1");
319  dec->observations_used.emplace_back("CObservationOdometry - Format #2");
320  dec->is_slam_2d = true;
321  dec->is_slam_3d = true;
322 
323  regs_descriptions.push_back(dec);
324  }
325  { // CICPCriteriaNRD
326  auto* dec = new TRegistrationDeciderProps;
327  dec->name = "CICPCriteriaNRD";
328  dec->description =
329  "Register a new node if the distance from the previous node "
330  "surpasses a predefined distance threshold. Uses 2D/3D RangeScans "
331  "alignment for estimating the robot movement";
332  dec->type = "Node";
333  dec->rawlog_format = "#2 - Observation-only";
334  dec->observations_used.emplace_back(
335  "CObservation2DRangeScan - Format #2");
336  dec->observations_used.emplace_back(
337  "CObservation3DRangeScan - Format #2");
338  dec->is_slam_2d = true;
339 
340  regs_descriptions.push_back(dec);
341  }
342  { // CEmptyNRD
343  auto* dec = new TRegistrationDeciderProps;
344  dec->name = "CEmptyNRD";
345  dec->description =
346  "Empty Decider - does nothing when its class methods are called";
347  dec->type = "Node";
348  dec->rawlog_format = "Both";
349  dec->is_mr_slam_class = true;
350  dec->is_slam_2d = true;
351  dec->is_slam_3d = true;
352 
353  regs_descriptions.push_back(dec);
354  }
355  { // CICPCriteriaERD
356  auto* dec = new TRegistrationDeciderProps;
357  dec->name = "CICPCriteriaERD";
358  dec->description =
359  "Register a new edge by aligning the provided 2D/3D RangeScans of "
360  "2 nodes. Uses the goodness of the ICP Alignment as the criterium "
361  "for adding a new edge";
362  dec->type = "Edge";
363  dec->rawlog_format = "Both";
364  dec->observations_used.emplace_back(
365  "CObservation2DRangeScan - Format #1, #2");
366  dec->observations_used.emplace_back(
367  "CObservation3DRangeScan - Format #2");
368  dec->is_slam_2d = true;
369 
370  regs_descriptions.push_back(dec);
371  }
372  { // CEmptyERD
373  auto* dec = new TRegistrationDeciderProps;
374  dec->name = "CEmptyERD";
375  dec->description =
376  "Empty Decider - does nothing when its class methods are called";
377  dec->type = "Edge";
378  dec->rawlog_format = "Both";
379  dec->is_mr_slam_class = true;
380  dec->is_slam_2d = true;
381  dec->is_slam_3d = true;
382 
383  regs_descriptions.push_back(dec);
384  }
385  { // CLoopCloserERD
386  auto* dec = new TRegistrationDeciderProps;
387  dec->name = "CLoopCloserERD";
388  dec->description =
389  "Partition the map and register *sets* of edges based on the "
390  "Pairwise consistency matrix of each set.";
391  dec->type = "Edge";
392  dec->rawlog_format = "Both";
393  dec->observations_used.emplace_back(
394  "CObservation2DRangeScan - Format #1, #2");
395  dec->is_slam_2d = true;
396 
397  regs_descriptions.push_back(dec);
398  }
399 
400  // registering the available optimizers
401  { // CEmptyGSO
402  auto* opt = new TOptimizerProps;
403  opt->name = "CEmptyGSO";
404  opt->description =
405  "Empty Optimizer - does nothing when its class methods are called";
406  opt->is_mr_slam_class = true;
407  opt->is_slam_2d = true;
408  opt->is_slam_3d = true;
409 
410  optimizers_descriptions.push_back(opt);
411  }
412 
413  { // CLevMarqGSO
414  auto* opt = new TOptimizerProps;
415  opt->name = "CLevMarqGSO";
416  opt->description = "Levenberg-Marqurdt non-linear graphSLAM solver";
417  opt->is_mr_slam_class = true;
418  opt->is_slam_2d = true;
419  opt->is_slam_3d = true;
420 
421  optimizers_descriptions.push_back(opt);
422  }
423 
424  MRPT_END
425 }
426 } // namespace mrpt::graphslam::apps
Class containing the declarations of supplementary methods that can be used in application-related co...
#define MRPT_START
Definition: exceptions.h:241
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
Abstract graph and tree data structures, plus generic graph algorithms.
Properties struct for the Registration Decider classes.
ICP-based Fixed Intervals Node Registration.
std::vector< std::string > observations_used
Measurements that the current decider class can utilize.
bool is_mr_slam_class
Class indicating if the current decider/optimizer class can be used in a multi-robot SLAM operation...
STL namespace.
A directed graph of pose constraints, with edges being the relative poses between pairs of nodes iden...
typename GRAPH_T ::constraint_t::type_value pose_t
#define IS_CLASS(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is of the given class...
Definition: CObject.h:146
std::string description
General description of the decicder or optimizer class.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
#define ASSERTDEBMSG_(f, __ERROR_MSG)
Definition: exceptions.h:191
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
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
std::string name
Name of the decider or optimizer class.
std::string upperCase(const std::string &str)
Returns a upper-case version of a string.
#define MRPT_END
Definition: exceptions.h:245
Properties struct for the Optimizer classes.
std::string rawlog_format
Rawlog formats that the decider can be used in.
bool strCmpI(const std::string &s1, const std::string &s2)
Return true if the two strings are equal (case insensitive)



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020