Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 #ifndef CLEVMARQGSO_IMPL_H
11 #define CLEVMARQGSO_IMPL_H
12 
13 namespace mrpt { namespace graphslam { namespace optimizers {
14 
15 // Ctors, Dtors
16 //////////////////////////////////////////////////////////////
17 
18 template<class GRAPH_T>
20  m_first_time_call(false),
21  m_has_read_config(false),
22  m_autozoom_active(true),
23  m_last_total_num_of_nodes(5),
24  m_optimization_policy(FOP_USE_LC),
25  m_curr_used_consec_lcs(0),
26  m_curr_ignored_consec_lcs(0),
27  m_just_fully_optimized_graph(false),
28  m_min_nodes_for_optimization(3)
29 {
30  MRPT_START;
31  using namespace mrpt::utils;
32  this->initializeLoggers("CLevMarqGSO");
33 
34  MRPT_END;
35 }
36 template<class GRAPH_T>
38  // JL Removed MRPT_END since it could launch an exception, not allowed in a dtor
39 }
40 
41 // Member function implementations
42 //////////////////////////////////////////////////////////////
43 template<class GRAPH_T>
45  mrpt::obs::CActionCollectionPtr action,
46  mrpt::obs::CSensoryFramePtr observations,
47  mrpt::obs::CObservationPtr observation ) {
48  MRPT_START;
49  using namespace mrpt::utils;
50  this->logFmt(LVL_DEBUG, "In updateOptimizerState... ");
51 
52  if (this->m_graph->nodeCount() > m_last_total_num_of_nodes) {
53  m_last_total_num_of_nodes = this->m_graph->nodeCount();
54  registered_new_node = true;
55 
56  if (m_first_time_call) {
58  m_first_time_call = true;
59  }
60 
61 
63  //join the previous optimization thread
65 
66  // optimize the graph - run on a seperate thread
68  /*obj = */ this,
69  /* func = */ &CLevMarqGSO::optimizeGraph);
70 
71  }
72  else { // single threaded implementation
73  bool is_full_update = this->checkForFullOptimization();
74  this->_optimizeGraph(is_full_update);
75  }
76 
77  }
78 
79  return true;
80  MRPT_END;
81 }
82 
83 template<class GRAPH_T>
85  MRPT_START;
88 
89  this->initGraphVisualization();
91 
92  MRPT_END;
93 }
94 
95 template<class GRAPH_T>
97  MRPT_START;
99 
102  }
103 
104  this->updateGraphVisualization();
105 
106  MRPT_END;
107 }
108 
109 template<class GRAPH_T>
111  const std::map<std::string, bool>& events_occurred) {
112  MRPT_START;
113  using namespace std;
114  parent::notifyOfWindowEvents(events_occurred);
115 
116  // I know the keys exists - I registered them explicitly
117 
118  // optimization_distance toggling
120  if (events_occurred.find(
123  }
124 
125  if (events_occurred.find(opt_params.keystroke_optimize_graph)->second) {
126  this->_optimizeGraph(/*is_full_update=*/ true);
127  }
128  }
129 
130  // graph toggling
131  if (events_occurred.find(viz_params.keystroke_graph_toggle)->second) {
132  this->toggleGraphVisualization();
133  }
134 
135 
136  // if mouse event, let the user decide about the camera
137  if (events_occurred.find("mouse_clicked")->second) {
138  MRPT_LOG_DEBUG_STREAM("Mouse was clicked. Disabling autozoom.");
139  m_autozoom_active = false;
140  }
141 
142  // autofit the graph once
143  if (events_occurred.find(viz_params.keystroke_graph_autofit)->second) {
144  MRPT_LOG_DEBUG_STREAM("Autofit button was pressed");
145  this->fitGraphInView();
146  }
147 
148 
149  MRPT_END;
150 } // end of notifyOfWindowEvents
151 
152 template<class GRAPH_T>
154  MRPT_START;
155  ASSERTMSG_(this->m_win_manager, "No CWindowManager* is given");
156 
160  "Toggle Graph visualization");
163  "Fit Graph in view");
164 
166  /* offset_y* = */ &viz_params.offset_y_graph,
167  /* text_index* = */ &viz_params.text_index_graph );
168  }
169 
170  MRPT_END;
171 }
172 template<class GRAPH_T>
174  MRPT_START;
175  ASSERTMSG_(this->m_win_manager, "No CWindowManager* is given");
176  using namespace mrpt::opengl;
177  using namespace mrpt::utils;
178 
179  this->logFmt(mrpt::utils::LVL_DEBUG, "In the updateGraphVisualization function");
180 
181  // update the graph (clear and rewrite..)
182  COpenGLScenePtr& scene = this->m_win->get3DSceneAndLock();
183 
184  // remove previous graph and insert the new instance
185  // TODO - make this an incremental proocecure
186  CRenderizablePtr prev_object = scene->getByName("optimized_graph");
187  bool prev_visibility = true;
188  if (prev_object) { // set the visibility of the graph correctly
189  prev_visibility = prev_object->isVisible();
190  }
191  scene->removeObject(prev_object);
192 
193  //CSetOfObjectsPtr graph_obj =
194  //graph_tools::graph_visualize(*this->m_graph, viz_params.cfg);
195  CSetOfObjectsPtr graph_obj = CSetOfObjects::Create();
196  this->m_graph->getAs3DObject(graph_obj, viz_params.cfg);
197 
198  graph_obj->setName("optimized_graph");
199  graph_obj->setVisibility(prev_visibility);
200  scene->insert(graph_obj);
201  this->m_win->unlockAccess3DScene();
202 
204  format("Optimized Graph: #nodes %d",
205  static_cast<int>(this->m_graph->nodeCount())),
206  TColorf(0.0, 0.0, 0.0),
207  /* unique_index = */ viz_params.text_index_graph);
208 
209  this->m_win->forceRepaint();
210 
211  if (m_autozoom_active) {
212  this->fitGraphInView();
213  }
214 
215  MRPT_END;
216 }
217 
218 template<class GRAPH_T>
220  MRPT_START;
221  using namespace mrpt::opengl;
222 
223  COpenGLScenePtr& scene = this->m_win->get3DSceneAndLock();
224 
225  CRenderizablePtr graph_obj = scene->getByName("optimized_graph");
226  graph_obj->setVisibility(!graph_obj->isVisible());
227 
228  this->m_win->unlockAccess3DScene();
229  this->m_win->forceRepaint();
230 
231  MRPT_END;
232 }
233 
234 template<class GRAPH_T>
236  MRPT_START;
237  using namespace mrpt::opengl;
238 
239  ASSERTMSG_(this->m_win,
240  "\nVisualization of data was requested but no CDisplayWindow3D pointer was given\n");
241 
242  // first fetch the graph object
243  COpenGLScenePtr& scene = this->m_win->get3DSceneAndLock();
244  CRenderizablePtr obj = scene->getByName("optimized_graph");
245  CSetOfObjectsPtr graph_obj = static_cast<CSetOfObjectsPtr>(obj);
246  this->m_win->unlockAccess3DScene();
247  this->m_win->forceRepaint();
248 
249  // autofit it based on its grid
250  CGridPlaneXYPtr obj_grid =
251  graph_obj->CSetOfObjects::getByClass<CGridPlaneXY>();
252  if (obj_grid) {
253  float x_min,x_max, y_min,y_max;
254  obj_grid->getPlaneLimits(x_min,x_max, y_min,y_max);
255  const float z_min = obj_grid->getPlaneZcoord();
257  0.5*(x_min+x_max), 0.5*(y_min+y_max), z_min);
258  this->m_win->setCameraZoom(
259  2.0f * std::max(10.0f, std::max(x_max-x_min, y_max-y_min)));
260  }
261  this->m_win->setCameraAzimuthDeg(60);
262  this->m_win->setCameraElevationDeg(75);
263  this->m_win->setCameraProjective(true);
264 
265  MRPT_END;
266 }
267 
268 template<class GRAPH_T>
270  MRPT_START;
271  using namespace mrpt::opengl;
272 
276  "Toggle optimization distance on/off");
277 
280  "Manually trigger a full graph optimization");
281  }
282 
283  pose_t p;
284  CRenderizablePtr obj = this->initOptDistanceVisualizationInternal(p);
285  pose_t initial_pose;
286  obj->setPose(initial_pose);
287  obj->setName("optimization_distance_obj");
288 
289  COpenGLScenePtr scene = this->m_win->get3DSceneAndLock();
290  scene->insert(obj);
291  this->m_win->unlockAccess3DScene();
292  this->m_win->forceRepaint();
293 
294  // optimization distance disk - textMessage
298 
301  format("Radius for graph optimization"),
303  /* unique_index = */ opt_params.text_index_optimization_distance );
304  MRPT_END;
305 }
306 
307 template<class GRAPH_T>
308 mrpt::opengl::CRenderizablePtr CLevMarqGSO<GRAPH_T>::
310  const mrpt::poses::CPose2D& p_unused) {
311  using namespace mrpt::opengl;
312 
313  CDiskPtr obj = CDisk::Create();
314  obj->setDiskRadius(
318 
319  return obj;
320 }
321 template<class GRAPH_T>
322 mrpt::opengl::CRenderizablePtr CLevMarqGSO<GRAPH_T>::
324  const mrpt::poses::CPose3D& p_unused) {
325  using namespace mrpt::opengl;
326 
327  CSpherePtr obj = CSphere::Create();
329  obj->setColor_u8(
333  /*alpha = */ 60);
334 
335  return obj;
336 }
337 
338 template<class GRAPH_T>
340  MRPT_START;
341  ASSERTMSG_(this->m_win_manager, "No CWindowManager* is given");
342  using namespace mrpt::opengl;
343 
344  // update ICP_max_distance Disk
345  COpenGLScenePtr scene = this->m_win->get3DSceneAndLock();
346 
347  CRenderizablePtr 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  MRPT_START;
359  using namespace mrpt::opengl;
360 
361  COpenGLScenePtr scene = this->m_win->get3DSceneAndLock();
362 
363  CRenderizablePtr obj = scene->getByName("optimization_distance_obj");
364  obj->setVisibility(!obj->isVisible());
365 
366  this->m_win->unlockAccess3DScene();
367  this->m_win->forceRepaint();
368 
369  MRPT_END;
370 }
371 
372 
373 
374 template<class GRAPH_T>
376  MRPT_START;
377 
378  this->logFmt(mrpt::utils::LVL_DEBUG,
379  "In optimizeGraph\n\tThreadID: %lu\n\tTrying to grab lock... ",
381 
383  this->_optimizeGraph();
384 
385  this->logFmt(mrpt::utils::LVL_DEBUG, "2nd thread grabbed the lock..");
386 
387  MRPT_END;
388 }
389 
390 template<class GRAPH_T>
391 void CLevMarqGSO<GRAPH_T>::_optimizeGraph(bool is_full_update/*=false*/) {
392  MRPT_START;
393  this->m_time_logger.enter("CLevMarqGSO::_optimizeGraph");
394 
395  using namespace mrpt::utils;
396 
397  // if less than X nodes exist overall, do not try optimizing
398  if (m_min_nodes_for_optimization > this->m_graph->nodes.size()) {
399  return;
400  }
401 
402  CTicTac optimization_timer;
403  optimization_timer.Tic();
404 
405  // set of nodes for which the optimization procedure will take place
406  std::set< mrpt::utils::TNodeID>* nodes_to_optimize;
407 
408  // fill in the nodes in certain distance to the current node, only if
409  // is_full_update is not instructed
410  if (is_full_update) {
411  // nodes_to_optimize: List of nodes to optimize. NULL -> all but the root
412  // node.
413  nodes_to_optimize = NULL;
414  }
415  else {
416  nodes_to_optimize = new std::set<mrpt::utils::TNodeID>;
417 
418  // I am certain that this shall not be called when nodeCount = 0, since the
419  // optimization procedure starts only after certain number of nodes has
420  // been added
421  this->getNearbyNodesOf(nodes_to_optimize,
422  this->m_graph->nodeCount()-1,
424  nodes_to_optimize->insert(this->m_graph->nodeCount()-1);
425  }
426 
428 
429  // Execute the optimization
431  *(this->m_graph),
432  levmarq_info,
433  nodes_to_optimize,
434  opt_params.cfg,
435  &CLevMarqGSO<GRAPH_T>::levMarqFeedback); // functor feedback
436 
437  if (is_full_update) {
439  }
440  else {
442  }
443 
444 
445  double elapsed_time = optimization_timer.Tac();
446  this->logFmt(mrpt::utils::LVL_DEBUG,
447  "Optimization of graph took: %fs", elapsed_time);
448 
449  // deleting the nodes_to_optimize set
450  delete nodes_to_optimize;
451  nodes_to_optimize = NULL;
452 
453  this->m_time_logger.leave("CLevMarqGSO::_optimizeGraph");
454  MRPT_UNUSED_PARAM(elapsed_time);
455  MRPT_END;
456 } // end of _optimizeGraph
457 
458 template<class GRAPH_T>
460  MRPT_START;
461 
462  bool is_loop_closure = false;
463  typename GRAPH_T::edges_map_t curr_pair_nodes_to_edge = this->m_graph->edges;
464 
465  // find the *node pairs* that exist in current but not the last nodes_to_edge
466  // map If the distance of any of these pairs is greater than
467  // LC_min_nodeid_diff then consider this a loop closure
468  typename GRAPH_T::edges_map_t::const_iterator search;
469  mrpt::utils::TPairNodeIDs curr_pair;
470 
471  for (typename GRAPH_T::edges_map_t::const_iterator it =
472  curr_pair_nodes_to_edge.begin(); it != curr_pair_nodes_to_edge.end();
473  ++it) {
474  search = opt_params.last_pair_nodes_to_edge.find(it->first);
475  // if current node pair is not found in the last set...
476  if (search == opt_params.last_pair_nodes_to_edge.end()) {
477  curr_pair = it->first;
478 
479  if (std::abs(
480  static_cast<int>(curr_pair.first) -
481  static_cast<int>(curr_pair.second) ) >
483 
484  this->logFmt(mrpt::utils::LVL_DEBUG, "Registering loop closure... ");
485  is_loop_closure = true;
486  break; // no need for more iterations
487  }
488  }
489  }
490 
491  // update the pair_nodes_to_edge map
492  opt_params.last_pair_nodes_to_edge = curr_pair_nodes_to_edge;
493  return is_loop_closure;
494 
495  MRPT_END;
496 }
497 
498 template<class GRAPH_T>
500  bool is_full_update = false;
501 
502  if (opt_params.optimization_distance == -1) { // always optimize fully
503  return true;
504  }
505 
506  bool added_lc = this->checkForLoopClosures();
507 
508  // Decide on the LoopClosingAttitude I am in
509  if (!added_lc) { // reset both ignored and used counters
511  MRPT_LOG_DEBUG_STREAM("No new Loop Closure found.");
512  }
513 
517 
518  return is_full_update;
519  }
520  else { // lc found.
521  // have I used enough consecutive loop closures?
522  bool use_limit_reached =
524  // have I ignored enough consecutive loop closures?
525  bool ignore_limit_reached =
527 
528  // Have I reached any of the limits above?
529  if (ignore_limit_reached || use_limit_reached) {
532 
533  // decide of the my policy on full optimization
534  if (ignore_limit_reached) {
536  }
537  if (use_limit_reached) {
539  }
540  }
541  else { // no limits reached yet.
544  }
545  else {
547  }
548  }
549  }
550 
551  // Decide on whether to fully optimize the graph based on the mode I am in
553  is_full_update = false;
555  "*PARTIAL* graph optimization.. ignoring new loop closure");
556  }
557  else {
558  is_full_update = true;
559  MRPT_LOG_WARN_STREAM("Commencing with *FULL* graph optimization... ");
560  }
561  return is_full_update;
562 
563 } // end of checkForFullOptimization
564 
565 template<class GRAPH_T>
568 }
569 
570 
571 template<class GRAPH_T>
573  const GRAPH_T &graph,
574  const size_t iter,
575  const size_t max_iter,
576  const double cur_sq_error )
577 { }
578 
579 template<class GRAPH_T>
581  std::set<mrpt::utils::TNodeID> *nodes_set,
582  const mrpt::utils::TNodeID& cur_nodeID,
583  double distance ) {
584  MRPT_START;
585 
586  if (distance > 0) {
587  // check all but the last node.
588  for (mrpt::utils::TNodeID nodeID = 0;
589  nodeID < this->m_graph->nodeCount()-1;
590  ++nodeID) {
591  double curr_distance = this->m_graph->nodes[nodeID].distanceTo(
592  this->m_graph->nodes[cur_nodeID]);
593  if (curr_distance <= distance) {
594  nodes_set->insert(nodeID);
595  }
596  }
597  }
598  else { // check against all nodes
599  this->m_graph->getAllNodes(*nodes_set);
600  }
601 
602  MRPT_END;
603 }
604 
605 template<class GRAPH_T>
608 
611 }
612 template<class GRAPH_T>
614  MRPT_START;
615  using namespace mrpt::utils;
616  parent::loadParams(source_fname);
617 
618  opt_params.loadFromConfigFileName(source_fname, "OptimizerParameters");
619  viz_params.loadFromConfigFileName(source_fname, "VisualizationParameters");
620 
621  CConfigFile source(source_fname);
622 
623  // TODO - check that these work
624  m_max_used_consec_lcs = source.read_int(
625  "OptimizerParameters",
626  "max_used_consecutive_loop_closures",
627  2, false);
628 
629  m_max_ignored_consec_lcs = source.read_int(
630  "OptimizerParameters",
631  "max_ignored_consecutive_loop_closures",
632  15, false);
633 
634  // set the logging level if given by the user
635  // Minimum verbosity level of the logger
636  int min_verbosity_level = source.read_int(
637  "OptimizerParameters",
638  "class_verbosity",
639  1, false);
640  this->setMinLoggingLevel(VerbosityLevel(min_verbosity_level));
641 
642  this->logFmt(mrpt::utils::LVL_DEBUG, "Successfully loaded Params. ");
643  m_has_read_config = true;
644 
645  MRPT_END;
646 }
647 
648 template<class GRAPH_T>
650  MRPT_START;
651  using namespace std;
652 
653  const std::string report_sep(2, '\n');
654  const std::string header_sep(80, '#');
655 
656  // Report on graph
657  stringstream class_props_ss;
658  class_props_ss << "Levenberg Marquardt Optimization Summary: " << std::endl;
659  class_props_ss << header_sep << std::endl;
660 
661  // time and output logging
662  const std::string time_res = this->m_time_logger.getStatsAsText();
663  const std::string output_res = this->getLogAsString();
664 
665  // merge the individual reports
666  report_str->clear();
667  parent::getDescriptiveReport(report_str);
668 
669  *report_str += class_props_ss.str();
670  *report_str += report_sep;
671 
672  *report_str += time_res;
673  *report_str += report_sep;
674 
675  *report_str += output_res;
676  *report_str += report_sep;
677 
678  MRPT_END;
679 }
680 
681 
682 // OptimizationParams
683 //////////////////////////////////////////////////////////////
684 template<class GRAPH_T>
686  optimization_distance_color(0, 201, 87),
687  keystroke_optimization_distance("u"),
688  keystroke_optimize_graph("w")
689 { }
690 template<class GRAPH_T>
692 }
693 template<class GRAPH_T>
695  mrpt::utils::CStream &out) const {
696  MRPT_START;
697 
698  out.printf("------------------[ Levenberg-Marquardt Optimization ]------------------\n");
699  out.printf("Optimization on second thread = %s\n",
700  optimization_on_second_thread ? "TRUE" : "FALSE");
701  out.printf("Optimize nodes in distance = %.2f\n", optimization_distance);
702  out.printf("Min. node difference for LC = %d\n", LC_min_nodeid_diff);
703 
704  out.printf("%s", cfg.getAsString().c_str());
705  std::cout << std::endl;
706 
707  MRPT_END;
708 }
709 template<class GRAPH_T>
712  const std::string &section) {
713  MRPT_START;
714  optimization_on_second_thread = source.read_bool(
715  section,
716  "optimization_on_second_thread",
717  false, false);
718  LC_min_nodeid_diff = source.read_int(
719  "GeneralConfiguration",
720  "LC_min_nodeid_diff",
721  30, false);
722  optimization_distance = source.read_double(
723  section,
724  "optimization_distance",
725  5, false);
726  // asert the previous value
727  ASSERTMSG_(optimization_distance == -1 ||
728  optimization_distance > 0,
729  format("Invalid value for optimization distance: %.2f",
730  optimization_distance) );
731 
732  // optimization parameters
733  cfg["verbose"] = source.read_bool(
734  section,
735  "verbose",
736  0, false);
737  cfg["profiler"] = source.read_bool(
738  section,
739  "profiler",
740  0, false);
741  cfg["max_iterations"] = source.read_double(
742  section,
743  "max_iterations",
744  100, false);
745  cfg["scale_hessian"] = source.read_double(
746  "Optimization",
747  "scale_hessian",
748  0.2, false);
749  cfg["tau"] = source.read_double(
750  section,
751  "tau",
752  1e-3, false);
753 
754  MRPT_END;
755 }
756 
757 // GraphVisualizationParams
758 //////////////////////////////////////////////////////////////
759 template<class GRAPH_T>
761  keystroke_graph_toggle("s"),
762  keystroke_graph_autofit("a")
763 {
764 }
765 template<class GRAPH_T>
767 }
768 template<class GRAPH_T>
770  mrpt::utils::CStream &out) const {
771  MRPT_START;
772 
773  out.printf("-----------[ Graph Visualization Parameters ]-----------\n");
774  out.printf("Visualize optimized graph = %s\n",
775  visualize_optimized_graph ? "TRUE" : "FALSE");
776 
777  out.printf("%s", cfg.getAsString().c_str());
778 
779  std::cout << std::endl;
780 
781  MRPT_END;
782 }
783 template<class GRAPH_T>
786  const std::string &section) {
787  MRPT_START;
788  using namespace utils;
789 
790  visualize_optimized_graph = source.read_bool(
791  section,
792  "visualize_optimized_graph",
793  1, false);
794 
795  cfg["show_ID_labels"] = source.read_bool(
796  section,
797  "optimized_show_ID_labels",
798  0, false);
799  cfg["show_ground_grid"] = source.read_double(
800  section,
801  "optimized_show_ground_grid",
802  1, false);
803  cfg["show_edges"] = source.read_bool(
804  section,
805  "optimized_show_edges",
806  1, false);
807  cfg["edge_color"] = source.read_int(
808  section,
809  "optimized_edge_color",
810  4286611456, false);
811  cfg["edge_width"] = source.read_double(
812  section,
813  "optimized_edge_width",
814  1.5, false);
815  cfg["show_node_corners"] = source.read_bool(
816  section,
817  "optimized_show_node_corners",
818  1, false);
819  cfg["show_edge_rel_poses"] = source.read_bool(
820  section,
821  "optimized_show_edge_rel_poses",
822  1, false);
823  cfg["edge_rel_poses_color"] = source.read_int(
824  section,
825  "optimized_edge_rel_poses_color",
826  1090486272, false);
827  cfg["nodes_edges_corner_scale"] = source.read_double(
828  section,
829  "optimized_nodes_edges_corner_scale",
830  0.4, false);
831  cfg["nodes_corner_scale"] = source.read_double(
832  section,
833  "optimized_nodes_corner_scale",
834  0.7, false);
835  cfg["nodes_point_size"] = source.read_int(
836  section,
837  "optimized_nodes_point_size",
838  5, false);
839  cfg["nodes_point_color"] = source.read_int(
840  section,
841  "optimized_nodes_point_color",
842  10526880, false);
843 
844  MRPT_END;
845 }
846 
847 } } } // end of namespaces
848 
849 #endif /* end of include guard: CLEVMARQGSO_IMPL_H */
void optimize_graph_spa_levmarq(GRAPH_T &graph, TResultInfoSpaLevMarq &out_info, const std::set< mrpt::utils::TNodeID > *in_nodes_to_optimize=NULL, const mrpt::utils::TParametersDouble &extra_params=mrpt::utils::TParametersDouble(), typename graphslam_traits< GRAPH_T >::TFunctorFeedback functor_feedback=NULL)
Optimize a graph of pose constraints using the Sparse Pose Adjustment (SPA) sparse representation and...
Definition: levmarq.h:56
void initGraphVisualization()
Initialize objects relateed to the Graph Visualization.
virtual void notifyOfWindowEvents(const std::map< std::string, bool > &events_occurred)
Get a list of the window events that happened since the last call.
void unlockAccess3DScene()
Unlocks the access to the internal 3D scene.
A class acquiring a CCriticalSection at its constructor, and releasing it at destructor.
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.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
bool checkForFullOptimization()
Decide whether to issue a full graph optimization.
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section)
This method load the options from a ".ini"-like file or memory-stored string list.
mrpt::gui::CDisplayWindow3D * m_win
Window to use.
unsigned long BASE_IMPEXP getCurrentThreadId() MRPT_NO_THROWS
Returns the ID of the current thread.
Definition: threads.cpp:211
A grid of lines over the XY plane.
Definition: CGridPlaneXY.h:35
virtual void loadParams(const std::string &source_fname)
Load the necessary for the decider/optimizer configuration parameters.
void updateGraphVisualization()
Called internally for updating the visualization scene for the graph building procedure.
void toggleGraphVisualization()
Toggle the graph visualization on and off.
void optimizeGraph()
Wrapper around _optimizeGraph which first locks the section and then calls the _optimizeGraph method...
virtual void printParams() const
Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact w...
This class allows loading and storing values and vectors of different types from ".ini" files easily.
Definition: CConfigFile.h:30
void setCameraPointingToPoint(float x, float y, float z)
Changes the camera parameters programmatically.
static const std::string header_sep
Separator string to be used in debugging messages.
bool m_just_fully_optimized_graph
Indicates whether a full graph optimization was just issued.
Definition: CLevMarqGSO.h:399
mrpt::graphslam::CWindowObserver * m_win_observer
CWindowObserver object for monitoring various visual-oriented events.
STL namespace.
bool justFullyOptimizedGraph() const
Used by the caller to query for possible full graph optimization on the latest optimizer run...
const Scalar * const_iterator
Definition: eigen_plugins.h:24
void setCameraProjective(bool isProjective)
Sets the camera as projective, or orthogonal.
void registerKeystroke(const std::string key_str, const std::string key_desc)
Make new keystrokes available in the help message box.
void addTextMessage(const double x, const double y, const std::string &text, const mrpt::utils::TColorf &color=mrpt::utils::TColorf(1.0, 1.0, 1.0), const size_t unique_index=0)
Wrapper around the CDisplayWindow3D::addTextMessage method, so that the user does not have to specify...
std::string keystroke_optimize_graph
Keystroke to manually trigger a full graph optimization.
Definition: CLevMarqGSO.h:185
void Tic()
Starts the stopwatch.
Definition: CTicTac.cpp:77
GLsizei GLsizei GLuint * obj
Definition: glext.h:3902
double optimization_distance
optimize only for the nodes found in a certain distance from the current position.
Definition: CLevMarqGSO.h:178
void getNearbyNodesOf(std::set< mrpt::utils::TNodeID > *nodes_set, const mrpt::utils::TNodeID &cur_nodeID, double distance)
Get a list of the nodeIDs whose position is within a certain distance to the specified nodeID...
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
This class allows loading and storing values and vectors of different types from a configuration text...
OptimizationParams opt_params
Parameters relevant to the optimizatio nfo the graph.
Definition: CLevMarqGSO.h:227
void dumpToTextStream(mrpt::utils::CStream &out) const
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
mrpt::graphslam::CWindowManager * m_win_manager
Pointer to the CWindowManager object used to store visuals-related instances.
void initOptDistanceVisualization()
Initialize the Disk/Sphere used for visualizing the optimization distance.
mrpt::opengl::COpenGLScenePtr & get3DSceneAndLock()
Gets a reference to the smart shared pointer that holds the internal scene (carefuly read introductio...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
void loadFromConfigFileName(const std::string &config_file, const std::string &section)
Behaves like loadFromConfigFile, but you can pass directly a file name and a temporary CConfigFile ob...
uint64_t TNodeID
The type for node IDs in graphs of different types.
virtual void initializeVisuals()
Initialize visual objects in CDisplayWindow (e.g.
#define MRPT_END
void initializeVisuals()
Initialize visual objects in CDisplayWindow (e.g.
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
void loadParams(const std::string &source_fname)
Load the necessary for the decider/optimizer configuration parameters.
std::string getStatsAsText(const size_t column_width=80) const
Dump all stats to a multi-line text string.
This class implements a high-performance stopwatch.
Definition: CTicTac.h:24
FullOptimizationPolicy m_optimization_policy
Should I fully optimize the graph on loop closure?
Definition: CLevMarqGSO.h:361
size_t m_min_nodes_for_optimization
Minimum number of nodes before we try optimizing the graph.
Definition: CLevMarqGSO.h:402
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
std::pair< TNodeID, TNodeID > TPairNodeIDs
A pair of node IDs.
void notifyOfWindowEvents(const std::map< std::string, bool > &events_occurred)
Get a list of the window events that happened since the last call.
size_t m_max_ignored_consec_lcs
Number of consecutive loop closures to ignore after m_max_used_consec_lcs have already been issued...
Definition: CLevMarqGSO.h:388
void dumpToTextStream(mrpt::utils::CStream &out) const
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
#define MRPT_LOG_WARN_STREAM(__CONTENTS)
virtual void getDescriptiveReport(std::string *report_str) const
Fill the provided string with a detailed report of the decider/optimizer state.
virtual void initializeLoggers(const std::string &name)
Initialize the COutputLogger, CTimeLogger instances given the name of the decider/optimizer at hand...
GRAPH_T * m_graph
Pointer to the graph that is under construction.
GLsizei const GLchar ** string
Definition: glext.h:3919
GRAPH_T::constraint_t::type_value pose_t
Definition: CLevMarqGSO.h:135
bool updateState(mrpt::obs::CActionCollectionPtr action, mrpt::obs::CSensoryFramePtr observations, mrpt::obs::CObservationPtr observation)
Generic method for fetching the incremental action/observation readings from the calling function...
bool checkForLoopClosures()
Check if a loop closure edge was added in the graph.
mrpt::opengl::CRenderizablePtr initOptDistanceVisualizationInternal(const mrpt::poses::CPose2D &p_unused)
Setup the corresponding Disk/Sphere instance.
TThreadHandle createThreadFromObjectMethod(CLASS *obj, void(CLASS::*func)(PARAM), PARAM param)
Creates a new thread running a non-static method (so it will have access to "this") from another meth...
Definition: threads.h:216
size_t m_curr_ignored_consec_lcs
Consecutive Loop Closures that have currently been ignored.
Definition: CLevMarqGSO.h:393
std::string keystroke_optimization_distance
Keystroke to toggle the optimization distance on/off.
Definition: CLevMarqGSO.h:183
void forceRepaint()
Repaints the window. forceRepaint, repaint and updateWindow are all aliases of the same method...
#define MRPT_START
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...
mrpt::utils::CTimeLogger m_time_logger
Time logger instance.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section)
This method load the options from a ".ini"-like file or memory-stored string list.
virtual void updateVisuals()
Update the relevant visual features in CDisplayWindow.
size_t m_max_used_consec_lcs
Number of maximum cosecutive loop closures that are allowed to be issued.
Definition: CLevMarqGSO.h:377
Output information for mrpt::graphslam::optimize_graph_spa_levmarq()
mrpt::system::TThreadHandle m_thread_optimize
Definition: CLevMarqGSO.h:349
void setCameraZoom(float zoom)
Changes the camera parameters programmatically.
The namespace for 3D scene representation and rendering.
double Tac()
Stops the stopwatch.
Definition: CTicTac.cpp:92
GLsizei GLsizei GLchar * source
Definition: glext.h:3908
#define ASSERT_(f)
A RGB color - floats in the range [0,1].
Definition: TColor.h:80
double leave(const char *func_name)
End of a named section.
Definition: CTimeLogger.h:102
void dumpToConsole() const
Just like dumpToTextStream() but sending the text to the console (std::cout)
GraphVisualizationParams viz_params
Parameters relevant to the visualization of the graph.
Definition: CLevMarqGSO.h:229
GLenum GLsizei GLenum format
Definition: glext.h:3513
size_t m_curr_used_consec_lcs
Number of consecutive loop closures that are currently registered.
Definition: CLevMarqGSO.h:382
Levenberg-Marquardt non-linear graph slam optimization scheme.
Definition: CLevMarqGSO.h:126
void enter(const char *func_name)
Start of a named section.
Definition: CTimeLogger.h:97
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
#define ASSERTMSG_(f, __ERROR_MSG)
void getPlaneLimits(float &xmin, float &xmax, float &ymin, float &ymax) const
Definition: CGridPlaneXY.h:61
GLfloat GLfloat p
Definition: glext.h:5587
void fitGraphInView()
Set the camera parameters of the CDisplayWindow3D so that the whole graph is viewed in the window...
void getDescriptiveReport(std::string *report_str) const
Fill the provided string with a detailed report of the decider/optimizer state.
double BASE_IMPEXP distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1511
void updateVisuals()
Update the relevant visual features in CDisplayWindow.
void setCameraAzimuthDeg(float deg)
Changes the camera parameters programmatically.
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
Definition: CStream.cpp:507
void assignTextMessageParameters(double *offset_y, int *text_index)
Assign the next available offset_y and text_index for the textMessage under construction.
void setCameraElevationDeg(float deg)
Changes the camera parameters programmatically.
void BASE_IMPEXP joinThread(const TThreadHandle &threadHandle)
Waits until the given thread ends.
Definition: threads.cpp:190
void _optimizeGraph(bool is_full_update=false)
Optimize the given graph.



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 8277875f6 Mon Jun 11 02:47:32 2018 +0200 at lun oct 28 01:50:49 CET 2019