Main MRPT website > C++ reference for MRPT 1.9.9
CICPCriteriaERD_impl.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 #ifndef CICPCRITERIAERD_IMPL_H
11 #define CICPCRITERIAERD_IMPL_H
12 #include <mrpt/opengl/CDisk.h>
13 
14 namespace mrpt
15 {
16 namespace graphslam
17 {
18 namespace deciders
19 {
20 // Ctors, Dtors
21 // //////////////////////////////////
22 
23 template <class GRAPH_T>
25  : params(*this), // pass reference to self when initializing the parameters
26  m_search_disk_color(142, 142, 56),
27  m_laser_scans_color(0, 20, 255),
28  m_is_using_3DScan(false)
29 {
30  MRPT_START;
31 
32  this->initializeLoggers("CICPCriteriaERD");
33 
34  // start ICP constraint registration only when
35  m_edge_types_to_nums["ICP2D"] = 0;
36  m_edge_types_to_nums["ICP3D"] = 0;
37  m_edge_types_to_nums["LC"] = 0;
38 
39  this->m_last_total_num_nodes = 2;
40 
41  MRPT_LOG_DEBUG("Initialized class object");
42 
43  MRPT_END;
44 }
45 template <class GRAPH_T>
47 {
48 }
49 
50 // Methods implementations
51 // //////////////////////////////////
52 
53 template <class GRAPH_T>
56  mrpt::obs::CSensoryFrame::Ptr observations,
57  mrpt::obs::CObservation::Ptr observation)
58 {
59  MRPT_START;
60  MRPT_UNUSED_PARAM(action);
61  using namespace mrpt::obs;
62 
63  // check possible prior node registration
64  bool registered_new_node = false;
65 
66  if (this->m_last_total_num_nodes < this->m_graph->nodeCount())
67  {
68  registered_new_node = true;
69  this->m_last_total_num_nodes = this->m_graph->nodeCount();
70  MRPT_LOG_DEBUG("New node has been registered!");
71  }
72 
73  if (observation)
74  { // observation-only rawlog format
75  if (IS_CLASS(observation, CObservation2DRangeScan))
76  {
77  m_last_laser_scan2D =
78  std::dynamic_pointer_cast<mrpt::obs::CObservation2DRangeScan>(
79  observation);
80  m_is_using_3DScan = false;
81  }
82  if (IS_CLASS(observation, CObservation3DRangeScan))
83  {
84  m_last_laser_scan3D =
85  std::dynamic_pointer_cast<mrpt::obs::CObservation3DRangeScan>(
86  observation);
87  // just load the range/intensity images - CGraphSlanEngine takes
88  // care
89  // of the path
90  m_last_laser_scan3D->load();
91 
92  // grab fake 2D range scan for visualization
93  this->convert3DTo2DRangeScan(
94  /*from = */ m_last_laser_scan3D,
95  /*to = */ &m_fake_laser_scan2D);
96 
97  m_is_using_3DScan = true;
98  }
99 
100  // New node has been registered.
101  // add the last laser_scan
102  if (registered_new_node)
103  {
104  if (m_last_laser_scan2D)
105  {
106  this->m_nodes_to_laser_scans2D[this->m_graph->nodeCount() - 1] =
107  m_last_laser_scan2D;
109  mrpt::format(
110  "Added laser scans of nodeID: %u",
111  (unsigned)(this->m_graph->nodeCount() - 1)));
112  }
113  if (m_last_laser_scan3D)
114  {
115  m_nodes_to_laser_scans3D[this->m_graph->nodeCount() - 1] =
116  m_last_laser_scan3D;
118  mrpt::format(
119  "Added laser scans of nodeID: %u",
120  (unsigned)(this->m_graph->nodeCount() - 1)));
121  }
122  }
123  }
124  else
125  { // action-observations rawlog format
126  // append current laser scan
127  m_last_laser_scan2D =
128  observations->getObservationByClass<CObservation2DRangeScan>();
129  if (registered_new_node && m_last_laser_scan2D)
130  {
131  this->m_nodes_to_laser_scans2D[this->m_graph->nodeCount() - 1] =
132  m_last_laser_scan2D;
133  }
134  }
135 
136  // edge registration procedure - same for both rawlog formats
137  if (registered_new_node)
138  {
139  // get set of nodes within predefined distance for ICP
140  std::set<mrpt::graphs::TNodeID> nodes_to_check_ICP;
141  this->getNearbyNodesOf(
142  &nodes_to_check_ICP, this->m_graph->nodeCount() - 1,
143  params.ICP_max_distance);
145  "Found * %lu * nodes close to nodeID %lu",
146  nodes_to_check_ICP.size(), this->m_graph->nodeCount() - 1);
147 
148  // reset the loop_closure flag and run registration
149  this->m_just_inserted_lc = false;
150  registered_new_node = false;
151 
152  if (m_is_using_3DScan)
153  {
154  checkRegistrationCondition3D(nodes_to_check_ICP);
155  }
156  else
157  {
158  checkRegistrationCondition2D(nodes_to_check_ICP);
159  }
160  }
161 
162  return true;
163  MRPT_END;
164 }
165 
166 template <class GRAPH_T>
168  const std::set<mrpt::graphs::TNodeID>& nodes_set)
169 {
170  MRPT_START;
171  using namespace mrpt;
172  using namespace mrpt::obs;
173  using namespace mrpt::math;
174 
175  mrpt::graphs::TNodeID curr_nodeID = this->m_graph->nodeCount() - 1;
176  CObservation2DRangeScan::Ptr curr_laser_scan;
177  typename nodes_to_scans2D_t::const_iterator search;
178 
179  // search for curr_laser_scan
180  search = this->m_nodes_to_laser_scans2D.find(curr_nodeID);
181  if (search != this->m_nodes_to_laser_scans2D.end())
182  {
183  curr_laser_scan = search->second;
184  }
185 
186  // commence only if I have the current laser scan
187  if (curr_laser_scan)
188  {
189  // try adding ICP constraints with each node in the previous set
191  nodes_set.begin();
192  node_it != nodes_set.end(); ++node_it)
193  {
194  // get the ICP edge between current and last node
195  constraint_t rel_edge;
197  CObservation2DRangeScan::Ptr prev_laser_scan;
198 
199  // search for prev_laser_scan
200  search = this->m_nodes_to_laser_scans2D.find(*node_it);
201  if (search != this->m_nodes_to_laser_scans2D.end())
202  {
203  prev_laser_scan = search->second;
204 
205  // make use of initial node position difference for the ICP edge
206  pose_t initial_pose = this->m_graph->nodes[curr_nodeID] -
207  this->m_graph->nodes[*node_it];
208 
209  this->m_time_logger.enter("CICPCriteriaERD::getICPEdge");
210  this->getICPEdge(
211  *prev_laser_scan, *curr_laser_scan, &rel_edge,
212  &initial_pose, &icp_info);
213  this->m_time_logger.leave("CICPCriteriaERD::getICPEdge");
214 
215  // Debugging statements
217  ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>"
218  ">>>>>>>>>");
220  "ICP constraint between NON-successive nodes: "
221  << *node_it << " => " << curr_nodeID << std::endl
222  << "\tnIterations = " << icp_info.nIterations
223  << "\tgoodness = " << icp_info.goodness);
225  "ICP_goodness_thresh: " << params.ICP_goodness_thresh);
227  "<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<"
228  "<<<<<<<<<");
229 
230  // criterion for registering a new node
231  if (icp_info.goodness > params.ICP_goodness_thresh)
232  {
233  this->registerNewEdge(*node_it, curr_nodeID, rel_edge);
234  m_edge_types_to_nums["ICP2D"]++;
235  // in case of loop closure
236  if (absDiff(curr_nodeID, *node_it) >
237  params.LC_min_nodeid_diff)
238  {
239  m_edge_types_to_nums["LC"]++;
240  this->m_just_inserted_lc = true;
241  }
242  }
243  }
244  }
245  }
246 
247  MRPT_END;
248 }
249 template <class GRAPH_T>
251  const std::set<mrpt::graphs::TNodeID>& nodes_set)
252 {
253  MRPT_START;
254  using namespace std;
255  using namespace mrpt::obs;
256  using namespace mrpt::math;
257 
258  mrpt::graphs::TNodeID curr_nodeID = this->m_graph->nodeCount() - 1;
259  CObservation3DRangeScan::Ptr curr_laser_scan;
260  std::map<mrpt::graphs::TNodeID,
262  // search for curr_laser_scan
263  search = m_nodes_to_laser_scans3D.find(curr_nodeID);
264  if (search != m_nodes_to_laser_scans3D.end())
265  {
266  curr_laser_scan = search->second;
267  }
268 
269  // commence only if I have the current laser scan
270  if (curr_laser_scan)
271  {
272  // try adding ICP constraints with each node in the previous set
274  nodes_set.begin();
275  node_it != nodes_set.end(); ++node_it)
276  {
277  // get the ICP edge between current and last node
278  constraint_t rel_edge;
280  CObservation3DRangeScan::Ptr prev_laser_scan;
281 
282  // search for prev_laser_scan
283  search = m_nodes_to_laser_scans3D.find(*node_it);
284  if (search != m_nodes_to_laser_scans3D.end())
285  {
286  prev_laser_scan = search->second;
287 
288  // TODO - use initial edge estimation
289  this->m_time_logger.enter("CICPCriteriaERD::getICPEdge");
290  this->getICPEdge(
291  *prev_laser_scan, *curr_laser_scan, &rel_edge, nullptr,
292  &icp_info);
293  this->m_time_logger.leave("CICPCriteriaERD::getICPEdge");
294 
295  // criterion for registering a new node
296  if (icp_info.goodness > params.ICP_goodness_thresh)
297  {
298  this->registerNewEdge(*node_it, curr_nodeID, rel_edge);
299  m_edge_types_to_nums["ICP3D"]++;
300  // in case of loop closure
301  if (absDiff(curr_nodeID, *node_it) >
302  params.LC_min_nodeid_diff)
303  {
304  m_edge_types_to_nums["LC"]++;
305  this->m_just_inserted_lc = true;
306  }
307  }
308  }
309  }
310  }
311 
312  MRPT_END;
313 }
314 
315 template <class GRAPH_T>
317  const mrpt::graphs::TNodeID& from, const mrpt::graphs::TNodeID& to,
318  const constraint_t& rel_edge)
319 {
320  parent_t::registerNewEdge(from, to, rel_edge);
321 
322  this->m_graph->insertEdge(from, to, rel_edge);
323 }
324 
325 template <class GRAPH_T>
327  std::set<mrpt::graphs::TNodeID>* nodes_set,
328  const mrpt::graphs::TNodeID& cur_nodeID, double distance)
329 {
330  MRPT_START;
331 
332  if (distance > 0)
333  {
334  // check all but the last node.
335  for (mrpt::graphs::TNodeID nodeID = 0;
336  nodeID < this->m_graph->nodeCount() - 1; ++nodeID)
337  {
338  double curr_distance = this->m_graph->nodes[nodeID].distanceTo(
339  this->m_graph->nodes[cur_nodeID]);
340  if (curr_distance <= distance)
341  {
342  nodes_set->insert(nodeID);
343  }
344  }
345  }
346  else
347  { // check against all nodes
348  this->m_graph->getAllNodes(*nodes_set);
349  }
350 
351  MRPT_END;
352 }
353 
354 template <class GRAPH_T>
356  const std::map<std::string, bool>& events_occurred)
357 {
358  MRPT_START;
359  parent_t::notifyOfWindowEvents(events_occurred);
360 
361  // I know the key exists - I put it there explicitly
362  if (events_occurred.find(params.keystroke_laser_scans)->second)
363  {
364  this->toggleLaserScansVisualization();
365  }
366 
367  MRPT_END;
368 }
369 
370 template <class GRAPH_T>
372 {
373  MRPT_START;
374  ASSERTDEBMSG_(this->m_win, "No CDisplayWindow3D* was provided");
375  ASSERTDEBMSG_(this->m_win_manager, "No CWindowManager* was provided");
376 
377  using namespace mrpt::opengl;
378 
379  this->logFmt(
380  mrpt::system::LVL_INFO, "Toggling LaserScans visualization...");
381 
382  COpenGLScene::Ptr scene = this->m_win->get3DSceneAndLock();
383 
384  if (params.visualize_laser_scans)
385  {
386  CRenderizable::Ptr obj = scene->getByName("laser_scan_viz");
387  obj->setVisibility(!obj->isVisible());
388  }
389  else
390  {
391  dumpVisibilityErrorMsg("visualize_laser_scans");
392  }
393 
394  this->m_win->unlockAccess3DScene();
395  this->m_win->forceRepaint();
396 
397  MRPT_END;
398 }
399 
400 template <class GRAPH_T>
402  std::map<std::string, int>* edge_types_to_num) const
403 {
404  MRPT_START;
405 
406  *edge_types_to_num = m_edge_types_to_nums;
407 
408  MRPT_END;
409 }
410 
411 template <class GRAPH_T>
413 {
414  MRPT_START;
415  using namespace mrpt::opengl;
416  this->logFmt(mrpt::system::LVL_DEBUG, "Initializing visuals");
417  this->m_time_logger.enter("CICPCriteriaERD::Visuals");
418  parent_t::initializeVisuals();
419 
421  params.has_read_config, "Configuration parameters aren't loaded yet");
422 
423  this->m_win_observer->registerKeystroke(
424  params.keystroke_laser_scans, "Toggle LaserScans Visualization");
425 
426  // ICP_max_distance disk
427  if (params.ICP_max_distance > 0)
428  {
429  COpenGLScene::Ptr scene = this->m_win->get3DSceneAndLock();
430 
431  CDisk::Ptr obj = mrpt::make_aligned_shared<CDisk>();
432  pose_t initial_pose;
433  obj->setPose(initial_pose);
434  obj->setName("ICP_max_distance");
435  obj->setColor_u8(m_search_disk_color);
436  obj->setDiskRadius(
437  params.ICP_max_distance, params.ICP_max_distance - 0.1);
438  scene->insert(obj);
439 
440  this->m_win->unlockAccess3DScene();
441  this->m_win->forceRepaint();
442  }
443 
444  // laser scan visualization
445  if (params.visualize_laser_scans)
446  {
447  COpenGLScene::Ptr scene = this->m_win->get3DSceneAndLock();
448 
449  CPlanarLaserScan::Ptr laser_scan_viz =
450  mrpt::make_aligned_shared<mrpt::opengl::CPlanarLaserScan>();
451  laser_scan_viz->enablePoints(true);
452  laser_scan_viz->enableLine(true);
453  laser_scan_viz->enableSurface(true);
454  laser_scan_viz->setSurfaceColor(
455  m_laser_scans_color.R, m_laser_scans_color.G, m_laser_scans_color.B,
456  m_laser_scans_color.A);
457 
458  laser_scan_viz->setName("laser_scan_viz");
459 
460  scene->insert(laser_scan_viz);
461  this->m_win->unlockAccess3DScene();
462  this->m_win->forceRepaint();
463  }
464 
465  // max distance disk - textMessage
466  if (this->m_win && this->m_win_manager && params.ICP_max_distance > 0)
467  {
468  this->m_win_manager->assignTextMessageParameters(
469  &m_offset_y_search_disk, &m_text_index_search_disk);
470 
471  this->m_win_manager->addTextMessage(
472  5, -m_offset_y_search_disk, mrpt::format("ICP Edges search radius"),
473  mrpt::img::TColorf(m_search_disk_color),
474  /* unique_index = */ m_text_index_search_disk);
475  }
476 
477  this->m_time_logger.leave("CICPCriteriaERD::Visuals");
478  MRPT_END;
479 }
480 template <class GRAPH_T>
482 {
483  MRPT_START;
484  this->m_time_logger.enter("CICPCriteriaERD::Visuals");
485  using namespace mrpt::opengl;
486  using namespace mrpt::math;
487  using namespace mrpt::poses;
488  parent_t::updateVisuals();
489 
490  // update ICP_max_distance Disk
491  if (this->m_win && params.ICP_max_distance > 0)
492  {
493  COpenGLScene::Ptr scene = this->m_win->get3DSceneAndLock();
494 
495  CRenderizable::Ptr obj = scene->getByName("ICP_max_distance");
496  CDisk::Ptr disk_obj = std::dynamic_pointer_cast<CDisk>(obj);
497 
498  disk_obj->setPose(this->m_graph->nodes[this->m_graph->nodeCount() - 1]);
499 
500  this->m_win->unlockAccess3DScene();
501  this->m_win->forceRepaint();
502  }
503 
504  // update laser scan visual
505  if (this->m_win && params.visualize_laser_scans &&
506  (m_last_laser_scan2D || m_fake_laser_scan2D))
507  {
508  COpenGLScene::Ptr scene = this->m_win->get3DSceneAndLock();
509 
510  CRenderizable::Ptr obj = scene->getByName("laser_scan_viz");
511  CPlanarLaserScan::Ptr laser_scan_viz =
512  std::dynamic_pointer_cast<CPlanarLaserScan>(obj);
513 
514  // if fake 2D exists use it
515  if (m_fake_laser_scan2D)
516  {
517  laser_scan_viz->setScan(*m_fake_laser_scan2D);
518  }
519  else
520  {
521  laser_scan_viz->setScan(*m_last_laser_scan2D);
522  }
523 
524  // set the pose of the laser scan
526  this->m_graph->nodes.find(this->m_graph->nodeCount() - 1);
527  if (search != this->m_graph->nodes.end())
528  {
529  laser_scan_viz->setPose(
530  this->m_graph->nodes[this->m_graph->nodeCount() - 1]);
531  // put the laser scan *underneath* the graph, so that you can still
532  // visualize the loop closures with the nodes ahead
533  laser_scan_viz->setPose(
534  CPose3D(
535  laser_scan_viz->getPoseX(), laser_scan_viz->getPoseY(),
536  -0.15, DEG2RAD(laser_scan_viz->getPoseYaw()),
537  DEG2RAD(laser_scan_viz->getPosePitch()),
538  DEG2RAD(laser_scan_viz->getPoseRoll())));
539  }
540 
541  this->m_win->unlockAccess3DScene();
542  this->m_win->forceRepaint();
543  }
544 
545  this->m_time_logger.leave("CICPCriteriaERD::Visuals");
546  MRPT_END;
547 }
548 
549 template <class GRAPH_T>
551  std::string viz_flag, int sleep_time /* = 500 milliseconds */)
552 {
553  MRPT_START;
554  using namespace mrpt;
555 
557  "Cannot toggle visibility of specified object.\n "
558  "Make sure that the corresponding visualization flag ( %s "
559  ") is set to true in the .ini file.\n",
560  viz_flag.c_str());
561  std::this_thread::sleep_for(std::chrono::milliseconds(sleep_time));
562 
563  MRPT_END;
564 }
565 
566 template <class GRAPH_T>
568 {
569  MRPT_START;
570  parent_t::loadParams(source_fname);
571 
572  params.loadFromConfigFileName(
573  source_fname, "EdgeRegistrationDeciderParameters");
574  MRPT_LOG_DEBUG("Successfully loaded parameters. ");
575 
576  // set the logging level if given by the user
577  mrpt::config::CConfigFile source(source_fname);
578  int min_verbosity_level = source.read_int(
579  "EdgeRegistrationDeciderParameters", "class_verbosity", 1, false);
580  this->setMinLoggingLevel(mrpt::system::VerbosityLevel(min_verbosity_level));
581 
582  MRPT_END;
583 }
584 template <class GRAPH_T>
586 {
587  MRPT_START;
588  parent_t::printParams();
589  params.dumpToConsole();
590 
591  MRPT_END;
592 }
593 
594 template <class GRAPH_T>
596  std::string* report_str) const
597 {
598  MRPT_START;
599  using namespace std;
600 
601  const std::string report_sep(2, '\n');
602  const std::string header_sep(80, '#');
603 
604  // Report on graph
605  stringstream class_props_ss;
606  class_props_ss << "ICP Goodness-based Registration Procedure Summary: "
607  << std::endl;
608  class_props_ss << header_sep << std::endl;
609 
610  // time and output logging
611  const std::string time_res = this->m_time_logger.getStatsAsText();
612  const std::string output_res = this->getLogAsString();
613 
614  // merge the individual reports
615  report_str->clear();
616  parent_t::getDescriptiveReport(report_str);
617 
618  *report_str += class_props_ss.str();
619  *report_str += report_sep;
620 
621  *report_str += time_res;
622  *report_str += report_sep;
623 
624  *report_str += output_res;
625  *report_str += report_sep;
626 
627  MRPT_END;
628 }
629 
630 // TParameter
631 // //////////////////////////////////
632 
633 template <class GRAPH_T>
635  : decider(d), keystroke_laser_scans("l"), has_read_config(false)
636 {
637 }
638 
639 template <class GRAPH_T>
641 {
642 }
643 
644 template <class GRAPH_T>
646  std::ostream& out) const
647 {
648  MRPT_START;
649 
650  out << mrpt::format(
651  "------------------[ Goodness-based ICP Edge Registration "
652  "]------------------\n");
653  out << mrpt::format(
654  "ICP goodness threshold = %.2f%% \n",
655  ICP_goodness_thresh * 100);
656  out << mrpt::format(
657  "ICP max radius for edge search = %.2f\n", ICP_max_distance);
658  out << mrpt::format(
659  "Min. node difference for LC = %lu\n", LC_min_nodeid_diff);
660  out << mrpt::format(
661  "Visualize laser scans = %d\n", visualize_laser_scans);
662  out << mrpt::format(
663  "3DScans Image Directory = %s\n",
664  scans_img_external_dir.c_str());
665 
666  MRPT_END;
667 }
668 template <class GRAPH_T>
670  const mrpt::config::CConfigFileBase& source, const std::string& section)
671 {
672  MRPT_START;
673 
674  LC_min_nodeid_diff = source.read_int(
675  "GeneralConfiguration", "LC_min_nodeid_diff", 30, false);
676  ICP_max_distance =
677  source.read_double(section, "ICP_max_distance", 10, false);
678  ICP_goodness_thresh =
679  source.read_double(section, "ICP_goodness_thresh", 0.75, false);
680  visualize_laser_scans = source.read_bool(
681  "VisualizationParameters", "visualize_laser_scans", true, false);
682  scans_img_external_dir = source.read_string(
683  section, "scan_images_external_directory", "./", false);
684 
685  has_read_config = true;
686 
687  MRPT_END;
688 }
689 }
690 }
691 } // end of namespaces
692 
693 #endif /* end of include guard: CICPCRITERIAERD_IMPL_H */
void checkRegistrationCondition2D(const std::set< mrpt::graphs::TNodeID > &nodes_set)
#define MRPT_START
Definition: exceptions.h:262
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
typename GRAPH_T::constraint_t::type_value pose_t
type of underlying poses (2D/3D).
VerbosityLevel
Enumeration of available verbosity levels.
#define MRPT_LOG_ERROR_FMT(_FMT_STRING,...)
void dumpToTextStream(std::ostream &out) const
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
void notifyOfWindowEvents(const std::map< std::string, bool > &events_occurred)
Get a list of the window events that happened since the last call.
void dumpVisibilityErrorMsg(std::string viz_flag, int sleep_time=500)
double DEG2RAD(const double x)
Degrees to radians.
This class allows loading and storing values and vectors of different types from ".ini" files easily.
Declares a class derived from "CObservation" that encapsules a 3D range scan measurement, as from a time-of-flight range camera or any other RGBD sensor.
bool updateState(mrpt::obs::CActionCollection::Ptr action, mrpt::obs::CSensoryFrame::Ptr observations, mrpt::obs::CObservation::Ptr observation)
Generic method for fetching the incremental action/observation readings from the calling function...
STL namespace.
size_t m_last_total_num_nodes
Keep track of the total number of registered nodes since the last time class method was called...
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
void toggleLaserScansVisualization()
togle the LaserScans visualization on and off
float goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences...
Definition: CICP.h:199
This class allows loading and storing values and vectors of different types from a configuration text...
This base provides a set of functions for maths stuff.
void registerNewEdge(const mrpt::graphs::TNodeID &from, const mrpt::graphs::TNodeID &to, const constraint_t &rel_edge)
Register a new constraint/edge in the current graph.
#define MRPT_LOG_DEBUG_FMT(_FMT_STRING,...)
Use: MRPT_LOG_DEBUG_FMT("i=%u", i);
A planar disk in the XY plane.
Definition: CDisk.h:33
void getDescriptiveReport(std::string *report_str) const
Fill the provided string with a detailed report of the decider/optimizer state.
void checkRegistrationCondition3D(const std::set< mrpt::graphs::TNodeID > &nodes_set)
This namespace contains representation of robot actions and observations.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
This object renders a 2D laser scan by means of three elements: the points, the line along end-points...
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section)
This method load the options from a ".ini"-like file or memory-stored string list.
virtual void initializeLoggers(const std::string &name)
Initialize the COutputLogger, CTimeLogger instances given the name of the decider/optimizer at hand...
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
Use: MRPT_LOG_DEBUG_STREAM("Var=" << value << " foo=" << foo_var);
unsigned short nIterations
The number of executed iterations until convergence.
Definition: CICP.h:196
GLsizei const GLchar ** string
Definition: glext.h:4101
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
std::map< std::string, int > m_edge_types_to_nums
#define ASSERTDEBMSG_(f, __ERROR_MSG)
Definition: exceptions.h:208
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::rtti::CObject) is of the give...
Definition: CObject.h:103
#define MRPT_END
Definition: exceptions.h:266
A RGB color - floats in the range [0,1].
Definition: TColor.h:79
The ICP algorithm return information.
Definition: CICP.h:192
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
void printParams() const
Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact w...
GLsizei GLsizei GLchar * source
Definition: glext.h:4082
uint64_t TNodeID
A generic numeric type for unique IDs of nodes or entities.
Definition: TNodeID.h:17
void getNearbyNodesOf(std::set< mrpt::graphs::TNodeID > *nodes_set, const mrpt::graphs::TNodeID &cur_nodeID, double distance)
Get a list of the nodeIDs whose position is within a certain distance to the specified nodeID...
T absDiff(const T &lhs, const T &rhs)
Absolute difference between two numbers.
void updateVisuals()
Update the relevant visual features in CDisplayWindow.
void getEdgesStats(std::map< std::string, int > *edge_types_to_num) const
Fill the given map with the type of registered edges as well as the corresponding number of registrat...
GLenum const GLfloat * params
Definition: glext.h:3534
const Scalar * const_iterator
Definition: eigen_plugins.h:27
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1891
virtual void load() const override
Makes sure all images and other fields which may be externally stored are loaded in memory...
void loadParams(const std::string &source_fname)
Fetch the latest observation that the current instance received (most probably during a call to the u...
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
typename GRAPH_T::constraint_t constraint_t
Handy typedefs.
void initializeVisuals()
Initialize visual objects in CDisplayWindow (e.g.



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