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(
317  obj->setColor_u8(opt_params.optimization_distance_color);
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();
328  obj->setRadius(opt_params.optimization_distance);
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
625  "OptimizerParameters",
626  "max_used_consecutive_loop_closures",
627  2, false);
628 
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;
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);
723  section,
724  "optimization_distance",
725  5, false);
726  // asert the previous value
729  format("Invalid value for optimization distance: %.2f",
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 
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 printParams() const
Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact w...
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.
std::string getAsString() const
Returns a multi-line string representation of the parameters like : &#39;nam = val = val2...&#39;.
Definition: TParameters.h:101
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.
bool justFullyOptimizedGraph() const
Used by the caller to query for possible full graph optimization on the latest optimizer run...
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.
Definition: CConfigFile.h:30
void setCameraPointingToPoint(float x, float y, float z)
Changes the camera parameters programmatically.
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
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.
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...
void printParams() const
Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact w...
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
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 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
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.
virtual void getDescriptiveReport(std::string *report_str) const
Fill the provided string with a detailed report of the decider/optimizer state.
GLint GLint GLsizei GLsizei GLsizei GLint GLenum format
Definition: glew.h:1168
This class implements a high-performance stopwatch.
Definition: CTicTac.h:24
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.
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
GLhandleARB obj
Definition: glew.h:3276
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
#define MRPT_LOG_WARN_STREAM(__CONTENTS)
virtual void initializeLoggers(const std::string &name)
Initialize the COutputLogger, CTimeLogger instances given the name of the decider/optimizer at hand...
double read_double(const std::string &section, const std::string &name, double defaultValue, bool failIfNotFound=false) const
GRAPH_T * m_graph
Pointer to the graph that is under construction.
GLfloat GLfloat p
Definition: glew.h:10113
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.
void dumpToConsole() const
Just like dumpToTextStream() but sending the text to the console (std::cout)
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...
GLsizei const GLcharARB ** string
Definition: glew.h:3293
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
#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
std::string getStatsAsText(const size_t column_width=80) const
Dump all stats to a multi-line text string.
GraphVisualizationParams viz_params
Parameters relevant to the visualization of the graph.
Definition: CLevMarqGSO.h:229
size_t m_curr_used_consec_lcs
Number of consecutive loop closures that are currently registered.
Definition: CLevMarqGSO.h:382
void getDescriptiveReport(std::string *report_str) const
Fill the provided string with a detailed report of the decider/optimizer state.
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
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 fitGraphInView()
Set the camera parameters of the CDisplayWindow3D so that the whole graph is viewed in the window...
void getPlaneLimits(float &xmin, float &xmax, float &ymin, float &ymax) const
Definition: CGridPlaneXY.h:61
GLsizei GLsizei GLchar * source
Definition: glew.h:1739
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.
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092
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.



Page generated by Doxygen 1.8.11 for MRPT 1.5.7 Git: c588cff Thu May 17 07:30:58 2018 +0200 at jue may 17 07:44:27 CEST 2018