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 
15 {
16 // Ctors, Dtors
17 // //////////////////////////////////
18 
19 template <class GRAPH_T>
21  : params(*this), // pass reference to self when initializing the parameters
22  m_search_disk_color(142, 142, 56),
23  m_laser_scans_color(0, 20, 255),
24  m_is_using_3DScan(false)
25 {
26  MRPT_START;
27 
28  this->initializeLoggers("CICPCriteriaERD");
29 
30  // start ICP constraint registration only when
31  m_edge_types_to_nums["ICP2D"] = 0;
32  m_edge_types_to_nums["ICP3D"] = 0;
33  m_edge_types_to_nums["LC"] = 0;
34 
35  this->m_last_total_num_nodes = 2;
36 
37  MRPT_LOG_DEBUG("Initialized class object");
38 
39  MRPT_END;
40 }
41 template <class GRAPH_T>
43 {
44 }
45 
46 // Methods implementations
47 // //////////////////////////////////
48 
49 template <class GRAPH_T>
52  mrpt::obs::CSensoryFrame::Ptr observations,
53  mrpt::obs::CObservation::Ptr observation)
54 {
55  MRPT_START;
56  MRPT_UNUSED_PARAM(action);
57  using namespace mrpt::obs;
58 
59  // check possible prior node registration
60  bool registered_new_node = false;
61 
62  if (this->m_last_total_num_nodes < this->m_graph->nodeCount())
63  {
64  registered_new_node = true;
65  this->m_last_total_num_nodes = this->m_graph->nodeCount();
66  MRPT_LOG_DEBUG("New node has been registered!");
67  }
68 
69  if (observation)
70  { // observation-only rawlog format
71  if (IS_CLASS(observation, CObservation2DRangeScan))
72  {
73  m_last_laser_scan2D =
74  std::dynamic_pointer_cast<mrpt::obs::CObservation2DRangeScan>(
75  observation);
76  m_is_using_3DScan = false;
77  }
78  if (IS_CLASS(observation, CObservation3DRangeScan))
79  {
80  m_last_laser_scan3D =
81  std::dynamic_pointer_cast<mrpt::obs::CObservation3DRangeScan>(
82  observation);
83  // just load the range/intensity images - CGraphSlanEngine takes
84  // care
85  // of the path
86  m_last_laser_scan3D->load();
87 
88  // grab fake 2D range scan for visualization
89  this->convert3DTo2DRangeScan(
90  /*from = */ m_last_laser_scan3D,
91  /*to = */ &m_fake_laser_scan2D);
92 
93  m_is_using_3DScan = true;
94  }
95 
96  // New node has been registered.
97  // add the last laser_scan
98  if (registered_new_node)
99  {
100  if (m_last_laser_scan2D)
101  {
102  this->m_nodes_to_laser_scans2D[this->m_graph->nodeCount() - 1] =
103  m_last_laser_scan2D;
105  mrpt::format(
106  "Added laser scans of nodeID: %u",
107  (unsigned)(this->m_graph->nodeCount() - 1)));
108  }
109  if (m_last_laser_scan3D)
110  {
111  m_nodes_to_laser_scans3D[this->m_graph->nodeCount() - 1] =
112  m_last_laser_scan3D;
114  mrpt::format(
115  "Added laser scans of nodeID: %u",
116  (unsigned)(this->m_graph->nodeCount() - 1)));
117  }
118  }
119  }
120  else
121  { // action-observations rawlog format
122  // append current laser scan
123  m_last_laser_scan2D =
124  observations->getObservationByClass<CObservation2DRangeScan>();
125  if (registered_new_node && m_last_laser_scan2D)
126  {
127  this->m_nodes_to_laser_scans2D[this->m_graph->nodeCount() - 1] =
128  m_last_laser_scan2D;
129  }
130  }
131 
132  // edge registration procedure - same for both rawlog formats
133  if (registered_new_node)
134  {
135  // get set of nodes within predefined distance for ICP
136  std::set<mrpt::graphs::TNodeID> nodes_to_check_ICP;
137  this->getNearbyNodesOf(
138  &nodes_to_check_ICP, this->m_graph->nodeCount() - 1,
139  params.ICP_max_distance);
141  "Found * %lu * nodes close to nodeID %lu",
142  nodes_to_check_ICP.size(), this->m_graph->nodeCount() - 1);
143 
144  // reset the loop_closure flag and run registration
145  this->m_just_inserted_lc = false;
146  registered_new_node = false;
147 
148  if (m_is_using_3DScan)
149  {
150  checkRegistrationCondition3D(nodes_to_check_ICP);
151  }
152  else
153  {
154  checkRegistrationCondition2D(nodes_to_check_ICP);
155  }
156  }
157 
158  return true;
159  MRPT_END;
160 }
161 
162 template <class GRAPH_T>
164  const std::set<mrpt::graphs::TNodeID>& nodes_set)
165 {
166  MRPT_START;
167  using namespace mrpt;
168  using namespace mrpt::obs;
169  using namespace mrpt::math;
170 
171  mrpt::graphs::TNodeID curr_nodeID = this->m_graph->nodeCount() - 1;
172  CObservation2DRangeScan::Ptr curr_laser_scan;
173  typename nodes_to_scans2D_t::const_iterator search;
174 
175  // search for curr_laser_scan
176  search = this->m_nodes_to_laser_scans2D.find(curr_nodeID);
177  if (search != this->m_nodes_to_laser_scans2D.end())
178  {
179  curr_laser_scan = search->second;
180  }
181 
182  // commence only if I have the current laser scan
183  if (curr_laser_scan)
184  {
185  // try adding ICP constraints with each node in the previous set
187  nodes_set.begin();
188  node_it != nodes_set.end(); ++node_it)
189  {
190  // get the ICP edge between current and last node
191  constraint_t rel_edge;
193  CObservation2DRangeScan::Ptr prev_laser_scan;
194 
195  // search for prev_laser_scan
196  search = this->m_nodes_to_laser_scans2D.find(*node_it);
197  if (search != this->m_nodes_to_laser_scans2D.end())
198  {
199  prev_laser_scan = search->second;
200 
201  // make use of initial node position difference for the ICP edge
202  pose_t initial_pose = this->m_graph->nodes[curr_nodeID] -
203  this->m_graph->nodes[*node_it];
204 
205  this->m_time_logger.enter("CICPCriteriaERD::getICPEdge");
206  this->getICPEdge(
207  *prev_laser_scan, *curr_laser_scan, &rel_edge,
208  &initial_pose, &icp_info);
209  this->m_time_logger.leave("CICPCriteriaERD::getICPEdge");
210 
211  // Debugging statements
213  ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>"
214  ">>>>>>>>>");
216  "ICP constraint between NON-successive nodes: "
217  << *node_it << " => " << curr_nodeID << std::endl
218  << "\tnIterations = " << icp_info.nIterations
219  << "\tgoodness = " << icp_info.goodness);
221  "ICP_goodness_thresh: " << params.ICP_goodness_thresh);
223  "<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<"
224  "<<<<<<<<<");
225 
226  // criterion for registering a new node
227  if (icp_info.goodness > params.ICP_goodness_thresh)
228  {
229  this->registerNewEdge(*node_it, curr_nodeID, rel_edge);
230  m_edge_types_to_nums["ICP2D"]++;
231  // in case of loop closure
232  if (absDiff(curr_nodeID, *node_it) >
233  params.LC_min_nodeid_diff)
234  {
235  m_edge_types_to_nums["LC"]++;
236  this->m_just_inserted_lc = true;
237  }
238  }
239  }
240  }
241  }
242 
243  MRPT_END;
244 }
245 template <class GRAPH_T>
247  const std::set<mrpt::graphs::TNodeID>& nodes_set)
248 {
249  MRPT_START;
250  using namespace std;
251  using namespace mrpt::obs;
252  using namespace mrpt::math;
253 
254  mrpt::graphs::TNodeID curr_nodeID = this->m_graph->nodeCount() - 1;
255  CObservation3DRangeScan::Ptr curr_laser_scan;
256  std::map<mrpt::graphs::TNodeID,
258  // search for curr_laser_scan
259  search = m_nodes_to_laser_scans3D.find(curr_nodeID);
260  if (search != m_nodes_to_laser_scans3D.end())
261  {
262  curr_laser_scan = search->second;
263  }
264 
265  // commence only if I have the current laser scan
266  if (curr_laser_scan)
267  {
268  // try adding ICP constraints with each node in the previous set
270  nodes_set.begin();
271  node_it != nodes_set.end(); ++node_it)
272  {
273  // get the ICP edge between current and last node
274  constraint_t rel_edge;
276  CObservation3DRangeScan::Ptr prev_laser_scan;
277 
278  // search for prev_laser_scan
279  search = m_nodes_to_laser_scans3D.find(*node_it);
280  if (search != m_nodes_to_laser_scans3D.end())
281  {
282  prev_laser_scan = search->second;
283 
284  // TODO - use initial edge estimation
285  this->m_time_logger.enter("CICPCriteriaERD::getICPEdge");
286  this->getICPEdge(
287  *prev_laser_scan, *curr_laser_scan, &rel_edge, nullptr,
288  &icp_info);
289  this->m_time_logger.leave("CICPCriteriaERD::getICPEdge");
290 
291  // criterion for registering a new node
292  if (icp_info.goodness > params.ICP_goodness_thresh)
293  {
294  this->registerNewEdge(*node_it, curr_nodeID, rel_edge);
295  m_edge_types_to_nums["ICP3D"]++;
296  // in case of loop closure
297  if (absDiff(curr_nodeID, *node_it) >
298  params.LC_min_nodeid_diff)
299  {
300  m_edge_types_to_nums["LC"]++;
301  this->m_just_inserted_lc = true;
302  }
303  }
304  }
305  }
306  }
307 
308  MRPT_END;
309 }
310 
311 template <class GRAPH_T>
313  const mrpt::graphs::TNodeID& from, const mrpt::graphs::TNodeID& to,
314  const constraint_t& rel_edge)
315 {
316  parent_t::registerNewEdge(from, to, rel_edge);
317 
318  this->m_graph->insertEdge(from, to, rel_edge);
319 }
320 
321 template <class GRAPH_T>
323  std::set<mrpt::graphs::TNodeID>* nodes_set,
324  const mrpt::graphs::TNodeID& cur_nodeID, double distance)
325 {
326  MRPT_START;
327 
328  if (distance > 0)
329  {
330  // check all but the last node.
331  for (mrpt::graphs::TNodeID nodeID = 0;
332  nodeID < this->m_graph->nodeCount() - 1; ++nodeID)
333  {
334  double curr_distance = this->m_graph->nodes[nodeID].distanceTo(
335  this->m_graph->nodes[cur_nodeID]);
336  if (curr_distance <= distance)
337  {
338  nodes_set->insert(nodeID);
339  }
340  }
341  }
342  else
343  { // check against all nodes
344  this->m_graph->getAllNodes(*nodes_set);
345  }
346 
347  MRPT_END;
348 }
349 
350 template <class GRAPH_T>
352  const std::map<std::string, bool>& events_occurred)
353 {
354  MRPT_START;
355  parent_t::notifyOfWindowEvents(events_occurred);
356 
357  // I know the key exists - I put it there explicitly
358  if (events_occurred.find(params.keystroke_laser_scans)->second)
359  {
360  this->toggleLaserScansVisualization();
361  }
362 
363  MRPT_END;
364 }
365 
366 template <class GRAPH_T>
368 {
369  MRPT_START;
370  ASSERTDEBMSG_(this->m_win, "No CDisplayWindow3D* was provided");
371  ASSERTDEBMSG_(this->m_win_manager, "No CWindowManager* was provided");
372 
373  using namespace mrpt::opengl;
374 
375  this->logFmt(
376  mrpt::system::LVL_INFO, "Toggling LaserScans visualization...");
377 
378  COpenGLScene::Ptr scene = this->m_win->get3DSceneAndLock();
379 
380  if (params.visualize_laser_scans)
381  {
382  CRenderizable::Ptr obj = scene->getByName("laser_scan_viz");
383  obj->setVisibility(!obj->isVisible());
384  }
385  else
386  {
387  dumpVisibilityErrorMsg("visualize_laser_scans");
388  }
389 
390  this->m_win->unlockAccess3DScene();
391  this->m_win->forceRepaint();
392 
393  MRPT_END;
394 }
395 
396 template <class GRAPH_T>
398  std::map<std::string, int>* edge_types_to_num) const
399 {
400  MRPT_START;
401 
402  *edge_types_to_num = m_edge_types_to_nums;
403 
404  MRPT_END;
405 }
406 
407 template <class GRAPH_T>
409 {
410  MRPT_START;
411  using namespace mrpt::opengl;
412  this->logFmt(mrpt::system::LVL_DEBUG, "Initializing visuals");
413  this->m_time_logger.enter("CICPCriteriaERD::Visuals");
414  parent_t::initializeVisuals();
415 
417  params.has_read_config, "Configuration parameters aren't loaded yet");
418 
419  this->m_win_observer->registerKeystroke(
420  params.keystroke_laser_scans, "Toggle LaserScans Visualization");
421 
422  // ICP_max_distance disk
423  if (params.ICP_max_distance > 0)
424  {
425  COpenGLScene::Ptr scene = this->m_win->get3DSceneAndLock();
426 
427  CDisk::Ptr obj = mrpt::make_aligned_shared<CDisk>();
428  pose_t initial_pose;
429  obj->setPose(initial_pose);
430  obj->setName("ICP_max_distance");
431  obj->setColor_u8(m_search_disk_color);
432  obj->setDiskRadius(
433  params.ICP_max_distance, params.ICP_max_distance - 0.1);
434  scene->insert(obj);
435 
436  this->m_win->unlockAccess3DScene();
437  this->m_win->forceRepaint();
438  }
439 
440  // laser scan visualization
441  if (params.visualize_laser_scans)
442  {
443  COpenGLScene::Ptr scene = this->m_win->get3DSceneAndLock();
444 
445  CPlanarLaserScan::Ptr laser_scan_viz =
446  mrpt::make_aligned_shared<mrpt::opengl::CPlanarLaserScan>();
447  laser_scan_viz->enablePoints(true);
448  laser_scan_viz->enableLine(true);
449  laser_scan_viz->enableSurface(true);
450  laser_scan_viz->setSurfaceColor(
451  m_laser_scans_color.R, m_laser_scans_color.G, m_laser_scans_color.B,
452  m_laser_scans_color.A);
453 
454  laser_scan_viz->setName("laser_scan_viz");
455 
456  scene->insert(laser_scan_viz);
457  this->m_win->unlockAccess3DScene();
458  this->m_win->forceRepaint();
459  }
460 
461  // max distance disk - textMessage
462  if (this->m_win && this->m_win_manager && params.ICP_max_distance > 0)
463  {
464  this->m_win_manager->assignTextMessageParameters(
465  &m_offset_y_search_disk, &m_text_index_search_disk);
466 
467  this->m_win_manager->addTextMessage(
468  5, -m_offset_y_search_disk, mrpt::format("ICP Edges search radius"),
469  mrpt::img::TColorf(m_search_disk_color),
470  /* unique_index = */ m_text_index_search_disk);
471  }
472 
473  this->m_time_logger.leave("CICPCriteriaERD::Visuals");
474  MRPT_END;
475 }
476 template <class GRAPH_T>
478 {
479  MRPT_START;
480  this->m_time_logger.enter("CICPCriteriaERD::Visuals");
481  using namespace mrpt::opengl;
482  using namespace mrpt::math;
483  using namespace mrpt::poses;
484  parent_t::updateVisuals();
485 
486  // update ICP_max_distance Disk
487  if (this->m_win && params.ICP_max_distance > 0)
488  {
489  COpenGLScene::Ptr scene = this->m_win->get3DSceneAndLock();
490 
491  CRenderizable::Ptr obj = scene->getByName("ICP_max_distance");
492  CDisk::Ptr disk_obj = std::dynamic_pointer_cast<CDisk>(obj);
493 
494  disk_obj->setPose(this->m_graph->nodes[this->m_graph->nodeCount() - 1]);
495 
496  this->m_win->unlockAccess3DScene();
497  this->m_win->forceRepaint();
498  }
499 
500  // update laser scan visual
501  if (this->m_win && params.visualize_laser_scans &&
502  (m_last_laser_scan2D || m_fake_laser_scan2D))
503  {
504  COpenGLScene::Ptr scene = this->m_win->get3DSceneAndLock();
505 
506  CRenderizable::Ptr obj = scene->getByName("laser_scan_viz");
507  CPlanarLaserScan::Ptr laser_scan_viz =
508  std::dynamic_pointer_cast<CPlanarLaserScan>(obj);
509 
510  // if fake 2D exists use it
511  if (m_fake_laser_scan2D)
512  {
513  laser_scan_viz->setScan(*m_fake_laser_scan2D);
514  }
515  else
516  {
517  laser_scan_viz->setScan(*m_last_laser_scan2D);
518  }
519 
520  // set the pose of the laser scan
522  this->m_graph->nodes.find(this->m_graph->nodeCount() - 1);
523  if (search != this->m_graph->nodes.end())
524  {
525  laser_scan_viz->setPose(
526  this->m_graph->nodes[this->m_graph->nodeCount() - 1]);
527  // put the laser scan *underneath* the graph, so that you can still
528  // visualize the loop closures with the nodes ahead
529  laser_scan_viz->setPose(
530  CPose3D(
531  laser_scan_viz->getPoseX(), laser_scan_viz->getPoseY(),
532  -0.15, DEG2RAD(laser_scan_viz->getPoseYaw()),
533  DEG2RAD(laser_scan_viz->getPosePitch()),
534  DEG2RAD(laser_scan_viz->getPoseRoll())));
535  }
536 
537  this->m_win->unlockAccess3DScene();
538  this->m_win->forceRepaint();
539  }
540 
541  this->m_time_logger.leave("CICPCriteriaERD::Visuals");
542  MRPT_END;
543 }
544 
545 template <class GRAPH_T>
547  std::string viz_flag, int sleep_time /* = 500 milliseconds */)
548 {
549  MRPT_START;
550  using namespace mrpt;
551 
553  "Cannot toggle visibility of specified object.\n "
554  "Make sure that the corresponding visualization flag ( %s "
555  ") is set to true in the .ini file.\n",
556  viz_flag.c_str());
557  std::this_thread::sleep_for(std::chrono::milliseconds(sleep_time));
558 
559  MRPT_END;
560 }
561 
562 template <class GRAPH_T>
564 {
565  MRPT_START;
566  parent_t::loadParams(source_fname);
567 
568  params.loadFromConfigFileName(
569  source_fname, "EdgeRegistrationDeciderParameters");
570  MRPT_LOG_DEBUG("Successfully loaded parameters. ");
571 
572  // set the logging level if given by the user
573  mrpt::config::CConfigFile source(source_fname);
574  int min_verbosity_level = source.read_int(
575  "EdgeRegistrationDeciderParameters", "class_verbosity", 1, false);
576  this->setMinLoggingLevel(mrpt::system::VerbosityLevel(min_verbosity_level));
577 
578  MRPT_END;
579 }
580 template <class GRAPH_T>
582 {
583  MRPT_START;
584  parent_t::printParams();
585  params.dumpToConsole();
586 
587  MRPT_END;
588 }
589 
590 template <class GRAPH_T>
592  std::string* report_str) const
593 {
594  MRPT_START;
595  using namespace std;
596 
597  const std::string report_sep(2, '\n');
598  const std::string header_sep(80, '#');
599 
600  // Report on graph
601  stringstream class_props_ss;
602  class_props_ss << "ICP Goodness-based Registration Procedure Summary: "
603  << std::endl;
604  class_props_ss << header_sep << std::endl;
605 
606  // time and output logging
607  const std::string time_res = this->m_time_logger.getStatsAsText();
608  const std::string output_res = this->getLogAsString();
609 
610  // merge the individual reports
611  report_str->clear();
612  parent_t::getDescriptiveReport(report_str);
613 
614  *report_str += class_props_ss.str();
615  *report_str += report_sep;
616 
617  *report_str += time_res;
618  *report_str += report_sep;
619 
620  *report_str += output_res;
621  *report_str += report_sep;
622 
623  MRPT_END;
624 }
625 
626 // TParameter
627 // //////////////////////////////////
628 
629 template <class GRAPH_T>
631  : decider(d), keystroke_laser_scans("l"), has_read_config(false)
632 {
633 }
634 
635 template <class GRAPH_T>
637 {
638 }
639 
640 template <class GRAPH_T>
642  std::ostream& out) const
643 {
644  MRPT_START;
645 
646  out << mrpt::format(
647  "------------------[ Goodness-based ICP Edge Registration "
648  "]------------------\n");
649  out << mrpt::format(
650  "ICP goodness threshold = %.2f%% \n",
651  ICP_goodness_thresh * 100);
652  out << mrpt::format(
653  "ICP max radius for edge search = %.2f\n", ICP_max_distance);
654  out << mrpt::format(
655  "Min. node difference for LC = %lu\n", LC_min_nodeid_diff);
656  out << mrpt::format(
657  "Visualize laser scans = %d\n", visualize_laser_scans);
658  out << mrpt::format(
659  "3DScans Image Directory = %s\n",
660  scans_img_external_dir.c_str());
661 
662  MRPT_END;
663 }
664 template <class GRAPH_T>
666  const mrpt::config::CConfigFileBase& source, const std::string& section)
667 {
668  MRPT_START;
669 
670  LC_min_nodeid_diff = source.read_int(
671  "GeneralConfiguration", "LC_min_nodeid_diff", 30, false);
672  ICP_max_distance =
673  source.read_double(section, "ICP_max_distance", 10, false);
674  ICP_goodness_thresh =
675  source.read_double(section, "ICP_goodness_thresh", 0.75, false);
676  visualize_laser_scans = source.read_bool(
677  "VisualizationParameters", "visualize_laser_scans", true, false);
678  scans_img_external_dir = source.read_string(
679  section, "scan_images_external_directory", "./", false);
680 
681  has_read_config = true;
682 
683  MRPT_END;
684 }
685 } // end of namespaces
686 
687 #endif /* end of include guard: CICPCRITERIAERD_IMPL_H */
688 
689 
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:197
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:31
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.
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:194
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:86
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
#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:102
#define MRPT_END
Definition: exceptions.h:266
A RGB color - floats in the range [0,1].
Definition: TColor.h:77
The ICP algorithm return information.
Definition: CICP.h:190
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:16
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: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020