MRPT  1.9.9
CICPCriteriaERD_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-2019, 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 #pragma once
11 #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 
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 
42 // Methods implementations
43 // //////////////////////////////////
44 
45 template <class GRAPH_T>
48  mrpt::obs::CSensoryFrame::Ptr observations,
49  mrpt::obs::CObservation::Ptr observation)
50 {
51  MRPT_START;
52  MRPT_UNUSED_PARAM(action);
53  using namespace mrpt::obs;
54 
55  // check possible prior node registration
56  bool registered_new_node = false;
57 
58  if (this->m_last_total_num_nodes < this->m_graph->nodeCount())
59  {
60  registered_new_node = true;
61  this->m_last_total_num_nodes = this->m_graph->nodeCount();
62  MRPT_LOG_DEBUG("New node has been registered!");
63  }
64 
65  if (observation)
66  { // observation-only rawlog format
67  if (IS_CLASS(*observation, CObservation2DRangeScan))
68  {
69  m_last_laser_scan2D =
70  std::dynamic_pointer_cast<mrpt::obs::CObservation2DRangeScan>(
71  observation);
72  m_is_using_3DScan = false;
73  }
74  if (IS_CLASS(*observation, CObservation3DRangeScan))
75  {
76  m_last_laser_scan3D =
77  std::dynamic_pointer_cast<mrpt::obs::CObservation3DRangeScan>(
78  observation);
79  // just load the range/intensity images - CGraphSlanEngine takes
80  // care
81  // of the path
82  m_last_laser_scan3D->load();
83 
84  // grab fake 2D range scan for visualization
85  this->convert3DTo2DRangeScan(
86  /*from = */ m_last_laser_scan3D,
87  /*to = */ &m_fake_laser_scan2D);
88 
89  m_is_using_3DScan = true;
90  }
91 
92  // New node has been registered.
93  // add the last laser_scan
94  if (registered_new_node)
95  {
96  if (m_last_laser_scan2D)
97  {
98  this->m_nodes_to_laser_scans2D[this->m_graph->nodeCount() - 1] =
99  m_last_laser_scan2D;
101  "Added laser scans of nodeID: %u",
102  (unsigned)(this->m_graph->nodeCount() - 1)));
103  }
104  if (m_last_laser_scan3D)
105  {
106  m_nodes_to_laser_scans3D[this->m_graph->nodeCount() - 1] =
107  m_last_laser_scan3D;
109  "Added laser scans of nodeID: %u",
110  (unsigned)(this->m_graph->nodeCount() - 1)));
111  }
112  }
113  }
114  else
115  { // action-observations rawlog format
116  // append current laser scan
117  m_last_laser_scan2D =
118  observations->getObservationByClass<CObservation2DRangeScan>();
119  if (registered_new_node && m_last_laser_scan2D)
120  {
121  this->m_nodes_to_laser_scans2D[this->m_graph->nodeCount() - 1] =
122  m_last_laser_scan2D;
123  }
124  }
125 
126  // edge registration procedure - same for both rawlog formats
127  if (registered_new_node)
128  {
129  // get set of nodes within predefined distance for ICP
130  std::set<mrpt::graphs::TNodeID> nodes_to_check_ICP;
131  this->getNearbyNodesOf(
132  &nodes_to_check_ICP, this->m_graph->nodeCount() - 1,
133  params.ICP_max_distance);
135  "Found * %lu * nodes close to nodeID %lu",
136  nodes_to_check_ICP.size(), this->m_graph->nodeCount() - 1);
137 
138  // reset the loop_closure flag and run registration
139  this->m_just_inserted_lc = false;
140  registered_new_node = false;
141 
142  if (m_is_using_3DScan)
143  {
144  checkRegistrationCondition3D(nodes_to_check_ICP);
145  }
146  else
147  {
148  checkRegistrationCondition2D(nodes_to_check_ICP);
149  }
150  }
151 
152  return true;
153  MRPT_END;
154 }
155 
156 template <class GRAPH_T>
158  const std::set<mrpt::graphs::TNodeID>& nodes_set)
159 {
160  MRPT_START;
161  using namespace mrpt;
162  using namespace mrpt::obs;
163  using namespace mrpt::math;
164 
165  mrpt::graphs::TNodeID curr_nodeID = this->m_graph->nodeCount() - 1;
166  CObservation2DRangeScan::Ptr curr_laser_scan;
167  typename nodes_to_scans2D_t::const_iterator search;
168 
169  // search for curr_laser_scan
170  search = this->m_nodes_to_laser_scans2D.find(curr_nodeID);
171  if (search != this->m_nodes_to_laser_scans2D.end())
172  {
173  curr_laser_scan = search->second;
174  }
175 
176  // commence only if I have the current laser scan
177  if (curr_laser_scan)
178  {
179  // try adding ICP constraints with each node in the previous set
180  for (mrpt::graphs::TNodeID node_it : nodes_set)
181  {
182  // get the ICP edge between current and last node
183  constraint_t rel_edge;
185  CObservation2DRangeScan::Ptr prev_laser_scan;
186 
187  // search for prev_laser_scan
188  search = this->m_nodes_to_laser_scans2D.find(node_it);
189  if (search != this->m_nodes_to_laser_scans2D.end())
190  {
191  prev_laser_scan = search->second;
192 
193  // make use of initial node position difference for the ICP edge
194  pose_t initial_pose = this->m_graph->nodes[curr_nodeID] -
195  this->m_graph->nodes[node_it];
196 
197  this->m_time_logger.enter("CICPCriteriaERD::getICPEdge");
198  this->getICPEdge(
199  *prev_laser_scan, *curr_laser_scan, &rel_edge,
200  &initial_pose, &icp_info);
201  this->m_time_logger.leave("CICPCriteriaERD::getICPEdge");
202 
203  // Debugging statements
205  ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>"
206  ">>>>>>>>>");
208  "ICP constraint between NON-successive nodes: "
209  << node_it << " => " << curr_nodeID << std::endl
210  << "\tnIterations = " << icp_info.nIterations
211  << "\tgoodness = " << icp_info.goodness);
213  "ICP_goodness_thresh: " << params.ICP_goodness_thresh);
215  "<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<"
216  "<<<<<<<<<");
217 
218  // criterion for registering a new node
219  if (icp_info.goodness > params.ICP_goodness_thresh)
220  {
221  this->registerNewEdge(node_it, curr_nodeID, rel_edge);
222  m_edge_types_to_nums["ICP2D"]++;
223  // in case of loop closure
224  if (absDiff(curr_nodeID, node_it) >
225  params.LC_min_nodeid_diff)
226  {
227  m_edge_types_to_nums["LC"]++;
228  this->m_just_inserted_lc = true;
229  }
230  }
231  }
232  }
233  }
234 
235  MRPT_END;
236 }
237 template <class GRAPH_T>
239  const std::set<mrpt::graphs::TNodeID>& nodes_set)
240 {
241  MRPT_START;
242  using namespace std;
243  using namespace mrpt::obs;
244  using namespace mrpt::math;
245 
246  mrpt::graphs::TNodeID curr_nodeID = this->m_graph->nodeCount() - 1;
247  CObservation3DRangeScan::Ptr curr_laser_scan;
248  std::map<mrpt::graphs::TNodeID, mrpt::obs::CObservation3DRangeScan::Ptr>::
249  const_iterator search;
250  // search for curr_laser_scan
251  search = m_nodes_to_laser_scans3D.find(curr_nodeID);
252  if (search != m_nodes_to_laser_scans3D.end())
253  {
254  curr_laser_scan = search->second;
255  }
256 
257  // commence only if I have the current laser scan
258  if (curr_laser_scan)
259  {
260  // try adding ICP constraints with each node in the previous set
261  for (mrpt::graphs::TNodeID node_it : nodes_set)
262  {
263  // get the ICP edge between current and last node
264  constraint_t rel_edge;
266  CObservation3DRangeScan::Ptr prev_laser_scan;
267 
268  // search for prev_laser_scan
269  search = m_nodes_to_laser_scans3D.find(node_it);
270  if (search != m_nodes_to_laser_scans3D.end())
271  {
272  prev_laser_scan = search->second;
273 
274  // TODO - use initial edge estimation
275  this->m_time_logger.enter("CICPCriteriaERD::getICPEdge");
276  this->getICPEdge(
277  *prev_laser_scan, *curr_laser_scan, &rel_edge, nullptr,
278  &icp_info);
279  this->m_time_logger.leave("CICPCriteriaERD::getICPEdge");
280 
281  // criterion for registering a new node
282  if (icp_info.goodness > params.ICP_goodness_thresh)
283  {
284  this->registerNewEdge(node_it, curr_nodeID, rel_edge);
285  m_edge_types_to_nums["ICP3D"]++;
286  // in case of loop closure
287  if (absDiff(curr_nodeID, node_it) >
288  params.LC_min_nodeid_diff)
289  {
290  m_edge_types_to_nums["LC"]++;
291  this->m_just_inserted_lc = true;
292  }
293  }
294  }
295  }
296  }
297 
298  MRPT_END;
299 }
300 
301 template <class GRAPH_T>
303  const mrpt::graphs::TNodeID& from, const mrpt::graphs::TNodeID& to,
304  const constraint_t& rel_edge)
305 {
306  parent_t::registerNewEdge(from, to, rel_edge);
307 
308  this->m_graph->insertEdge(from, to, rel_edge);
309 }
310 
311 template <class GRAPH_T>
313  std::set<mrpt::graphs::TNodeID>* nodes_set,
314  const mrpt::graphs::TNodeID& cur_nodeID, double distance)
315 {
316  MRPT_START;
317 
318  if (distance > 0)
319  {
320  // check all but the last node.
321  for (mrpt::graphs::TNodeID nodeID = 0;
322  nodeID < this->m_graph->nodeCount() - 1; ++nodeID)
323  {
324  double curr_distance = this->m_graph->nodes[nodeID].distanceTo(
325  this->m_graph->nodes[cur_nodeID]);
326  if (curr_distance <= distance)
327  {
328  nodes_set->insert(nodeID);
329  }
330  }
331  }
332  else
333  { // check against all nodes
334  this->m_graph->getAllNodes(*nodes_set);
335  }
336 
337  MRPT_END;
338 }
339 
340 template <class GRAPH_T>
342  const std::map<std::string, bool>& events_occurred)
343 {
344  MRPT_START;
345  parent_t::notifyOfWindowEvents(events_occurred);
346 
347  // I know the key exists - I put it there explicitly
348  if (events_occurred.find(params.keystroke_laser_scans)->second)
349  {
350  this->toggleLaserScansVisualization();
351  }
352 
353  MRPT_END;
354 }
355 
356 template <class GRAPH_T>
358 {
359  MRPT_START;
360  ASSERTDEBMSG_(this->m_win, "No CDisplayWindow3D* was provided");
361  ASSERTDEBMSG_(this->m_win_manager, "No CWindowManager* was provided");
362 
363  using namespace mrpt::opengl;
364 
365  this->logFmt(
366  mrpt::system::LVL_INFO, "Toggling LaserScans visualization...");
367 
368  COpenGLScene::Ptr scene = this->m_win->get3DSceneAndLock();
369 
370  if (params.visualize_laser_scans)
371  {
372  CRenderizable::Ptr obj = scene->getByName("laser_scan_viz");
373  obj->setVisibility(!obj->isVisible());
374  }
375  else
376  {
377  dumpVisibilityErrorMsg("visualize_laser_scans");
378  }
379 
380  this->m_win->unlockAccess3DScene();
381  this->m_win->forceRepaint();
382 
383  MRPT_END;
384 }
385 
386 template <class GRAPH_T>
388  std::map<std::string, int>* edge_types_to_num) const
389 {
390  MRPT_START;
391 
392  *edge_types_to_num = m_edge_types_to_nums;
393 
394  MRPT_END;
395 }
396 
397 template <class GRAPH_T>
399 {
400  MRPT_START;
401  using namespace mrpt::opengl;
402  this->logFmt(mrpt::system::LVL_DEBUG, "Initializing visuals");
403  this->m_time_logger.enter("CICPCriteriaERD::Visuals");
404  parent_t::initializeVisuals();
405 
407  params.has_read_config, "Configuration parameters aren't loaded yet");
408 
409  this->m_win_observer->registerKeystroke(
410  params.keystroke_laser_scans, "Toggle LaserScans Visualization");
411 
412  // ICP_max_distance disk
413  if (params.ICP_max_distance > 0)
414  {
415  COpenGLScene::Ptr scene = this->m_win->get3DSceneAndLock();
416 
417  CDisk::Ptr obj = std::make_shared<CDisk>();
418  pose_t initial_pose;
419  obj->setPose(initial_pose);
420  obj->setName("ICP_max_distance");
421  obj->setColor_u8(m_search_disk_color);
422  obj->setDiskRadius(
423  params.ICP_max_distance, params.ICP_max_distance - 0.1);
424  scene->insert(obj);
425 
426  this->m_win->unlockAccess3DScene();
427  this->m_win->forceRepaint();
428  }
429 
430  // laser scan visualization
431  if (params.visualize_laser_scans)
432  {
433  COpenGLScene::Ptr scene = this->m_win->get3DSceneAndLock();
434 
435  CPlanarLaserScan::Ptr laser_scan_viz =
437  laser_scan_viz->enablePoints(true);
438  laser_scan_viz->enableLine(true);
439  laser_scan_viz->enableSurface(true);
440  laser_scan_viz->setSurfaceColor(
441  m_laser_scans_color.R, m_laser_scans_color.G, m_laser_scans_color.B,
442  m_laser_scans_color.A);
443 
444  laser_scan_viz->setName("laser_scan_viz");
445 
446  scene->insert(laser_scan_viz);
447  this->m_win->unlockAccess3DScene();
448  this->m_win->forceRepaint();
449  }
450 
451  // max distance disk - textMessage
452  if (this->m_win && this->m_win_manager && params.ICP_max_distance > 0)
453  {
454  this->m_win_manager->assignTextMessageParameters(
455  &m_offset_y_search_disk, &m_text_index_search_disk);
456 
457  this->m_win_manager->addTextMessage(
458  5, -m_offset_y_search_disk, "ICP Edges search radius",
459  mrpt::img::TColorf(m_search_disk_color),
460  /* unique_index = */ m_text_index_search_disk);
461  }
462 
463  this->m_time_logger.leave("CICPCriteriaERD::Visuals");
464  MRPT_END;
465 }
466 template <class GRAPH_T>
468 {
469  MRPT_START;
470  this->m_time_logger.enter("CICPCriteriaERD::Visuals");
471  using namespace mrpt::opengl;
472  using namespace mrpt::math;
473  using namespace mrpt::poses;
474  parent_t::updateVisuals();
475 
476  // update ICP_max_distance Disk
477  if (this->m_win && params.ICP_max_distance > 0)
478  {
479  COpenGLScene::Ptr scene = this->m_win->get3DSceneAndLock();
480 
481  CRenderizable::Ptr obj = scene->getByName("ICP_max_distance");
482  CDisk::Ptr disk_obj = std::dynamic_pointer_cast<CDisk>(obj);
483 
484  disk_obj->setPose(this->m_graph->nodes[this->m_graph->nodeCount() - 1]);
485 
486  this->m_win->unlockAccess3DScene();
487  this->m_win->forceRepaint();
488  }
489 
490  // update laser scan visual
491  if (this->m_win && params.visualize_laser_scans &&
492  (m_last_laser_scan2D || m_fake_laser_scan2D))
493  {
494  COpenGLScene::Ptr scene = this->m_win->get3DSceneAndLock();
495 
496  CRenderizable::Ptr obj = scene->getByName("laser_scan_viz");
497  CPlanarLaserScan::Ptr laser_scan_viz =
498  std::dynamic_pointer_cast<CPlanarLaserScan>(obj);
499 
500  // if fake 2D exists use it
501  if (m_fake_laser_scan2D)
502  {
503  laser_scan_viz->setScan(*m_fake_laser_scan2D);
504  }
505  else
506  {
507  laser_scan_viz->setScan(*m_last_laser_scan2D);
508  }
509 
510  // set the pose of the laser scan
511  auto search = this->m_graph->nodes.find(this->m_graph->nodeCount() - 1);
512  if (search != this->m_graph->nodes.end())
513  {
514  laser_scan_viz->setPose(
515  this->m_graph->nodes[this->m_graph->nodeCount() - 1]);
516  // put the laser scan *underneath* the graph, so that you can still
517  // visualize the loop closures with the nodes ahead
518  laser_scan_viz->setPose(CPose3D(
519  laser_scan_viz->getPoseX(), laser_scan_viz->getPoseY(), -0.15,
520  DEG2RAD(laser_scan_viz->getPoseYaw()),
521  DEG2RAD(laser_scan_viz->getPosePitch()),
522  DEG2RAD(laser_scan_viz->getPoseRoll())));
523  }
524 
525  this->m_win->unlockAccess3DScene();
526  this->m_win->forceRepaint();
527  }
528 
529  this->m_time_logger.leave("CICPCriteriaERD::Visuals");
530  MRPT_END;
531 }
532 
533 template <class GRAPH_T>
535  std::string viz_flag, int sleep_time /* = 500 milliseconds */)
536 {
537  MRPT_START;
538  using namespace mrpt;
539 
541  "Cannot toggle visibility of specified object.\n "
542  "Make sure that the corresponding visualization flag ( %s "
543  ") is set to true in the .ini file.\n",
544  viz_flag.c_str());
545  std::this_thread::sleep_for(std::chrono::milliseconds(sleep_time));
546 
547  MRPT_END;
548 }
549 
550 template <class GRAPH_T>
552 {
553  MRPT_START;
554  parent_t::loadParams(source_fname);
555 
556  params.loadFromConfigFileName(
557  source_fname, "EdgeRegistrationDeciderParameters");
558  MRPT_LOG_DEBUG("Successfully loaded parameters. ");
559 
560  // set the logging level if given by the user
561  mrpt::config::CConfigFile source(source_fname);
562  int min_verbosity_level = source.read_int(
563  "EdgeRegistrationDeciderParameters", "class_verbosity", 1, false);
564  this->setMinLoggingLevel(mrpt::system::VerbosityLevel(min_verbosity_level));
565 
566  MRPT_END;
567 }
568 template <class GRAPH_T>
570 {
571  MRPT_START;
572  parent_t::printParams();
573  params.dumpToConsole();
574 
575  MRPT_END;
576 }
577 
578 template <class GRAPH_T>
580  std::string* report_str) const
581 {
582  MRPT_START;
583  using namespace std;
584 
585  const std::string report_sep(2, '\n');
586  const std::string header_sep(80, '#');
587 
588  // Report on graph
589  stringstream class_props_ss;
590  class_props_ss << "ICP Goodness-based Registration Procedure Summary: "
591  << std::endl;
592  class_props_ss << header_sep << std::endl;
593 
594  // time and output logging
595  const std::string time_res = this->m_time_logger.getStatsAsText();
596  const std::string output_res = this->getLogAsString();
597 
598  // merge the individual reports
599  report_str->clear();
600  parent_t::getDescriptiveReport(report_str);
601 
602  *report_str += class_props_ss.str();
603  *report_str += report_sep;
604 
605  *report_str += time_res;
606  *report_str += report_sep;
607 
608  *report_str += output_res;
609  *report_str += report_sep;
610 
611  MRPT_END;
612 }
613 
614 // TParameter
615 // //////////////////////////////////
616 
617 template <class GRAPH_T>
619  : decider(d), keystroke_laser_scans("l"), has_read_config(false)
620 {
621 }
622 
623 template <class GRAPH_T>
625 
626 template <class GRAPH_T>
628  std::ostream& out) const
629 {
630  MRPT_START;
631 
632  out << "------------------[ Goodness-based ICP Edge Registration "
633  "]------------------\n";
634  out << mrpt::format(
635  "ICP goodness threshold = %.2f%% \n",
636  ICP_goodness_thresh * 100);
637  out << mrpt::format(
638  "ICP max radius for edge search = %.2f\n", ICP_max_distance);
639  out << mrpt::format(
640  "Min. node difference for LC = %lu\n", LC_min_nodeid_diff);
641  out << mrpt::format(
642  "Visualize laser scans = %d\n", visualize_laser_scans);
643  out << mrpt::format(
644  "3DScans Image Directory = %s\n",
645  scans_img_external_dir.c_str());
646 
647  MRPT_END;
648 }
649 template <class GRAPH_T>
651  const mrpt::config::CConfigFileBase& source, const std::string& section)
652 {
653  MRPT_START;
654 
655  LC_min_nodeid_diff = source.read_int(
656  "GeneralConfiguration", "LC_min_nodeid_diff", 30, false);
657  ICP_max_distance =
658  source.read_double(section, "ICP_max_distance", 10, false);
659  ICP_goodness_thresh =
660  source.read_double(section, "ICP_goodness_thresh", 0.75, false);
661  visualize_laser_scans = source.read_bool(
662  "VisualizationParameters", "visualize_laser_scans", true, false);
663  scans_img_external_dir = source.read_string(
664  section, "scan_images_external_directory", "./", false);
665 
666  has_read_config = true;
667 
668  MRPT_END;
669 }
670 } // namespace mrpt::graphslam::deciders
void checkRegistrationCondition2D(const std::set< mrpt::graphs::TNodeID > &nodes_set)
#define MRPT_START
Definition: exceptions.h:241
#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 override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
app setMinLoggingLevel(mrpt::system::LVL_ERROR)
static Ptr Create(Args &&... args)
void dumpVisibilityErrorMsg(std::string viz_flag, int sleep_time=500)
void loadParams(const std::string &source_fname) override
Load the necessary for the decider/optimizer configuration parameters.
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.
void updateVisuals() override
Update the relevant visual features in CDisplayWindow.
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...
void printParams() const override
Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact w...
GLsizei GLsizei GLuint * obj
Definition: glext.h:4085
void notifyOfWindowEvents(const std::map< std::string, bool > &events_occurred) override
Get a list of the window events that happened since the last call.
void toggleLaserScansVisualization()
togle the LaserScans visualization on and off
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
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.
#define MRPT_LOG_DEBUG_FMT(_FMT_STRING,...)
Use: MRPT_LOG_DEBUG_FMT("i=%u", i);
constexpr double DEG2RAD(const double x)
Degrees to radians.
A planar disk in the XY plane.
Definition: CDisk.h:30
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...
#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
void getEdgesStats(std::map< std::string, int > *edge_types_to_num) const override
Fill the given map with the type of registered edges as well as the corresponding number of registrat...
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:4116
void getDescriptiveReport(std::string *report_str) const override
Fill the provided string with a detailed report of the decider/optimizer state.
bool updateState(mrpt::obs::CActionCollection::Ptr action, mrpt::obs::CSensoryFrame::Ptr observations, mrpt::obs::CObservation::Ptr observation) override
Generic method for fetching the incremental action-observations (or observation-only) measurements...
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:191
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...
void registerNewEdge(const mrpt::graphs::TNodeID &from, const mrpt::graphs::TNodeID &to, const constraint_t &rel_edge) override
Register a new constraint/edge in the current graph.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
void initializeVisuals() override
Initialize visual objects in CDisplayWindow (e.g.
#define MRPT_END
Definition: exceptions.h:245
A RGB color - floats in the range [0,1].
Definition: TColor.h:78
The ICP algorithm return information.
Definition: CICP.h:190
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
GLsizei GLsizei GLchar * source
Definition: glext.h:4097
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.
GLenum const GLfloat * params
Definition: glext.h:3538
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1891
void load() const override
Makes sure all images and other fields which may be externally stored are loaded in memory...
#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.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 9b18308f3 Mon Nov 18 23:39:25 2019 +0100 at lun nov 18 23:45:12 CET 2019