MRPT  2.0.2
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
void initGraphVisualization()
Initialize objects relateed to the Graph Visualization.
double Tac() noexcept
Stops the stopwatch.
Definition: CTicTac.cpp:86
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.
#define MRPT_START
Definition: exceptions.h:241
bool checkForFullOptimization()
Decide whether to issue a full graph optimization.
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
void updateVisuals() override
Update the relevant visual features in CDisplayWindow.
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.
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...
VerbosityLevel
Enumeration of available verbosity levels.
A grid of lines over the XY plane.
Definition: CGridPlaneXY.h:29
mrpt::opengl::CRenderizable::Ptr initOptDistanceVisualizationInternal(const mrpt::poses::CPose2D &p_unused)
Setup the corresponding Disk/Sphere instance.
void updateGraphVisualization()
Called internally for updating the visualization scene for the graph building procedure.
bool justFullyOptimizedGraph() const override
Used by the caller to query for possible full graph optimization on the latest optimizer run...
void toggleGraphVisualization()
Toggle the graph visualization on and off.
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
A set of object, which are referenced to the coordinates framework established in this object...
Definition: CSetOfObjects.h:26
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
app setMinLoggingLevel(mrpt::system::LVL_ERROR)
This class allows loading and storing values and vectors of different types from ".ini" files easily.
typename GRAPH_T::constraint_t::type_value pose_t
type of underlying poses (2D/3D)
Definition: CLevMarqGSO.h:127
A high-performance stopwatch, with typical resolution of nanoseconds.
STL namespace.
void printParams() const override
Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact w...
#define MRPT_LOG_WARN_STREAM(__CONTENTS)
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
void toggleOptDistanceVisualization()
toggle the optimization distance object on and off
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
void initOptDistanceVisualization()
Initialize the Disk/Sphere used for visualizing the optimization distance.
This class allows loading and storing values and vectors of different types from a configuration text...
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...
OptimizationPolicy
Enumeration that defines the behaviors towards using or ignoring a newly added loop closure to fully ...
Definition: CLevMarqGSO.h:340
void optimizeGraph() override
Wrapper around _optimizeGraph which first locks the section and then calls the _optimizeGraph method...
double read_double(const std::string &section, const std::string &name, double defaultValue, bool failIfNotFound=false) const
virtual void initializeLoggers(const std::string &name)
Initialize the COutputLogger, CTimeLogger instances given the name of the decider/optimizer at hand...
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
Use: MRPT_LOG_DEBUG_STREAM("Var=" << value << " foo=" << foo_var);
bool checkForLoopClosures()
Check if a loop closure edge was added in the graph.
#define ASSERTDEBMSG_(f, __ERROR_MSG)
Definition: exceptions.h:191
void updateOptDistanceVisualization()
Update the position of the disk indicating the distance in which Levenberg-Marquardt graph optimizati...
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
Definition: exceptions.h:190
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
#define MRPT_END
Definition: exceptions.h:245
An RGBA color - floats in the range [0,1].
Definition: TColor.h:88
Output information for mrpt::graphslam::optimize_graph_spa_levmarq()
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:13
uint64_t TNodeID
A generic numeric type for unique IDs of nodes or entities.
Definition: TNodeID.h:16
void getDescriptiveReport(std::string *report_str) const override
Fill the provided string with a detailed report of the decider/optimizer state.
void loadParams(const std::string &source_fname) override
Load the necessary for the decider/optimizer configuration parameters.
std::pair< TNodeID, TNodeID > TPairNodeIDs
A pair of node IDs.
Definition: TNodeID.h:18
Levenberg-Marquardt non-linear graph slam optimization scheme.
Definition: CLevMarqGSO.h:117
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:75
void initializeVisuals() override
Initialize visual objects in CDisplayWindow (e.g.
void fitGraphInView()
Set the camera parameters of the CDisplayWindow3D so that the whole graph is viewed in the window...
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.
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1807
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
void notifyOfWindowEvents(const std::map< std::string, bool > &events_occurred) override
Get a list of the window events that happened since the last call.
void _optimizeGraph(bool is_full_update=false)
Optimize the given graph.



Page generated by Doxygen 1.8.14 for MRPT 2.0.2 Git: 9efc2a654 Mon Apr 6 11:24:47 2020 +0200 at lun abr 6 11:30:12 CEST 2020