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