MRPT  1.9.9
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-2019, 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 {
28  MRPT_START;
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 {
62  MRPT_START;
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 {
75  MRPT_START;
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 {
92  MRPT_START;
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_UNUSED_PARAM(elapsed_time);
462  MRPT_END;
463 } // end of _optimizeGraph
464 
465 template <class GRAPH_T>
467 {
468  MRPT_START;
469 
470  bool is_loop_closure = false;
471  auto curr_pair_nodes_to_edge = this->m_graph->edges;
472 
473  // find the *node pairs* that exist in current but not the last
474  // nodes_to_edge
475  // map If the distance of any of these pairs is greater than
476  // LC_min_nodeid_diff then consider this a loop closure
477  typename GRAPH_T::edges_map_t::const_iterator search;
478  mrpt::graphs::TPairNodeIDs curr_pair;
479 
480  for (auto it = curr_pair_nodes_to_edge.begin();
481  it != curr_pair_nodes_to_edge.end(); ++it)
482  {
483  search = opt_params.last_pair_nodes_to_edge.find(it->first);
484  // if current node pair is not found in the last set...
485  if (search == opt_params.last_pair_nodes_to_edge.end())
486  {
487  curr_pair = it->first;
488 
489  if (std::abs(
490  static_cast<int>(curr_pair.first) -
491  static_cast<int>(curr_pair.second)) >
492  opt_params.LC_min_nodeid_diff)
493  {
494  this->logFmt(
495  mrpt::system::LVL_DEBUG, "Registering loop closure... ");
496  is_loop_closure = true;
497  break; // no need for more iterations
498  }
499  }
500  }
501 
502  // update the pair_nodes_to_edge map
503  opt_params.last_pair_nodes_to_edge = curr_pair_nodes_to_edge;
504  return is_loop_closure;
505 
506  MRPT_END;
507 }
508 
509 template <class GRAPH_T>
511 {
512  bool is_full_update = false;
513 
514  if (opt_params.optimization_distance == -1)
515  { // always optimize fully
516  return true;
517  }
518 
519  bool added_lc = this->checkForLoopClosures();
520 
521  // Decide on the LoopClosingAttitude I am in
522  if (!added_lc)
523  { // reset both ignored and used counters
524  if (m_curr_used_consec_lcs != 0 || m_curr_ignored_consec_lcs != 0)
525  {
526  MRPT_LOG_DEBUG_STREAM("No new Loop Closure found.");
527  }
528 
529  m_curr_used_consec_lcs = 0;
530  m_curr_ignored_consec_lcs = 0;
531  m_optimization_policy = OptimizationPolicy::UseLoopClosures;
532 
533  return is_full_update;
534  }
535  else
536  { // lc found.
537  // have I used enough consecutive loop closures?
538  bool use_limit_reached =
539  m_curr_used_consec_lcs == m_max_used_consec_lcs;
540  // have I ignored enough consecutive loop closures?
541  bool ignore_limit_reached =
542  m_curr_ignored_consec_lcs == m_max_ignored_consec_lcs;
543 
544  // Have I reached any of the limits above?
545  if (ignore_limit_reached || use_limit_reached)
546  {
547  m_curr_ignored_consec_lcs = 0;
548  m_curr_used_consec_lcs = 0;
549 
550  // decide of the my policy on full optimization
551  if (ignore_limit_reached)
552  {
553  m_optimization_policy = OptimizationPolicy::UseLoopClosures;
554  }
555  if (use_limit_reached)
556  {
557  m_optimization_policy = OptimizationPolicy::IgnoreLoopClosures;
558  }
559  }
560  else
561  { // no limits reached yet.
562  if (m_optimization_policy == OptimizationPolicy::UseLoopClosures)
563  {
564  m_curr_used_consec_lcs += 1;
565  }
566  else
567  {
568  m_curr_ignored_consec_lcs += 1;
569  }
570  }
571  }
572 
573  // Decide on whether to fully optimize the graph based on the mode I am in
574  if (m_optimization_policy == OptimizationPolicy::IgnoreLoopClosures)
575  {
576  is_full_update = false;
578  "*PARTIAL* graph optimization.. ignoring new loop closure");
579  }
580  else
581  {
582  is_full_update = true;
583  MRPT_LOG_WARN_STREAM("Commencing with *FULL* graph optimization... ");
584  }
585  return is_full_update;
586 
587 } // end of checkForFullOptimization
588 
589 template <class GRAPH_T>
591 {
592  return m_just_fully_optimized_graph;
593 }
594 
595 template <class GRAPH_T>
597  const GRAPH_T& graph, const size_t iter, const size_t max_iter,
598  const double cur_sq_error)
599 {
600 }
601 
602 template <class GRAPH_T>
604  std::set<mrpt::graphs::TNodeID>* nodes_set,
605  const mrpt::graphs::TNodeID& cur_nodeID, double distance)
606 {
607  MRPT_START;
608 
609  if (distance > 0)
610  {
611  // check all but the last node.
612  for (mrpt::graphs::TNodeID nodeID = 0;
613  nodeID < this->m_graph->nodeCount() - 1; ++nodeID)
614  {
615  double curr_distance = this->m_graph->nodes[nodeID].distanceTo(
616  this->m_graph->nodes[cur_nodeID]);
617  if (curr_distance <= distance)
618  {
619  nodes_set->insert(nodeID);
620  }
621  }
622  }
623  else
624  { // check against all nodes
625  this->m_graph->getAllNodes(*nodes_set);
626  }
627 
628  MRPT_END;
629 }
630 
631 template <class GRAPH_T>
633 {
634  parent::printParams();
635 
636  opt_params.dumpToConsole();
637  viz_params.dumpToConsole();
638 }
639 template <class GRAPH_T>
641 {
642  MRPT_START;
643  parent::loadParams(source_fname);
644 
645  opt_params.loadFromConfigFileName(source_fname, "OptimizerParameters");
646  viz_params.loadFromConfigFileName(source_fname, "VisualizationParameters");
647 
648  mrpt::config::CConfigFile source(source_fname);
649 
650  // TODO - check that these work
651  m_max_used_consec_lcs = source.read_int(
652  "OptimizerParameters", "max_used_consecutive_loop_closures", 2, false);
653 
654  m_max_ignored_consec_lcs = source.read_int(
655  "OptimizerParameters", "max_ignored_consecutive_loop_closures", 15,
656  false);
657 
658  // set the logging level if given by the user
659  // Minimum verbosity level of the logger
660  int min_verbosity_level =
661  source.read_int("OptimizerParameters", "class_verbosity", 1, false);
662  this->setMinLoggingLevel(mrpt::system::VerbosityLevel(min_verbosity_level));
663 
664  MRPT_LOG_DEBUG("Successfully loaded Params. ");
665  m_has_read_config = true;
666 
667  MRPT_END;
668 }
669 
670 template <class GRAPH_T>
672 {
673  MRPT_START;
674  using namespace std;
675 
676  const std::string report_sep(2, '\n');
677  const std::string header_sep(80, '#');
678 
679  // Report on graph
680  stringstream class_props_ss;
681  class_props_ss << "Levenberg Marquardt Optimization Summary: " << std::endl;
682  class_props_ss << header_sep << std::endl;
683 
684  // time and output logging
685  const std::string time_res = this->m_time_logger.getStatsAsText();
686  const std::string output_res = this->getLogAsString();
687 
688  // merge the individual reports
689  report_str->clear();
690  parent::getDescriptiveReport(report_str);
691 
692  *report_str += class_props_ss.str();
693  *report_str += report_sep;
694 
695  *report_str += time_res;
696  *report_str += report_sep;
697 
698  *report_str += output_res;
699  *report_str += report_sep;
700 
701  MRPT_END;
702 }
703 
704 template <class GRAPH_T>
706  : optimization_distance_color(0, 201, 87),
707  keystroke_optimization_distance("u"),
708  keystroke_optimize_graph("w")
709 {
710 }
711 template <class GRAPH_T>
713 template <class GRAPH_T>
715  std::ostream& out) const
716 {
717  MRPT_START;
718  out << "-----------[ Levenberg-Marquardt Optimization ] -------\n";
719  out << "Optimization on second thread = "
720  << (optimization_on_second_thread ? "TRUE" : "FALSE") << std::endl;
721  out << "Optimize nodes in distance = " << optimization_distance << "\n";
722  out << "Min. node difference for LC = " << LC_min_nodeid_diff << "\n";
723  out << cfg.getAsString() << std::endl;
724  MRPT_END;
725 }
726 template <class GRAPH_T>
728  const mrpt::config::CConfigFileBase& source, const std::string& section)
729 {
730  MRPT_START;
731  optimization_on_second_thread = source.read_bool(
732  section, "optimization_on_second_thread", false, false);
733  LC_min_nodeid_diff = source.read_int(
734  "GeneralConfiguration", "LC_min_nodeid_diff", 30, false);
735  optimization_distance =
736  source.read_double(section, "optimization_distance", 5, false);
737  // asert the previous value
739  optimization_distance == 1 || optimization_distance > 0,
740  format(
741  "Invalid value for optimization distance: %.2f",
742  optimization_distance));
743 
744  // optimization parameters
745  cfg["verbose"] = source.read_bool(section, "verbose", false, false);
746  cfg["profiler"] = source.read_bool(section, "profiler", false, false);
747  cfg["max_iterations"] =
748  source.read_double(section, "max_iterations", 100, false);
749  cfg["scale_hessian"] =
750  source.read_double("Optimization", "scale_hessian", 0.2, false);
751  cfg["tau"] = source.read_double(section, "tau", 1e-3, false);
752 
753  MRPT_END;
754 }
755 
756 template <class GRAPH_T>
758  : keystroke_graph_toggle("s"), keystroke_graph_autofit("a")
759 {
760 }
761 template <class GRAPH_T>
763  default;
764 template <class GRAPH_T>
766  std::ostream& out) const
767 {
768  MRPT_START;
769 
770  out << "-----------[ Graph Visualization Parameters ]-----------\n";
771  out << mrpt::format(
772  "Visualize optimized graph = %s\n",
773  visualize_optimized_graph ? "TRUE" : "FALSE");
774 
775  out << mrpt::format("%s", cfg.getAsString().c_str());
776 
777  std::cout << std::endl;
778 
779  MRPT_END;
780 }
781 template <class GRAPH_T>
783  const mrpt::config::CConfigFileBase& source, const std::string& section)
784 {
785  MRPT_START;
786 
787  visualize_optimized_graph =
788  source.read_bool(section, "visualize_optimized_graph", true, false);
789 
790  cfg["show_ID_labels"] =
791  source.read_bool(section, "optimized_show_ID_labels", false, false);
792  cfg["show_ground_grid"] =
793  source.read_double(section, "optimized_show_ground_grid", 1, false);
794  cfg["show_edges"] =
795  source.read_bool(section, "optimized_show_edges", true, false);
796  cfg["edge_color"] =
797  source.read_int(section, "optimized_edge_color", 1500, false);
798  cfg["edge_width"] =
799  source.read_double(section, "optimized_edge_width", 1.5, false);
800  cfg["show_node_corners"] =
801  source.read_bool(section, "optimized_show_node_corners", true, false);
802  cfg["show_edge_rel_poses"] =
803  source.read_bool(section, "optimized_show_edge_rel_poses", true, false);
804  cfg["edge_rel_poses_color"] =
805  source.read_int(section, "optimized_edge_rel_poses_color", 2000, false);
806  cfg["nodes_edges_corner_scale"] = source.read_double(
807  section, "optimized_nodes_edges_corner_scale", 0.4, false);
808  cfg["nodes_corner_scale"] =
809  source.read_double(section, "optimized_nodes_corner_scale", 0.7, false);
810  cfg["nodes_point_size"] =
811  source.read_int(section, "optimized_nodes_point_size", 5, false);
812  cfg["nodes_point_color"] =
813  source.read_int(section, "optimized_nodes_point_color", 3000, false);
814 
815  MRPT_END;
816 }
817 } // 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...
GLsizei GLsizei GLuint * obj
Definition: glext.h:4085
#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
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...
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);
GLsizei const GLchar ** string
Definition: glext.h:4116
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
#define MRPT_END
Definition: exceptions.h:245
A RGB color - floats in the range [0,1].
Definition: TColor.h:78
Output information for mrpt::graphslam::optimize_graph_spa_levmarq()
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
GLsizei GLsizei GLchar * source
Definition: glext.h:4097
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
GLenum GLsizei GLenum format
Definition: glext.h:3535
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.
GLfloat GLfloat p
Definition: glext.h:6398
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:1891
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.
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 9b18308f3 Mon Nov 18 23:39:25 2019 +0100 at lun nov 18 23:45:12 CET 2019