Main MRPT website > C++ reference for MRPT 1.5.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-2017, 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 
13 namespace mrpt { namespace graphslam { namespace deciders {
14 
15 // Ctors, Dtors
16 // //////////////////////////////////
17 
18 template<class GRAPH_T>
20  params(*this), // pass reference to self when initializing the parameters
21  m_search_disk_color(142, 142, 56),
22  m_laser_scans_color(0, 20, 255),
23  m_is_using_3DScan(false)
24 {
25  MRPT_START;
26  using namespace mrpt::utils;
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  this->logFmt(LVL_DEBUG, "Initialized class object");
38 
39  MRPT_END;
40 }
41 template<class GRAPH_T>
43 
44 // Methods implementations
45 // //////////////////////////////////
46 
47 template<class GRAPH_T> bool CICPCriteriaERD<GRAPH_T>::updateState(
48  mrpt::obs::CActionCollectionPtr action,
49  mrpt::obs::CSensoryFramePtr observations,
50  mrpt::obs::CObservationPtr observation ) {
51  MRPT_START;
52  MRPT_UNUSED_PARAM(action);
53  using namespace mrpt::obs;
54  using namespace mrpt::utils;
55 
56  // check possible prior node registration
57  bool registered_new_node = false;
58 
59  if (this->m_last_total_num_nodes < this->m_graph->nodeCount()) {
60  registered_new_node = true;
61  this->m_last_total_num_nodes = this->m_graph->nodeCount();
62  this->logFmt(LVL_DEBUG, "New node has been registered!");
63  }
64 
65  if (observation.present()) { // observation-only rawlog format
66  if (IS_CLASS(observation, CObservation2DRangeScan)) {
68  static_cast<mrpt::obs::CObservation2DRangeScanPtr>(observation);
69  m_is_using_3DScan = false;
70  }
71  if (IS_CLASS(observation, CObservation3DRangeScan)) {
73  static_cast<mrpt::obs::CObservation3DRangeScanPtr>(observation);
74  // just load the range/intensity images - CGraphSlanEngine takes care
75  // of the path
76  m_last_laser_scan3D->load();
77 
78  // grab fake 2D range scan for visualization
80  /*from = */ m_last_laser_scan3D,
81  /*to = */ &m_fake_laser_scan2D);
82 
83  m_is_using_3DScan = true;
84  }
85 
86  // New node has been registered.
87  // add the last laser_scan
88  if (registered_new_node) {
89  if (m_last_laser_scan2D.get()!=NULL) {
90  this->m_nodes_to_laser_scans2D[this->m_graph->nodeCount()-1] =
92  this->logFmt(LVL_DEBUG,
93  "Added laser scans of nodeID: %lu",
94  this->m_graph->nodeCount()-1);
95  }
96  if (m_last_laser_scan3D.get()!=NULL) {
98  this->m_graph->nodeCount()-1] = m_last_laser_scan3D;
99  this->logFmt(LVL_DEBUG,
100  "Added laser scans of nodeID: %lu",
101  this->m_graph->nodeCount()-1);
102  }
103  }
104  }
105  else { // action-observations rawlog format
106  // append current laser scan
108  observations->getObservationByClass<CObservation2DRangeScan>();
109  if (registered_new_node && m_last_laser_scan2D) {
111  this->m_graph->nodeCount()-1] = m_last_laser_scan2D;
112  }
113  }
114 
115  // edge registration procedure - same for both rawlog formats
116  if (registered_new_node) {
117  // get set of nodes within predefined distance for ICP
118  std::set<mrpt::utils::TNodeID> nodes_to_check_ICP;
119  this->getNearbyNodesOf(
120  &nodes_to_check_ICP,
121  this->m_graph->nodeCount()-1,
122  params.ICP_max_distance);
123  this->logFmt(LVL_DEBUG,
124  "Found * %lu * nodes close to nodeID %lu",
125  nodes_to_check_ICP.size(),
126  this->m_graph->nodeCount()-1);
127 
128  // reset the loop_closure flag and run registration
129  this->m_just_inserted_lc = false;
130  registered_new_node = false;
131 
132  if (m_is_using_3DScan) {
133  checkRegistrationCondition3D(nodes_to_check_ICP);
134  }
135  else {
136  checkRegistrationCondition2D(nodes_to_check_ICP);
137  }
138  }
139 
140  return true;
141  MRPT_END;
142 }
143 
144 template<class GRAPH_T>
146  const std::set<mrpt::utils::TNodeID>& nodes_set) {
147  MRPT_START;
148  using namespace mrpt;
149  using namespace mrpt::obs;
150  using namespace mrpt::math;
151 
152  mrpt::utils::TNodeID curr_nodeID = this->m_graph->nodeCount()-1;
153  CObservation2DRangeScanPtr curr_laser_scan;
154  typename nodes_to_scans2D_t::const_iterator search;
155 
156  // search for curr_laser_scan
157  search = this->m_nodes_to_laser_scans2D.find(curr_nodeID);
158  if (search != this->m_nodes_to_laser_scans2D.end()) {
159  curr_laser_scan = search->second;
160  }
161 
162  // commence only if I have the current laser scan
163  if (curr_laser_scan) {
164  // try adding ICP constraints with each node in the previous set
166  node_it = nodes_set.begin();
167  node_it != nodes_set.end(); ++node_it) {
168 
169  // get the ICP edge between current and last node
170  constraint_t rel_edge;
172  CObservation2DRangeScanPtr prev_laser_scan;
173 
174  // search for prev_laser_scan
175  search = this->m_nodes_to_laser_scans2D.find(*node_it);
176  if (search != this->m_nodes_to_laser_scans2D.end()) {
177  prev_laser_scan = search->second;
178 
179  // make use of initial node position difference for the ICP edge
180  pose_t initial_pose = this->m_graph->nodes[curr_nodeID] -
181  this->m_graph->nodes[*node_it];
182 
183  this->m_time_logger.enter("CICPCriteriaERD::getICPEdge");
184  this->getICPEdge(
185  *prev_laser_scan,
186  *curr_laser_scan,
187  &rel_edge,
188  &initial_pose,
189  &icp_info);
190  this->m_time_logger.leave("CICPCriteriaERD::getICPEdge");
191 
192  // Debugging statements
193  MRPT_LOG_DEBUG_STREAM(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>");
195  "ICP constraint between NON-successive nodes: " <<
196  *node_it << " => " << curr_nodeID <<
197  std::endl <<
198  "\tnIterations = " << icp_info.nIterations <<
199  "\tgoodness = " << icp_info.goodness);
200  MRPT_LOG_DEBUG_STREAM("ICP_goodness_thresh: " <<params.ICP_goodness_thresh);
201  MRPT_LOG_DEBUG_STREAM("<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<");
202 
203 
204  // criterion for registering a new node
205  if (icp_info.goodness > params.ICP_goodness_thresh) {
206  this->registerNewEdge(*node_it, curr_nodeID, rel_edge);
207  m_edge_types_to_nums["ICP2D"]++;
208  // in case of loop closure
209  if (absDiff(curr_nodeID, *node_it) >
210  params.LC_min_nodeid_diff) {
211  m_edge_types_to_nums["LC"]++;
212  this->m_just_inserted_lc = true;
213  }
214  }
215  }
216  }
217  }
218 
219  MRPT_END;
220 }
221 template<class GRAPH_T>
223  const std::set<mrpt::utils::TNodeID>& nodes_set) {
224  MRPT_START;
225  using namespace std;
226  using namespace mrpt::obs;
227  using namespace mrpt::math;
228 
229  mrpt::utils::TNodeID curr_nodeID = this->m_graph->nodeCount()-1;
230  CObservation3DRangeScanPtr curr_laser_scan;
231  std::map<mrpt::utils::TNodeID,
232  mrpt::obs::CObservation3DRangeScanPtr>::const_iterator search;
233  // search for curr_laser_scan
234  search = m_nodes_to_laser_scans3D.find(curr_nodeID);
235  if (search != m_nodes_to_laser_scans3D.end()) {
236  curr_laser_scan = search->second;
237  }
238 
239  // commence only if I have the current laser scan
240  if (curr_laser_scan) {
241  // try adding ICP constraints with each node in the previous set
243  node_it = nodes_set.begin();
244  node_it != nodes_set.end(); ++node_it) {
245 
246  // get the ICP edge between current and last node
247  constraint_t rel_edge;
249  CObservation3DRangeScanPtr prev_laser_scan;
250 
251  // search for prev_laser_scan
252  search = m_nodes_to_laser_scans3D.find(*node_it);
253  if (search != m_nodes_to_laser_scans3D.end()) {
254  prev_laser_scan = search->second;
255 
256  // TODO - use initial edge estimation
257  this->m_time_logger.enter("CICPCriteriaERD::getICPEdge");
258  this->getICPEdge(
259  *prev_laser_scan,
260  *curr_laser_scan,
261  &rel_edge,
262  NULL,
263  &icp_info);
264  this->m_time_logger.leave("CICPCriteriaERD::getICPEdge");
265 
266  // criterion for registering a new node
267  if (icp_info.goodness > params.ICP_goodness_thresh) {
268  this->registerNewEdge(*node_it, curr_nodeID, rel_edge);
269  m_edge_types_to_nums["ICP3D"]++;
270  // in case of loop closure
271  if (absDiff(curr_nodeID, *node_it) > params.LC_min_nodeid_diff) {
272  m_edge_types_to_nums["LC"]++;
273  this->m_just_inserted_lc = true;
274  }
275  }
276  }
277  }
278  }
279 
280  MRPT_END;
281 }
282 
283 template<class GRAPH_T>
285  const mrpt::utils::TNodeID& from,
286  const mrpt::utils::TNodeID& to,
287  const constraint_t& rel_edge ) {
288  using namespace mrpt::utils;
289  parent_t::registerNewEdge(from, to, rel_edge);
290 
291  this->m_graph->insertEdge(from, to, rel_edge);
292 
293 }
294 
295 template<class GRAPH_T>
297  std::set<mrpt::utils::TNodeID> *nodes_set,
298  const mrpt::utils::TNodeID& cur_nodeID,
299  double distance ) {
300  MRPT_START;
301  using namespace mrpt::utils;
302 
303  if (distance > 0) {
304  // check all but the last node.
305  for (TNodeID nodeID = 0;
306  nodeID < this->m_graph->nodeCount()-1;
307  ++nodeID) {
308  double curr_distance = this->m_graph->nodes[nodeID].distanceTo(
309  this->m_graph->nodes[cur_nodeID]);
310  if (curr_distance <= distance) {
311  nodes_set->insert(nodeID);
312  }
313  }
314  }
315  else { // check against all nodes
316  this->m_graph->getAllNodes(*nodes_set);
317  }
318 
319  MRPT_END;
320 }
321 
322 template<class GRAPH_T>
324  const std::map<std::string, bool>& events_occurred) {
325  MRPT_START;
326  parent_t::notifyOfWindowEvents(events_occurred);
327 
328  // I know the key exists - I put it there explicitly
329  if (events_occurred.find(params.keystroke_laser_scans)->second) {
331  }
332 
333  MRPT_END;
334 }
335 
336 template<class GRAPH_T>
338  MRPT_START;
339  ASSERTMSG_(this->m_win, "No CDisplayWindow3D* was provided");
340  ASSERTMSG_(this->m_win_manager, "No CWindowManager* was provided");
341 
342  using namespace mrpt::opengl;
343 
344  this->logFmt(mrpt::utils::LVL_INFO, "Toggling LaserScans visualization...");
345 
346  COpenGLScenePtr scene = this->m_win->get3DSceneAndLock();
347 
348  if (params.visualize_laser_scans) {
349  CRenderizablePtr obj = scene->getByName("laser_scan_viz");
350  obj->setVisibility(!obj->isVisible());
351  }
352  else {
353  dumpVisibilityErrorMsg("visualize_laser_scans");
354  }
355 
356  this->m_win->unlockAccess3DScene();
357  this->m_win->forceRepaint();
358 
359  MRPT_END;
360 }
361 
362 
363 template<class GRAPH_T>
365  std::map<std::string, int>* edge_types_to_num) const {
366  MRPT_START;
367 
368  *edge_types_to_num = m_edge_types_to_nums;
369 
370  MRPT_END;
371 }
372 
373 template<class GRAPH_T>
375  MRPT_START;
376  using namespace mrpt::opengl;
377  this->logFmt(mrpt::utils::LVL_DEBUG, "Initializing visuals");
378  this->m_time_logger.enter("CICPCriteriaERD::Visuals");
380 
381  ASSERTMSG_(params.has_read_config,
382  "Configuration parameters aren't loaded yet");
383 
384  this->m_win_observer->registerKeystroke(params.keystroke_laser_scans,
385  "Toggle LaserScans Visualization");
386 
387  // ICP_max_distance disk
388  if (params.ICP_max_distance > 0) {
389  COpenGLScenePtr scene = this->m_win->get3DSceneAndLock();
390 
391  CDiskPtr obj = CDisk::Create();
392  pose_t initial_pose;
393  obj->setPose(initial_pose);
394  obj->setName("ICP_max_distance");
395  obj->setColor_u8(m_search_disk_color);
396  obj->setDiskRadius(params.ICP_max_distance, params.ICP_max_distance-0.1);
397  scene->insert(obj);
398 
399  this->m_win->unlockAccess3DScene();
400  this->m_win->forceRepaint();
401  }
402 
403  // laser scan visualization
404  if (params.visualize_laser_scans) {
405  COpenGLScenePtr scene = this->m_win->get3DSceneAndLock();
406 
407  CPlanarLaserScanPtr laser_scan_viz = mrpt::opengl::CPlanarLaserScan::Create();
408  laser_scan_viz->enablePoints(true);
409  laser_scan_viz->enableLine(true);
410  laser_scan_viz->enableSurface(true);
411  laser_scan_viz->setSurfaceColor(
416 
417  laser_scan_viz->setName("laser_scan_viz");
418 
419  scene->insert(laser_scan_viz);
420  this->m_win->unlockAccess3DScene();
421  this->m_win->forceRepaint();
422  }
423 
424  // max distance disk - textMessage
425  if (this->m_win && this->m_win_manager && params.ICP_max_distance > 0) {
429 
431  mrpt::format("ICP Edges search radius"),
433  /* unique_index = */ m_text_index_search_disk );
434  }
435 
436  this->m_time_logger.leave("CICPCriteriaERD::Visuals");
437  MRPT_END;
438 }
439 template<class GRAPH_T>
441  MRPT_START;
442  this->m_time_logger.enter("CICPCriteriaERD::Visuals");
443  using namespace mrpt::opengl;
444  using namespace mrpt::utils;
445  using namespace mrpt::math;
446  using namespace mrpt::poses;
448 
449  // update ICP_max_distance Disk
450  if (this->m_win && params.ICP_max_distance > 0) {
451  COpenGLScenePtr scene = this->m_win->get3DSceneAndLock();
452 
453  CRenderizablePtr obj = scene->getByName("ICP_max_distance");
454  CDiskPtr disk_obj = static_cast<CDiskPtr>(obj);
455 
456  disk_obj->setPose(
457  this->m_graph->nodes[this->m_graph->nodeCount()-1]);
458 
459  this->m_win->unlockAccess3DScene();
460  this->m_win->forceRepaint();
461  }
462 
463  // update laser scan visual
464  if (this->m_win && params.visualize_laser_scans &&
465  (m_last_laser_scan2D.get()!=NULL || m_fake_laser_scan2D.get()!=NULL)) {
466  COpenGLScenePtr scene = this->m_win->get3DSceneAndLock();
467 
468  CRenderizablePtr obj = scene->getByName("laser_scan_viz");
469  CPlanarLaserScanPtr laser_scan_viz = static_cast<CPlanarLaserScanPtr>(obj);
470 
471  // if fake 2D exists use it
472  if (m_fake_laser_scan2D.get()!=NULL) {
473  laser_scan_viz->setScan(*m_fake_laser_scan2D);
474  }
475  else {
476  laser_scan_viz->setScan(*m_last_laser_scan2D);
477  }
478 
479  // set the pose of the laser scan
481  this->m_graph->nodes.find(this->m_graph->nodeCount()-1);
482  if (search != this->m_graph->nodes.end()) {
483  laser_scan_viz->setPose(
484  this->m_graph->nodes[this->m_graph->nodeCount()-1]);
485  // put the laser scan *underneath* the graph, so that you can still
486  // visualize the loop closures with the nodes ahead
487  laser_scan_viz->setPose(CPose3D(
488  laser_scan_viz->getPoseX(), laser_scan_viz->getPoseY(), -0.15,
489  DEG2RAD(laser_scan_viz->getPoseYaw()),
490  DEG2RAD(laser_scan_viz->getPosePitch()),
491  DEG2RAD(laser_scan_viz->getPoseRoll())
492  ));
493  }
494 
495  this->m_win->unlockAccess3DScene();
496  this->m_win->forceRepaint();
497  }
498 
499  this->m_time_logger.leave("CICPCriteriaERD::Visuals");
500  MRPT_END;
501 }
502 
503 template<class GRAPH_T>
505  std::string viz_flag, int sleep_time /* = 500 milliseconds */) {
506  MRPT_START;
507  using namespace mrpt::utils;
508  using namespace mrpt;
509 
510  this->logFmt(LVL_ERROR,
511  "Cannot toggle visibility of specified object.\n "
512  "Make sure that the corresponding visualization flag ( %s "
513  ") is set to true in the .ini file.\n",
514  viz_flag.c_str());
515  mrpt::system::sleep(sleep_time);
516 
517  MRPT_END;
518 }
519 
520 
521 template<class GRAPH_T>
523  MRPT_START;
524  using namespace mrpt::utils;
525  parent_t::loadParams(source_fname);
526 
527  params.loadFromConfigFileName(source_fname,
528  "EdgeRegistrationDeciderParameters");
529  this->logFmt(LVL_DEBUG, "Successfully loaded parameters. ");
530 
531  // set the logging level if given by the user
532  CConfigFile source(source_fname);
533  int min_verbosity_level = source.read_int(
534  "EdgeRegistrationDeciderParameters",
535  "class_verbosity",
536  1, false);
537  this->setMinLoggingLevel(VerbosityLevel(min_verbosity_level));
538 
539  MRPT_END;
540 }
541 template<class GRAPH_T>
543  MRPT_START;
545  params.dumpToConsole();
546 
547  MRPT_END;
548 }
549 
550 template<class GRAPH_T>
552  MRPT_START;
553  using namespace std;
554 
555  const std::string report_sep(2, '\n');
556  const std::string header_sep(80, '#');
557 
558  // Report on graph
559  stringstream class_props_ss;
560  class_props_ss << "ICP Goodness-based Registration Procedure Summary: " << std::endl;
561  class_props_ss << header_sep << std::endl;
562 
563  // time and output logging
564  const std::string time_res = this->m_time_logger.getStatsAsText();
565  const std::string output_res = this->getLogAsString();
566 
567  // merge the individual reports
568  report_str->clear();
569  parent_t::getDescriptiveReport(report_str);
570 
571  *report_str += class_props_ss.str();
572  *report_str += report_sep;
573 
574  *report_str += time_res;
575  *report_str += report_sep;
576 
577  *report_str += output_res;
578  *report_str += report_sep;
579 
580  MRPT_END;
581 }
582 
583 
584 
585 // TParameter
586 // //////////////////////////////////
587 
588 template<class GRAPH_T>
590  decider(d),
591  keystroke_laser_scans("l"),
592  has_read_config(false)
593 { }
594 
595 template<class GRAPH_T>
597 }
598 
599 template<class GRAPH_T>
601  mrpt::utils::CStream &out) const {
602  MRPT_START;
603 
604  out.printf("------------------[ Goodness-based ICP Edge Registration ]------------------\n");
605  out.printf("ICP goodness threshold = %.2f%% \n", ICP_goodness_thresh*100);
606  out.printf("ICP max radius for edge search = %.2f\n", ICP_max_distance);
607  out.printf("Min. node difference for LC = %lu\n", LC_min_nodeid_diff);
608  out.printf("Visualize laser scans = %d\n", visualize_laser_scans);
609  out.printf("3DScans Image Directory = %s\n", scans_img_external_dir.c_str());
610 
611  MRPT_END;
612 }
613 template<class GRAPH_T>
616  const std::string& section) {
617  MRPT_START;
618 
619  LC_min_nodeid_diff = source.read_int(
620  "GeneralConfiguration",
621  "LC_min_nodeid_diff",
622  30, false);
623  ICP_max_distance = source.read_double(
624  section,
625  "ICP_max_distance",
626  10, false);
627  ICP_goodness_thresh = source.read_double(
628  section,
629  "ICP_goodness_thresh",
630  0.75, false);
631  visualize_laser_scans = source.read_bool(
632  "VisualizationParameters",
633  "visualize_laser_scans",
634  true, false);
635  scans_img_external_dir = source.read_string(
636  section,
637  "scan_images_external_directory",
638  "./", false);
639 
640  has_read_config = true;
641 
642  MRPT_END;
643 }
644 
645 } } } // end of namespaces
646 
647 #endif /* end of include guard: CICPCRITERIAERD_IMPL_H */
virtual void notifyOfWindowEvents(const std::map< std::string, bool > &events_occurred)
Get a list of the window events that happened since the last call.
virtual void loadParams(const std::string &source_fname)
Fetch the latest observation that the current instance received (most probably during a call to the u...
void unlockAccess3DScene()
Unlocks the access to the internal 3D scene.
std::map< mrpt::utils::TNodeID, mrpt::obs::CObservation2DRangeScanPtr > m_nodes_to_laser_scans2D
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
mrpt::gui::CDisplayWindow3D * m_win
Window to use.
void notifyOfWindowEvents(const std::map< std::string, bool > &events_occurred)
Get a list of the window events that happened since the last call.
virtual void printParams() const
Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact w...
bool updateState(mrpt::obs::CActionCollectionPtr action, mrpt::obs::CSensoryFramePtr observations, mrpt::obs::CObservationPtr observation)
Generic method for fetching the incremental action/observation readings from the calling function...
void dumpVisibilityErrorMsg(std::string viz_flag, int sleep_time=500)
This class allows loading and storing values and vectors of different types from ".ini" files easily.
Definition: CConfigFile.h:30
void checkRegistrationCondition2D(const std::set< mrpt::utils::TNodeID > &nodes_set)
static const std::string header_sep
Separator string to be used in debugging messages.
GRAPH_T::constraint_t constraint_t
Handy typedefs.
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.
mrpt::graphslam::CWindowObserver * m_win_observer
CWindowObserver object for monitoring various visual-oriented events.
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...
const Scalar * const_iterator
Definition: eigen_plugins.h:24
mrpt::utils::TColor m_search_disk_color
see Constructor for initialization
void registerKeystroke(const std::string key_str, const std::string key_desc)
Make new keystrokes available in the help message box.
void addTextMessage(const double x, const double y, const std::string &text, const mrpt::utils::TColorf &color=mrpt::utils::TColorf(1.0, 1.0, 1.0), const size_t unique_index=0)
Wrapper around the CDisplayWindow3D::addTextMessage method, so that the user does not have to specify...
GLsizei GLsizei GLuint * obj
Definition: glext.h:3902
void toggleLaserScansVisualization()
togle the LaserScans visualization on and off
This class allows loading and storing values and vectors of different types from a configuration text...
mrpt::graphslam::CWindowManager * m_win_manager
Pointer to the CWindowManager object used to store visuals-related instances.
float goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences...
Definition: CICP.h:137
mrpt::opengl::COpenGLScenePtr & get3DSceneAndLock()
Gets a reference to the smart shared pointer that holds the internal scene (carefuly read introductio...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
uint64_t TNodeID
The type for node IDs in graphs of different types.
virtual void initializeVisuals()
Initialize visual objects in CDisplayWindow (e.g.
#define MRPT_END
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
void BASE_IMPEXP sleep(int time_ms) MRPT_NO_THROWS
An OS-independent method for sending the current thread to "sleep" for a given period of time...
Definition: threads.cpp:57
mrpt::obs::CObservation2DRangeScanPtr m_last_laser_scan2D
mrpt::obs::CObservation2DRangeScanPtr m_fake_laser_scan2D
std::string getStatsAsText(const size_t column_width=80) const
Dump all stats to a multi-line text string.
void getDescriptiveReport(std::string *report_str) const
Fill the provided string with a detailed report of the decider/optimizer state.
virtual void registerNewEdge(const mrpt::utils::TNodeID &from, const mrpt::utils::TNodeID &to, const constraint_t &rel_edge)
Register a new constraint/edge in the current graph.
This namespace contains representation of robot actions and observations.
uint64_t TNodeID
The type for node IDs in graphs of different types.
Definition: types_simple.h:45
virtual void initializeLoggers(const std::string &name)
Initialize the COutputLogger, CTimeLogger instances given the name of the decider/optimizer at hand...
GRAPH_T * m_graph
Pointer to the graph that is under construction.
#define DEG2RAD
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
unsigned short nIterations
The number of executed iterations until convergence.
Definition: CICP.h:136
GLsizei const GLchar ** string
Definition: glext.h:3919
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
void dumpToTextStream(mrpt::utils::CStream &out) const
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
mrpt::utils::TColor m_laser_scans_color
see Constructor for initialization
void getICPEdge(const mrpt::obs::CObservation2DRangeScan &from, const mrpt::obs::CObservation2DRangeScan &to, constraint_t *rel_edge, const mrpt::poses::CPose2D *initial_pose=NULL, mrpt::slam::CICP::TReturnInfo *icp_info=NULL)
Align the 2D range scans provided and fill the potential edge that can transform the one into the oth...
std::map< std::string, int > m_edge_types_to_nums
void forceRepaint()
Repaints the window. forceRepaint, repaint and updateWindow are all aliases of the same method...
#define MRPT_START
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void registerNewEdge(const mrpt::utils::TNodeID &from, const mrpt::utils::TNodeID &to, const constraint_t &rel_edge)
Register a new constraint/edge in the current graph.
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section)
This method load the options from a ".ini"-like file or memory-stored string list.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
mrpt::utils::CTimeLogger m_time_logger
Time logger instance.
virtual void getDescriptiveReport(std::string *report_str) const
Fill the provided string with a detailed report of the decider/optimizer state.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
static CPlanarLaserScanPtr Create()
virtual void updateVisuals()
Update the relevant visual features in CDisplayWindow.
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::utils::CSerializable) is of t...
Definition: CObject.h:93
std::map< mrpt::utils::TNodeID, mrpt::obs::CObservation3DRangeScanPtr > m_nodes_to_laser_scans3D
The ICP algorithm return information.
Definition: CICP.h:128
The namespace for 3D scene representation and rendering.
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:3908
A RGB color - floats in the range [0,1].
Definition: TColor.h:80
double leave(const char *func_name)
End of a named section.
Definition: CTimeLogger.h:102
void getNearbyNodesOf(std::set< mrpt::utils::TNodeID > *nodes_set, const mrpt::utils::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.
GRAPH_T::constraint_t::type_value pose_t
type of underlying poses (2D/3D).
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...
void enter(const char *func_name)
Start of a named section.
Definition: CTimeLogger.h:97
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
#define ASSERTMSG_(f, __ERROR_MSG)
GLenum const GLfloat * params
Definition: glext.h:3514
bool convert3DTo2DRangeScan(mrpt::obs::CObservation3DRangeScanPtr &scan3D_in, mrpt::obs::CObservation2DRangeScanPtr *scan2D_out=NULL)
Wrapper around the CObservation3DRangeScan::convertTo2DScan corresponding method. ...
double BASE_IMPEXP distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1504
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
Definition: CStream.cpp:507
void loadParams(const std::string &source_fname)
Fetch the latest observation that the current instance received (most probably during a call to the u...
void assignTextMessageParameters(double *offset_y, int *text_index)
Assign the next available offset_y and text_index for the textMessage under construction.
mrpt::obs::CObservation3DRangeScanPtr m_last_laser_scan3D
void initializeVisuals()
Initialize visual objects in CDisplayWindow (e.g.
void checkRegistrationCondition3D(const std::set< mrpt::utils::TNodeID > &nodes_set)



Page generated by Doxygen 1.8.14 for MRPT 1.5.9 Git: 690a4699f Wed Apr 15 19:29:53 2020 +0200 at miƩ abr 15 19:30:12 CEST 2020