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



Page generated by Doxygen 1.8.14 for MRPT 2.0.5 Git: e84511500 Thu Jul 2 23:53:34 2020 +0200 at vie jul 3 00:00:10 CEST 2020