MRPT  2.0.3
CLevMarqGSO_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 
13 {
14 template <class GRAPH_T>
16  : m_optimization_policy(OptimizationPolicy::UseLoopClosures)
17 
18 {
19  this->initializeLoggers("CLevMarqGSO");
20 }
21 
22 template <class GRAPH_T>
25  mrpt::obs::CSensoryFrame::Ptr observations,
26  mrpt::obs::CObservation::Ptr observation)
27 {
29  if (this->m_graph->nodeCount() > m_last_total_num_of_nodes)
30  {
31  m_last_total_num_of_nodes = this->m_graph->nodeCount();
32  registered_new_node = true;
33 
34  if (m_first_time_call)
35  {
36  opt_params.last_pair_nodes_to_edge = this->m_graph->edges;
37  m_first_time_call = true;
38  }
39 
40  if (opt_params.optimization_on_second_thread)
41  {
42  // join the previous optimization thread
43  m_thread_optimize.join();
44 
45  // optimize the graph - run on a seperate thread
46  m_thread_optimize = std::thread(&CLevMarqGSO::optimizeGraph, this);
47  }
48  else
49  { // single threaded implementation
50  bool is_full_update = this->checkForFullOptimization();
51  this->_optimizeGraph(is_full_update);
52  }
53  }
54 
55  return true;
56  MRPT_END
57 }
58 
59 template <class GRAPH_T>
61 {
63  ASSERTDEB_(m_has_read_config);
64  parent::initializeVisuals();
65 
66  this->initGraphVisualization();
67  this->initOptDistanceVisualization();
68 
69  MRPT_END
70 }
71 
72 template <class GRAPH_T>
74 {
76  parent::updateVisuals();
77 
78  if (opt_params.optimization_distance > 0)
79  {
80  this->updateOptDistanceVisualization();
81  }
82 
83  this->updateGraphVisualization();
84 
85  MRPT_END
86 }
87 
88 template <class GRAPH_T>
90  const std::map<std::string, bool>& events_occurred)
91 {
93  using namespace std;
94  parent::notifyOfWindowEvents(events_occurred);
95 
96  // I know the keys exists - I registered them explicitly
97 
98  // optimization_distance toggling
99  if (opt_params.optimization_distance > 0)
100  {
101  if (events_occurred.find(opt_params.keystroke_optimization_distance)
102  ->second)
103  {
104  this->toggleOptDistanceVisualization();
105  }
106 
107  if (events_occurred.find(opt_params.keystroke_optimize_graph)->second)
108  {
109  this->_optimizeGraph(/*is_full_update=*/true);
110  }
111  }
112 
113  // graph toggling
114  if (events_occurred.find(viz_params.keystroke_graph_toggle)->second)
115  {
116  this->toggleGraphVisualization();
117  }
118 
119  // if mouse event, let the user decide about the camera
120  if (events_occurred.find("mouse_clicked")->second)
121  {
122  MRPT_LOG_DEBUG_STREAM("Mouse was clicked. Disabling autozoom.");
123  m_autozoom_active = false;
124  }
125 
126  // autofit the graph once
127  if (events_occurred.find(viz_params.keystroke_graph_autofit)->second)
128  {
129  MRPT_LOG_DEBUG_STREAM("Autofit button was pressed");
130  this->fitGraphInView();
131  }
132 
133  MRPT_END
134 } // end of notifyOfWindowEvents
135 
136 template <class GRAPH_T>
138 {
139  MRPT_START
140  ASSERTDEBMSG_(this->m_win_manager, "No CWindowManager* is given");
141 
142  if (viz_params.visualize_optimized_graph)
143  {
144  this->m_win_observer->registerKeystroke(
145  viz_params.keystroke_graph_toggle, "Toggle Graph visualization");
146  this->m_win_observer->registerKeystroke(
147  viz_params.keystroke_graph_autofit, "Fit Graph in view");
148 
149  this->m_win_manager->assignTextMessageParameters(
150  /* offset_y* = */ &viz_params.offset_y_graph,
151  /* text_index* = */ &viz_params.text_index_graph);
152  }
153 
154  MRPT_END
155 }
156 template <class GRAPH_T>
158 {
159  MRPT_START
160  ASSERTDEBMSG_(this->m_win_manager, "No CWindowManager* is given");
161  using namespace mrpt::opengl;
162 
163  this->logFmt(
164  mrpt::system::LVL_DEBUG, "In the updateGraphVisualization function");
165 
166  // update the graph (clear and rewrite..)
167  COpenGLScene::Ptr& scene = this->m_win->get3DSceneAndLock();
168 
169  // remove previous graph and insert the new instance
170  // TODO - make this an incremental proocecure
171  CRenderizable::Ptr prev_object = scene->getByName("optimized_graph");
172  bool prev_visibility = true;
173  if (prev_object)
174  { // set the visibility of the graph correctly
175  prev_visibility = prev_object->isVisible();
176  }
177  scene->removeObject(prev_object);
178 
179  // CSetOfObjects::Ptr graph_obj =
180  // graph_tools::graph_visualize(*this->m_graph, viz_params.cfg);
181  CSetOfObjects::Ptr graph_obj = std::make_shared<CSetOfObjects>();
182  this->m_graph->getAs3DObject(graph_obj, viz_params.cfg);
183 
184  graph_obj->setName("optimized_graph");
185  graph_obj->setVisibility(prev_visibility);
186  scene->insert(graph_obj);
187  this->m_win->unlockAccess3DScene();
188 
189  this->m_win_manager->addTextMessage(
190  5, -viz_params.offset_y_graph,
191  format(
192  "Optimized Graph: #nodes %d",
193  static_cast<int>(this->m_graph->nodeCount())),
194  mrpt::img::TColorf(0.0, 0.0, 0.0),
195  /* unique_index = */ viz_params.text_index_graph);
196 
197  this->m_win->forceRepaint();
198 
199  if (m_autozoom_active)
200  {
201  this->fitGraphInView();
202  }
203 
204  MRPT_END
205 }
206 
207 template <class GRAPH_T>
209 {
210  MRPT_START
211  using namespace mrpt::opengl;
212 
213  COpenGLScene::Ptr& scene = this->m_win->get3DSceneAndLock();
214 
215  CRenderizable::Ptr graph_obj = scene->getByName("optimized_graph");
216  graph_obj->setVisibility(!graph_obj->isVisible());
217 
218  this->m_win->unlockAccess3DScene();
219  this->m_win->forceRepaint();
220 
221  MRPT_END
222 }
223 
224 template <class GRAPH_T>
226 {
227  MRPT_START
228  using namespace mrpt::opengl;
229 
231  this->m_win,
232  "\nVisualization of data was requested but no CDisplayWindow3D pointer "
233  "was given\n");
234 
235  // first fetch the graph object
236  COpenGLScene::Ptr& scene = this->m_win->get3DSceneAndLock();
237  CRenderizable::Ptr obj = scene->getByName("optimized_graph");
238  CSetOfObjects::Ptr graph_obj =
239  std::dynamic_pointer_cast<CSetOfObjects>(obj);
240  this->m_win->unlockAccess3DScene();
241  this->m_win->forceRepaint();
242 
243  // autofit it based on its grid
244  CGridPlaneXY::Ptr obj_grid =
245  graph_obj->CSetOfObjects::getByClass<CGridPlaneXY>();
246  if (obj_grid)
247  {
248  float x_min, x_max, y_min, y_max;
249  obj_grid->getPlaneLimits(x_min, x_max, y_min, y_max);
250  const float z_min = obj_grid->getPlaneZcoord();
251  this->m_win->setCameraPointingToPoint(
252  0.5 * (x_min + x_max), 0.5 * (y_min + y_max), z_min);
253  this->m_win->setCameraZoom(
254  2.0f * std::max(10.0f, std::max(x_max - x_min, y_max - y_min)));
255  }
256  this->m_win->setCameraAzimuthDeg(60);
257  this->m_win->setCameraElevationDeg(75);
258  this->m_win->setCameraProjective(true);
259 
260  MRPT_END
261 }
262 
263 template <class GRAPH_T>
265 {
266  MRPT_START
267  using namespace mrpt::opengl;
268 
269  if (opt_params.optimization_distance > 0)
270  {
271  this->m_win_observer->registerKeystroke(
272  opt_params.keystroke_optimization_distance,
273  "Toggle optimization distance on/off");
274 
275  this->m_win_observer->registerKeystroke(
276  opt_params.keystroke_optimize_graph,
277  "Manually trigger a full graph optimization");
278  }
279 
280  pose_t p;
281  CRenderizable::Ptr obj = this->initOptDistanceVisualizationInternal(p);
282  pose_t initial_pose;
283  obj->setPose(initial_pose);
284  obj->setName("optimization_distance_obj");
285 
286  COpenGLScene::Ptr scene = this->m_win->get3DSceneAndLock();
287  scene->insert(obj);
288  this->m_win->unlockAccess3DScene();
289  this->m_win->forceRepaint();
290 
291  // optimization distance disk - textMessage
292  this->m_win_manager->assignTextMessageParameters(
293  &opt_params.offset_y_optimization_distance,
294  &opt_params.text_index_optimization_distance);
295 
296  this->m_win_manager->addTextMessage(
297  5, -opt_params.offset_y_optimization_distance,
298  "Radius for graph optimization",
299  mrpt::img::TColorf(opt_params.optimization_distance_color),
300  /* unique_index = */ opt_params.text_index_optimization_distance);
301  MRPT_END
302 }
303 
304 template <class GRAPH_T>
307  const mrpt::poses::CPose2D& p_unused)
308 {
309  using namespace mrpt::opengl;
310 
311  CDisk::Ptr obj = std::make_shared<CDisk>();
312  obj->setDiskRadius(
313  opt_params.optimization_distance,
314  opt_params.optimization_distance - 0.1);
315  obj->setColor_u8(opt_params.optimization_distance_color);
316 
317  return obj;
318 }
319 template <class GRAPH_T>
322  const mrpt::poses::CPose3D& p_unused)
323 {
324  using namespace mrpt::opengl;
325 
326  CSphere::Ptr obj = std::make_shared<CSphere>();
327  obj->setRadius(opt_params.optimization_distance);
328  obj->setColor_u8(
329  opt_params.optimization_distance_color.R,
330  opt_params.optimization_distance_color.G,
331  opt_params.optimization_distance_color.B,
332  /*alpha = */ 60);
333 
334  return obj;
335 }
336 
337 template <class GRAPH_T>
339 {
340  MRPT_START
341  ASSERTDEBMSG_(this->m_win_manager, "No CWindowManager* is given");
342  using namespace mrpt::opengl;
343 
344  // update ICP_max_distance Disk
345  COpenGLScene::Ptr scene = this->m_win->get3DSceneAndLock();
346 
347  CRenderizable::Ptr obj = scene->getByName("optimization_distance_obj");
348  obj->setPose(this->m_graph->nodes.rbegin()->second);
349 
350  this->m_win->unlockAccess3DScene();
351  this->m_win->forceRepaint();
352  MRPT_END
353 }
354 
355 // TODO - implement this
356 template <class GRAPH_T>
358 {
359  MRPT_START
360  using namespace mrpt::opengl;
361 
362  COpenGLScene::Ptr scene = this->m_win->get3DSceneAndLock();
363 
364  CRenderizable::Ptr obj = scene->getByName("optimization_distance_obj");
365  obj->setVisibility(!obj->isVisible());
366 
367  this->m_win->unlockAccess3DScene();
368  this->m_win->forceRepaint();
369 
370  MRPT_END
371 }
372 
373 template <class GRAPH_T>
375 {
376  MRPT_START
377  using namespace std;
378 
380  "optimizeGraph:: ThreadID:" << endl
381  << "\t" << std::this_thread::get_id()
382  << endl
383  << "\t"
384  << "Trying to grab lock... ");
385 
386  std::lock_guard<std::mutex> graph_lock(*this->m_graph_section);
387  this->_optimizeGraph();
388 
389  this->logFmt(mrpt::system::LVL_DEBUG, "2nd thread grabbed the lock..");
390 
391  MRPT_END
392 }
393 
394 template <class GRAPH_T>
395 void CLevMarqGSO<GRAPH_T>::_optimizeGraph(bool is_full_update /*=false*/)
396 {
397  MRPT_START
398  this->m_time_logger.enter("CLevMarqGSO::_optimizeGraph");
399 
400  // if less than X nodes exist overall, do not try optimizing
401  if (m_min_nodes_for_optimization > this->m_graph->nodes.size())
402  {
403  return;
404  }
405 
406  mrpt::system::CTicTac optimization_timer;
407  optimization_timer.Tic();
408 
409  // set of nodes for which the optimization procedure will take place
410  std::set<mrpt::graphs::TNodeID>* nodes_to_optimize;
411 
412  // fill in the nodes in certain distance to the current node, only if
413  // is_full_update is not instructed
414  if (is_full_update)
415  {
416  // nodes_to_optimize: List of nodes to optimize. nullptr -> all but the
417  // root
418  // node.
419  nodes_to_optimize = nullptr;
420  }
421  else
422  {
423  nodes_to_optimize = new std::set<mrpt::graphs::TNodeID>;
424 
425  // I am certain that this shall not be called when nodeCount = 0, since
426  // the
427  // optimization procedure starts only after certain number of nodes has
428  // been added
429  this->getNearbyNodesOf(
430  nodes_to_optimize, this->m_graph->nodeCount() - 1,
431  opt_params.optimization_distance);
432  nodes_to_optimize->insert(this->m_graph->nodeCount() - 1);
433  }
434 
436 
437  // Execute the optimization
439  *(this->m_graph), levmarq_info, nodes_to_optimize, opt_params.cfg,
440  &CLevMarqGSO<GRAPH_T>::levMarqFeedback); // functor feedback
441 
442  if (is_full_update)
443  {
444  m_just_fully_optimized_graph = true;
445  }
446  else
447  {
448  m_just_fully_optimized_graph = false;
449  }
450 
451  double elapsed_time = optimization_timer.Tac();
452  this->logFmt(
453  mrpt::system::LVL_DEBUG, "Optimization of graph took: %fs",
454  elapsed_time);
455 
456  // deleting the nodes_to_optimize set
457  delete nodes_to_optimize;
458  nodes_to_optimize = nullptr;
459 
460  this->m_time_logger.leave("CLevMarqGSO::_optimizeGraph");
461  MRPT_END
462 } // end of _optimizeGraph
463 
464 template <class GRAPH_T>
466 {
467  MRPT_START
468 
469  bool is_loop_closure = false;
470  auto curr_pair_nodes_to_edge = this->m_graph->edges;
471 
472  // find the *node pairs* that exist in current but not the last
473  // nodes_to_edge
474  // map If the distance of any of these pairs is greater than
475  // LC_min_nodeid_diff then consider this a loop closure
476  typename GRAPH_T::edges_map_t::const_iterator search;
477  mrpt::graphs::TPairNodeIDs curr_pair;
478 
479  for (auto it = curr_pair_nodes_to_edge.begin();
480  it != curr_pair_nodes_to_edge.end(); ++it)
481  {
482  search = opt_params.last_pair_nodes_to_edge.find(it->first);
483  // if current node pair is not found in the last set...
484  if (search == opt_params.last_pair_nodes_to_edge.end())
485  {
486  curr_pair = it->first;
487 
488  if (std::abs(
489  static_cast<int>(curr_pair.first) -
490  static_cast<int>(curr_pair.second)) >
491  opt_params.LC_min_nodeid_diff)
492  {
493  this->logFmt(
494  mrpt::system::LVL_DEBUG, "Registering loop closure... ");
495  is_loop_closure = true;
496  break; // no need for more iterations
497  }
498  }
499  }
500 
501  // update the pair_nodes_to_edge map
502  opt_params.last_pair_nodes_to_edge = curr_pair_nodes_to_edge;
503  return is_loop_closure;
504 
505  MRPT_END
506 }
507 
508 template <class GRAPH_T>
510 {
511  bool is_full_update = false;
512 
513  if (opt_params.optimization_distance == -1)
514  { // always optimize fully
515  return true;
516  }
517 
518  bool added_lc = this->checkForLoopClosures();
519 
520  // Decide on the LoopClosingAttitude I am in
521  if (!added_lc)
522  { // reset both ignored and used counters
523  if (m_curr_used_consec_lcs != 0 || m_curr_ignored_consec_lcs != 0)
524  {
525  MRPT_LOG_DEBUG_STREAM("No new Loop Closure found.");
526  }
527 
528  m_curr_used_consec_lcs = 0;
529  m_curr_ignored_consec_lcs = 0;
530  m_optimization_policy = OptimizationPolicy::UseLoopClosures;
531 
532  return is_full_update;
533  }
534  else
535  { // lc found.
536  // have I used enough consecutive loop closures?
537  bool use_limit_reached =
538  m_curr_used_consec_lcs == m_max_used_consec_lcs;
539  // have I ignored enough consecutive loop closures?
540  bool ignore_limit_reached =
541  m_curr_ignored_consec_lcs == m_max_ignored_consec_lcs;
542 
543  // Have I reached any of the limits above?
544  if (ignore_limit_reached || use_limit_reached)
545  {
546  m_curr_ignored_consec_lcs = 0;
547  m_curr_used_consec_lcs = 0;
548 
549  // decide of the my policy on full optimization
550  if (ignore_limit_reached)
551  {
552  m_optimization_policy = OptimizationPolicy::UseLoopClosures;
553  }
554  if (use_limit_reached)
555  {
556  m_optimization_policy = OptimizationPolicy::IgnoreLoopClosures;
557  }
558  }
559  else
560  { // no limits reached yet.
561  if (m_optimization_policy == OptimizationPolicy::UseLoopClosures)
562  {
563  m_curr_used_consec_lcs += 1;
564  }
565  else
566  {
567  m_curr_ignored_consec_lcs += 1;
568  }
569  }
570  }
571 
572  // Decide on whether to fully optimize the graph based on the mode I am in
573  if (m_optimization_policy == OptimizationPolicy::IgnoreLoopClosures)
574  {
575  is_full_update = false;
577  "*PARTIAL* graph optimization.. ignoring new loop closure");
578  }
579  else
580  {
581  is_full_update = true;
582  MRPT_LOG_WARN_STREAM("Commencing with *FULL* graph optimization... ");
583  }
584  return is_full_update;
585 
586 } // end of checkForFullOptimization
587 
588 template <class GRAPH_T>
590 {
591  return m_just_fully_optimized_graph;
592 }
593 
594 template <class GRAPH_T>
596  const GRAPH_T& graph, const size_t iter, const size_t max_iter,
597  const double cur_sq_error)
598 {
599 }
600 
601 template <class GRAPH_T>
603  std::set<mrpt::graphs::TNodeID>* nodes_set,
604  const mrpt::graphs::TNodeID& cur_nodeID, double distance)
605 {
606  MRPT_START
607 
608  if (distance > 0)
609  {
610  // check all but the last node.
611  for (mrpt::graphs::TNodeID nodeID = 0;
612  nodeID < this->m_graph->nodeCount() - 1; ++nodeID)
613  {
614  double curr_distance = this->m_graph->nodes[nodeID].distanceTo(
615  this->m_graph->nodes[cur_nodeID]);
616  if (curr_distance <= distance)
617  {
618  nodes_set->insert(nodeID);
619  }
620  }
621  }
622  else
623  { // check against all nodes
624  this->m_graph->getAllNodes(*nodes_set);
625  }
626 
627  MRPT_END
628 }
629 
630 template <class GRAPH_T>
632 {
633  parent::printParams();
634 
635  opt_params.dumpToConsole();
636  viz_params.dumpToConsole();
637 }
638 template <class GRAPH_T>
639 void CLevMarqGSO<GRAPH_T>::loadParams(const std::string& source_fname)
640 {
641  MRPT_START
642  parent::loadParams(source_fname);
643 
644  opt_params.loadFromConfigFileName(source_fname, "OptimizerParameters");
645  viz_params.loadFromConfigFileName(source_fname, "VisualizationParameters");
646 
647  mrpt::config::CConfigFile source(source_fname);
648 
649  // TODO - check that these work
650  m_max_used_consec_lcs = source.read_int(
651  "OptimizerParameters", "max_used_consecutive_loop_closures", 2, false);
652 
653  m_max_ignored_consec_lcs = source.read_int(
654  "OptimizerParameters", "max_ignored_consecutive_loop_closures", 15,
655  false);
656 
657  // set the logging level if given by the user
658  // Minimum verbosity level of the logger
659  int min_verbosity_level =
660  source.read_int("OptimizerParameters", "class_verbosity", 1, false);
661  this->setMinLoggingLevel(mrpt::system::VerbosityLevel(min_verbosity_level));
662 
663  MRPT_LOG_DEBUG("Successfully loaded Params. ");
664  m_has_read_config = true;
665 
666  MRPT_END
667 }
668 
669 template <class GRAPH_T>
670 void CLevMarqGSO<GRAPH_T>::getDescriptiveReport(std::string* report_str) const
671 {
672  MRPT_START
673  using namespace std;
674 
675  const std::string report_sep(2, '\n');
676  const std::string header_sep(80, '#');
677 
678  // Report on graph
679  stringstream class_props_ss;
680  class_props_ss << "Levenberg Marquardt Optimization Summary: " << std::endl;
681  class_props_ss << header_sep << std::endl;
682 
683  // time and output logging
684  const std::string time_res = this->m_time_logger.getStatsAsText();
685  const std::string output_res = this->getLogAsString();
686 
687  // merge the individual reports
688  report_str->clear();
689  parent::getDescriptiveReport(report_str);
690 
691  *report_str += class_props_ss.str();
692  *report_str += report_sep;
693 
694  *report_str += time_res;
695  *report_str += report_sep;
696 
697  *report_str += output_res;
698  *report_str += report_sep;
699 
700  MRPT_END
701 }
702 
703 template <class GRAPH_T>
705  : optimization_distance_color(0, 201, 87),
706  keystroke_optimization_distance("u"),
707  keystroke_optimize_graph("w")
708 {
709 }
710 template <class GRAPH_T>
712 template <class GRAPH_T>
714  std::ostream& out) const
715 {
716  MRPT_START
717  out << "-----------[ Levenberg-Marquardt Optimization ] -------\n";
718  out << "Optimization on second thread = "
719  << (optimization_on_second_thread ? "TRUE" : "FALSE") << std::endl;
720  out << "Optimize nodes in distance = " << optimization_distance << "\n";
721  out << "Min. node difference for LC = " << LC_min_nodeid_diff << "\n";
722  out << cfg.getAsString() << std::endl;
723  MRPT_END
724 }
725 template <class GRAPH_T>
727  const mrpt::config::CConfigFileBase& source, const std::string& section)
728 {
729  MRPT_START
730  optimization_on_second_thread = source.read_bool(
731  section, "optimization_on_second_thread", false, false);
732  LC_min_nodeid_diff = source.read_int(
733  "GeneralConfiguration", "LC_min_nodeid_diff", 30, false);
734  optimization_distance =
735  source.read_double(section, "optimization_distance", 5, false);
736  // asert the previous value
738  optimization_distance == 1 || optimization_distance > 0,
739  format(
740  "Invalid value for optimization distance: %.2f",
741  optimization_distance));
742 
743  // optimization parameters
744  cfg["verbose"] = source.read_bool(section, "verbose", false, false);
745  cfg["profiler"] = source.read_bool(section, "profiler", false, false);
746  cfg["max_iterations"] =
747  source.read_double(section, "max_iterations", 100, false);
748  cfg["scale_hessian"] =
749  source.read_double("Optimization", "scale_hessian", 0.2, false);
750  cfg["tau"] = source.read_double(section, "tau", 1e-3, false);
751 
752  MRPT_END
753 }
754 
755 template <class GRAPH_T>
757  : keystroke_graph_toggle("s"), keystroke_graph_autofit("a")
758 {
759 }
760 template <class GRAPH_T>
762  default;
763 template <class GRAPH_T>
765  std::ostream& out) const
766 {
767  MRPT_START
768 
769  out << "-----------[ Graph Visualization Parameters ]-----------\n";
770  out << mrpt::format(
771  "Visualize optimized graph = %s\n",
772  visualize_optimized_graph ? "TRUE" : "FALSE");
773 
774  out << mrpt::format("%s", cfg.getAsString().c_str());
775 
776  std::cout << std::endl;
777 
778  MRPT_END
779 }
780 template <class GRAPH_T>
782  const mrpt::config::CConfigFileBase& source, const std::string& section)
783 {
784  MRPT_START
785 
786  visualize_optimized_graph =
787  source.read_bool(section, "visualize_optimized_graph", true, false);
788 
789  cfg["show_ID_labels"] =
790  source.read_bool(section, "optimized_show_ID_labels", false, false);
791  cfg["show_ground_grid"] =
792  source.read_double(section, "optimized_show_ground_grid", 1, false);
793  cfg["show_edges"] =
794  source.read_bool(section, "optimized_show_edges", true, false);
795  cfg["edge_color"] =
796  source.read_int(section, "optimized_edge_color", 1500, false);
797  cfg["edge_width"] =
798  source.read_double(section, "optimized_edge_width", 1.5, false);
799  cfg["show_node_corners"] =
800  source.read_bool(section, "optimized_show_node_corners", true, false);
801  cfg["show_edge_rel_poses"] =
802  source.read_bool(section, "optimized_show_edge_rel_poses", true, false);
803  cfg["edge_rel_poses_color"] =
804  source.read_int(section, "optimized_edge_rel_poses_color", 2000, false);
805  cfg["nodes_edges_corner_scale"] = source.read_double(
806  section, "optimized_nodes_edges_corner_scale", 0.4, false);
807  cfg["nodes_corner_scale"] =
808  source.read_double(section, "optimized_nodes_corner_scale", 0.7, false);
809  cfg["nodes_point_size"] =
810  source.read_int(section, "optimized_nodes_point_size", 5, false);
811  cfg["nodes_point_color"] =
812  source.read_int(section, "optimized_nodes_point_color", 3000, false);
813 
814  MRPT_END
815 }
816 } // namespace mrpt::graphslam::optimizers
mrpt::obs::CObservation::Ptr
std::shared_ptr< CObservation > Ptr
Definition: CObservation.h:45
mrpt::graphslam::optimizers::CLevMarqGSO::OptimizationParams::loadFromConfigFile
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.
Definition: CLevMarqGSO_impl.h:726
mrpt::graphslam::optimizers::CLevMarqGSO::GraphVisualizationParams::GraphVisualizationParams
GraphVisualizationParams()
Definition: CLevMarqGSO_impl.h:756
mrpt::graphslam::optimizers::CLevMarqGSO::OptimizationPolicy
OptimizationPolicy
Enumeration that defines the behaviors towards using or ignoring a newly added loop closure to fully ...
Definition: CLevMarqGSO.h:340
mrpt::graphslam::optimizers::CLevMarqGSO::optimizeGraph
void optimizeGraph() override
Wrapper around _optimizeGraph which first locks the section and then calls the _optimizeGraph method.
Definition: CLevMarqGSO_impl.h:374
mrpt::opengl::CSetOfObjects::Ptr
std::shared_ptr< mrpt::opengl ::CSetOfObjects > Ptr
Definition: CSetOfObjects.h:28
mrpt::graphslam::optimizers::CLevMarqGSO::loadParams
void loadParams(const std::string &source_fname) override
Load the necessary for the decider/optimizer configuration parameters.
Definition: CLevMarqGSO_impl.h:639
mrpt::opengl::CGridPlaneXY::Ptr
std::shared_ptr< mrpt::opengl ::CGridPlaneXY > Ptr
Definition: CGridPlaneXY.h:31
mrpt::system::CTicTac
A high-performance stopwatch, with typical resolution of nanoseconds.
Definition: system/CTicTac.h:17
MRPT_LOG_DEBUG
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
Definition: system/COutputLogger.h:427
mrpt::obs::CSensoryFrame::Ptr
std::shared_ptr< mrpt::obs ::CSensoryFrame > Ptr
Definition: CSensoryFrame.h:53
mrpt::graphslam::optimize_graph_spa_levmarq
void optimize_graph_spa_levmarq(GRAPH_T &graph, TResultInfoSpaLevMarq &out_info, const std::set< mrpt::graphs::TNodeID > *in_nodes_to_optimize=nullptr, const mrpt::system::TParametersDouble &extra_params=mrpt::system::TParametersDouble(), FEEDBACK_CALLABLE functor_feedback=FEEDBACK_CALLABLE())
Optimize a graph of pose constraints using the Sparse Pose Adjustment (SPA) sparse representation and...
Definition: levmarq.h:79
mrpt::graphslam::optimizers::CLevMarqGSO::updateState
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.
Definition: CLevMarqGSO_impl.h:23
mrpt::config::CConfigFileBase::read_double
double read_double(const std::string &section, const std::string &name, double defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:106
mrpt::graphslam::optimizers::CLevMarqGSO::GraphVisualizationParams::loadFromConfigFile
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.
Definition: CLevMarqGSO_impl.h:781
mrpt::graphslam::optimizers::CLevMarqGSO::checkForLoopClosures
bool checkForLoopClosures()
Check if a loop closure edge was added in the graph.
Definition: CLevMarqGSO_impl.h:465
mrpt::graphslam::optimizers::CLevMarqGSO::getDescriptiveReport
void getDescriptiveReport(std::string *report_str) const override
Fill the provided string with a detailed report of the decider/optimizer state.
Definition: CLevMarqGSO_impl.h:670
mrpt::graphslam::optimizers::CLevMarqGSO::notifyOfWindowEvents
void notifyOfWindowEvents(const std::map< std::string, bool > &events_occurred) override
Get a list of the window events that happened since the last call.
Definition: CLevMarqGSO_impl.h:89
mrpt::graphs::TNodeID
uint64_t TNodeID
A generic numeric type for unique IDs of nodes or entities.
Definition: TNodeID.h:16
mrpt::graphslam::TResultInfoSpaLevMarq
Output information for mrpt::graphslam::optimize_graph_spa_levmarq()
Definition: graphslam/include/mrpt/graphslam/types.h:68
mrpt::config::CConfigFileBase::read_bool
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:155
mrpt::opengl::CDisk::Ptr
std::shared_ptr< mrpt::opengl ::CDisk > Ptr
Definition: CDisk.h:32
setMinLoggingLevel
app setMinLoggingLevel(mrpt::system::LVL_ERROR)
mrpt::obs::CActionCollection::Ptr
std::shared_ptr< mrpt::obs ::CActionCollection > Ptr
Definition: CActionCollection.h:28
out
mrpt::vision::TStereoCalibResults out
Definition: chessboard_stereo_camera_calib_unittest.cpp:25
mrpt::graphslam::optimizers::CLevMarqGSO::OptimizationParams::~OptimizationParams
~OptimizationParams() override
mrpt::opengl::CRenderizable::Ptr
std::shared_ptr< CRenderizable > Ptr
Definition: CRenderizable.h:50
mrpt::math::distance
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1807
mrpt::graphslam::optimizers::CLevMarqGSO::initializeVisuals
void initializeVisuals() override
Initialize visual objects in CDisplayWindow (e.g.
Definition: CLevMarqGSO_impl.h:60
mrpt::graphslam::optimizers::CLevMarqGSO::fitGraphInView
void fitGraphInView()
Set the camera parameters of the CDisplayWindow3D so that the whole graph is viewed in the window.
Definition: CLevMarqGSO_impl.h:225
MRPT_LOG_WARN_STREAM
#define MRPT_LOG_WARN_STREAM(__CONTENTS)
Definition: system/COutputLogger.h:475
mrpt::graphslam::optimizers::CLevMarqGSO::updateGraphVisualization
void updateGraphVisualization()
Called internally for updating the visualization scene for the graph building procedure.
Definition: CLevMarqGSO_impl.h:157
mrpt::system::CTicTac::Tac
double Tac() noexcept
Stops the stopwatch.
Definition: CTicTac.cpp:86
mrpt::graphslam::CRegistrationDeciderOrOptimizer< typename mrpt::graphs::CNetworkOfPoses2DInf >::initializeLoggers
virtual void initializeLoggers(const std::string &name)
Initialize the COutputLogger, CTimeLogger instances given the name of the decider/optimizer at hand.
Definition: CRegistrationDeciderOrOptimizer_impl.h:22
mrpt::config::CConfigFileBase::read_int
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:130
mrpt::graphslam::optimizers::CLevMarqGSO::initOptDistanceVisualizationInternal
mrpt::opengl::CRenderizable::Ptr initOptDistanceVisualizationInternal(const mrpt::poses::CPose2D &p_unused)
Setup the corresponding Disk/Sphere instance.
Definition: CLevMarqGSO_impl.h:306
mrpt::graphslam::optimizers::CLevMarqGSO::GraphVisualizationParams::~GraphVisualizationParams
~GraphVisualizationParams() override
mrpt::opengl::CGridPlaneXY
A grid of lines over the XY plane.
Definition: CGridPlaneXY.h:29
mrpt::opengl::CGridPlaneXY::getPlaneLimits
void getPlaneLimits(float &xmin, float &xmax, float &ymin, float &ymax) const
Definition: CGridPlaneXY.h:49
mrpt::system::VerbosityLevel
VerbosityLevel
Enumeration of available verbosity levels.
Definition: system/COutputLogger.h:28
MRPT_START
#define MRPT_START
Definition: exceptions.h:241
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
mrpt::graphs::TPairNodeIDs
std::pair< TNodeID, TNodeID > TPairNodeIDs
A pair of node IDs.
Definition: TNodeID.h:18
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Definition: CPose2D.h:39
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::img::TColorf
An RGBA color - floats in the range [0,1].
Definition: TColor.h:88
mrpt::graphslam::optimizers
Definition: CEmptyGSO.h:17
mrpt::graphslam::optimizers::CLevMarqGSO::justFullyOptimizedGraph
bool justFullyOptimizedGraph() const override
Definition: CLevMarqGSO_impl.h:589
mrpt::graphslam::optimizers::CLevMarqGSO::_optimizeGraph
void _optimizeGraph(bool is_full_update=false)
Optimize the given graph.
Definition: CLevMarqGSO_impl.h:395
mrpt::system::CTicTac::Tic
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:75
mrpt::graphslam::optimizers::CLevMarqGSO::OptimizationParams::OptimizationParams
OptimizationParams()
Definition: CLevMarqGSO_impl.h:704
mrpt::graphslam::optimizers::CLevMarqGSO::checkForFullOptimization
bool checkForFullOptimization()
Decide whether to issue a full graph optimization.
Definition: CLevMarqGSO_impl.h:509
mrpt::graphslam::optimizers::CLevMarqGSO::getNearbyNodesOf
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.
Definition: CLevMarqGSO_impl.h:602
mrpt::opengl::CSphere::Ptr
std::shared_ptr< mrpt::opengl ::CSphere > Ptr
Definition: CSphere.h:31
mrpt::graphslam::optimizers::CLevMarqGSO::GraphVisualizationParams::dumpToTextStream
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form,...
Definition: CLevMarqGSO_impl.h:764
mrpt::graphslam::optimizers::CLevMarqGSO::toggleOptDistanceVisualization
void toggleOptDistanceVisualization()
toggle the optimization distance object on and off
Definition: CLevMarqGSO_impl.h:357
mrpt::graphslam::optimizers::CLevMarqGSO::initGraphVisualization
void initGraphVisualization()
Initialize objects relateed to the Graph Visualization.
Definition: CLevMarqGSO_impl.h:137
mrpt::graphslam::optimizers::CLevMarqGSO::updateVisuals
void updateVisuals() override
Update the relevant visual features in CDisplayWindow.
Definition: CLevMarqGSO_impl.h:73
MRPT_LOG_DEBUG_STREAM
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
Use: MRPT_LOG_DEBUG_STREAM("Var=" << value << " foo=" << foo_var);
Definition: system/COutputLogger.h:471
mrpt::opengl::COpenGLScene::Ptr
std::shared_ptr< mrpt::opengl ::COpenGLScene > Ptr
Definition: COpenGLScene.h:58
mrpt::system::LVL_DEBUG
@ LVL_DEBUG
Definition: system/COutputLogger.h:30
mrpt::graphslam::optimizers::CLevMarqGSO::levMarqFeedback
static void levMarqFeedback(const GRAPH_T &graph, const size_t iter, const size_t max_iter, const double cur_sq_error)
Feedback of the Levenberg-Marquardt graph optimization procedure.
Definition: CLevMarqGSO_impl.h:595
mrpt::graphslam::optimizers::CLevMarqGSO::pose_t
typename GRAPH_T::constraint_t::type_value pose_t
type of underlying poses (2D/3D)
Definition: CLevMarqGSO.h:127
mrpt::graphslam::optimizers::CLevMarqGSO::CLevMarqGSO
CLevMarqGSO()
Definition: CLevMarqGSO_impl.h:15
MRPT_END
#define MRPT_END
Definition: exceptions.h:245
mrpt::graphslam::optimizers::CLevMarqGSO::initOptDistanceVisualization
void initOptDistanceVisualization()
Initialize the Disk/Sphere used for visualizing the optimization distance.
Definition: CLevMarqGSO_impl.h:264
mrpt::graphslam::optimizers::CLevMarqGSO::toggleGraphVisualization
void toggleGraphVisualization()
Toggle the graph visualization on and off.
Definition: CLevMarqGSO_impl.h:208
mrpt::config::CConfigFile
This class allows loading and storing values and vectors of different types from "....
Definition: config/CConfigFile.h:31
ASSERTDEBMSG_
#define ASSERTDEBMSG_(f, __ERROR_MSG)
Definition: exceptions.h:191
ASSERTDEB_
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
Definition: exceptions.h:190
mrpt::graphslam::optimizers::CLevMarqGSO
Levenberg-Marquardt non-linear graph slam optimization scheme.
Definition: CLevMarqGSO.h:117
mrpt::graphslam::optimizers::CLevMarqGSO::printParams
void printParams() const override
Definition: CLevMarqGSO_impl.h:631
mrpt::opengl
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:13
mrpt::graphslam::optimizers::CLevMarqGSO::OptimizationParams::dumpToTextStream
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form,...
Definition: CLevMarqGSO_impl.h:713
mrpt::format
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
mrpt::graphslam::optimizers::CLevMarqGSO::updateOptDistanceVisualization
void updateOptDistanceVisualization()
Update the position of the disk indicating the distance in which Levenberg-Marquardt graph optimizati...
Definition: CLevMarqGSO_impl.h:338



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Thu May 21 21:53:32 UTC 2020