Main MRPT website > C++ reference for MRPT 1.5.6
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.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.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.null() || !m_fake_laser_scan2D.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.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
mrpt::gui::CDisplayWindow3D * m_win
Window to use.
void printParams() const
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)
Get a list of the window events that happened since the last call.
void getDescriptiveReport(std::string *report_str) const
Fill the provided string with a detailed report of the decider/optimizer state.
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)
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
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.
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...
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...
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
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
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...
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
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.
GLhandleARB obj
Definition: glew.h:3276
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...
double read_double(const std::string &section, const std::string &name, double defaultValue, bool failIfNotFound=false) const
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
mrpt::utils::TColor m_laser_scans_color
see Constructor for initialization
virtual void printParams() const
Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact w...
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
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...
GLsizei const GLcharARB ** string
Definition: glew.h:3293
mrpt::utils::CTimeLogger m_time_logger
Time logger instance.
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:103
std::map< mrpt::utils::TNodeID, mrpt::obs::CObservation3DRangeScanPtr > m_nodes_to_laser_scans3D
The ICP algorithm return information.
Definition: CICP.h:128
A RGB color - floats in the range [0,1].
Definition: TColor.h:80
GLfloat * params
Definition: glew.h:1436
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...
std::string getStatsAsText(const size_t column_width=80) const
Dump all stats to a multi-line text string.
virtual void getDescriptiveReport(std::string *report_str) const
Fill the provided string with a detailed report of the decider/optimizer state.
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).
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
void updateVisuals()
Update the relevant visual features in CDisplayWindow.
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.
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)
bool convert3DTo2DRangeScan(mrpt::obs::CObservation3DRangeScanPtr &scan3D_in, mrpt::obs::CObservation2DRangeScanPtr *scan2D_out=NULL)
Wrapper around the CObservation3DRangeScan::convertTo2DScan corresponding method. ...
GLsizei GLsizei GLchar * source
Definition: glew.h:1739
double BASE_IMPEXP distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1511
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.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018