14 template <
class GRAPH_T>
22 template <
class GRAPH_T>
29 if (this->m_graph->nodeCount() > m_last_total_num_of_nodes)
31 m_last_total_num_of_nodes = this->m_graph->nodeCount();
32 registered_new_node =
true;
34 if (m_first_time_call)
36 opt_params.last_pair_nodes_to_edge = this->m_graph->edges;
37 m_first_time_call =
true;
40 if (opt_params.optimization_on_second_thread)
43 m_thread_optimize.join();
50 bool is_full_update = this->checkForFullOptimization();
51 this->_optimizeGraph(is_full_update);
59 template <
class GRAPH_T>
64 parent::initializeVisuals();
66 this->initGraphVisualization();
67 this->initOptDistanceVisualization();
72 template <
class GRAPH_T>
76 parent::updateVisuals();
78 if (opt_params.optimization_distance > 0)
80 this->updateOptDistanceVisualization();
83 this->updateGraphVisualization();
88 template <
class GRAPH_T>
90 const std::map<std::string, bool>& events_occurred)
94 parent::notifyOfWindowEvents(events_occurred);
99 if (opt_params.optimization_distance > 0)
101 if (events_occurred.find(opt_params.keystroke_optimization_distance)
104 this->toggleOptDistanceVisualization();
107 if (events_occurred.find(opt_params.keystroke_optimize_graph)->second)
109 this->_optimizeGraph(
true);
114 if (events_occurred.find(viz_params.keystroke_graph_toggle)->second)
116 this->toggleGraphVisualization();
120 if (events_occurred.find(
"mouse_clicked")->second)
123 m_autozoom_active =
false;
127 if (events_occurred.find(viz_params.keystroke_graph_autofit)->second)
130 this->fitGraphInView();
136 template <
class GRAPH_T>
140 ASSERTDEBMSG_(this->m_win_manager,
"No CWindowManager* is given");
142 if (viz_params.visualize_optimized_graph)
144 this->m_win_observer->registerKeystroke(
145 viz_params.keystroke_graph_toggle,
"Toggle Graph visualization");
146 this->m_win_observer->registerKeystroke(
147 viz_params.keystroke_graph_autofit,
"Fit Graph in view");
149 this->m_win_manager->assignTextMessageParameters(
150 &viz_params.offset_y_graph,
151 &viz_params.text_index_graph);
156 template <
class GRAPH_T>
160 ASSERTDEBMSG_(this->m_win_manager,
"No CWindowManager* is given");
172 bool prev_visibility =
true;
175 prev_visibility = prev_object->isVisible();
177 scene->removeObject(prev_object);
182 this->m_graph->getAs3DObject(graph_obj, viz_params.cfg);
184 graph_obj->setName(
"optimized_graph");
185 graph_obj->setVisibility(prev_visibility);
186 scene->insert(graph_obj);
187 this->m_win->unlockAccess3DScene();
189 this->m_win_manager->addTextMessage(
190 5, -viz_params.offset_y_graph,
192 "Optimized Graph: #nodes %d",
193 static_cast<int>(this->m_graph->nodeCount())),
195 viz_params.text_index_graph);
197 this->m_win->forceRepaint();
199 if (m_autozoom_active)
201 this->fitGraphInView();
207 template <
class GRAPH_T>
216 graph_obj->setVisibility(!graph_obj->isVisible());
218 this->m_win->unlockAccess3DScene();
219 this->m_win->forceRepaint();
224 template <
class GRAPH_T>
232 "\nVisualization of data was requested but no CDisplayWindow3D pointer "
239 std::dynamic_pointer_cast<CSetOfObjects>(obj);
240 this->m_win->unlockAccess3DScene();
241 this->m_win->forceRepaint();
248 float x_min, x_max, y_min, y_max;
250 const float z_min = obj_grid->getPlaneZcoord();
251 this->m_win->setCameraPointingToPoint(
252 0.5 * (x_min + x_max), 0.5 * (y_min + y_max), z_min);
253 this->m_win->setCameraZoom(
254 2.0f * std::max(10.0f, std::max(x_max - x_min, y_max - y_min)));
256 this->m_win->setCameraAzimuthDeg(60);
257 this->m_win->setCameraElevationDeg(75);
258 this->m_win->setCameraProjective(
true);
263 template <
class GRAPH_T>
269 if (opt_params.optimization_distance > 0)
271 this->m_win_observer->registerKeystroke(
272 opt_params.keystroke_optimization_distance,
273 "Toggle optimization distance on/off");
275 this->m_win_observer->registerKeystroke(
276 opt_params.keystroke_optimize_graph,
277 "Manually trigger a full graph optimization");
283 obj->setPose(initial_pose);
284 obj->setName(
"optimization_distance_obj");
288 this->m_win->unlockAccess3DScene();
289 this->m_win->forceRepaint();
292 this->m_win_manager->assignTextMessageParameters(
293 &opt_params.offset_y_optimization_distance,
294 &opt_params.text_index_optimization_distance);
296 this->m_win_manager->addTextMessage(
297 5, -opt_params.offset_y_optimization_distance,
298 "Radius for graph optimization",
300 opt_params.text_index_optimization_distance);
304 template <
class GRAPH_T>
313 opt_params.optimization_distance,
314 opt_params.optimization_distance - 0.1);
315 obj->setColor_u8(opt_params.optimization_distance_color);
319 template <
class GRAPH_T>
327 obj->setRadius(opt_params.optimization_distance);
329 opt_params.optimization_distance_color.R,
330 opt_params.optimization_distance_color.G,
331 opt_params.optimization_distance_color.B,
337 template <
class GRAPH_T>
341 ASSERTDEBMSG_(this->m_win_manager,
"No CWindowManager* is given");
348 obj->setPose(this->m_graph->nodes.rbegin()->second);
350 this->m_win->unlockAccess3DScene();
351 this->m_win->forceRepaint();
356 template <
class GRAPH_T>
365 obj->setVisibility(!obj->isVisible());
367 this->m_win->unlockAccess3DScene();
368 this->m_win->forceRepaint();
373 template <
class GRAPH_T>
380 "optimizeGraph:: ThreadID:" << endl
381 <<
"\t" << std::this_thread::get_id()
384 <<
"Trying to grab lock... ");
386 std::lock_guard<std::mutex> graph_lock(*this->m_graph_section);
387 this->_optimizeGraph();
394 template <
class GRAPH_T>
398 this->m_time_logger.enter(
"CLevMarqGSO::_optimizeGraph");
401 if (m_min_nodes_for_optimization > this->m_graph->nodes.size())
407 optimization_timer.
Tic();
410 std::set<mrpt::graphs::TNodeID>* nodes_to_optimize;
419 nodes_to_optimize =
nullptr;
423 nodes_to_optimize =
new std::set<mrpt::graphs::TNodeID>;
429 this->getNearbyNodesOf(
430 nodes_to_optimize, this->m_graph->nodeCount() - 1,
431 opt_params.optimization_distance);
432 nodes_to_optimize->insert(this->m_graph->nodeCount() - 1);
439 *(this->m_graph), levmarq_info, nodes_to_optimize, opt_params.cfg,
444 m_just_fully_optimized_graph =
true;
448 m_just_fully_optimized_graph =
false;
451 double elapsed_time = optimization_timer.
Tac();
457 delete nodes_to_optimize;
458 nodes_to_optimize =
nullptr;
460 this->m_time_logger.leave(
"CLevMarqGSO::_optimizeGraph");
464 template <
class GRAPH_T>
469 bool is_loop_closure =
false;
470 auto curr_pair_nodes_to_edge = this->m_graph->edges;
476 typename GRAPH_T::edges_map_t::const_iterator search;
479 for (
auto it = curr_pair_nodes_to_edge.begin();
480 it != curr_pair_nodes_to_edge.end(); ++it)
482 search = opt_params.last_pair_nodes_to_edge.find(it->first);
484 if (search == opt_params.last_pair_nodes_to_edge.end())
486 curr_pair = it->first;
489 static_cast<int>(curr_pair.first) -
490 static_cast<int>(curr_pair.second)) >
491 opt_params.LC_min_nodeid_diff)
495 is_loop_closure =
true;
502 opt_params.last_pair_nodes_to_edge = curr_pair_nodes_to_edge;
503 return is_loop_closure;
508 template <
class GRAPH_T>
511 bool is_full_update =
false;
513 if (opt_params.optimization_distance == -1)
518 bool added_lc = this->checkForLoopClosures();
523 if (m_curr_used_consec_lcs != 0 || m_curr_ignored_consec_lcs != 0)
528 m_curr_used_consec_lcs = 0;
529 m_curr_ignored_consec_lcs = 0;
530 m_optimization_policy = OptimizationPolicy::UseLoopClosures;
532 return is_full_update;
537 bool use_limit_reached =
538 m_curr_used_consec_lcs == m_max_used_consec_lcs;
540 bool ignore_limit_reached =
541 m_curr_ignored_consec_lcs == m_max_ignored_consec_lcs;
544 if (ignore_limit_reached || use_limit_reached)
546 m_curr_ignored_consec_lcs = 0;
547 m_curr_used_consec_lcs = 0;
550 if (ignore_limit_reached)
552 m_optimization_policy = OptimizationPolicy::UseLoopClosures;
554 if (use_limit_reached)
556 m_optimization_policy = OptimizationPolicy::IgnoreLoopClosures;
561 if (m_optimization_policy == OptimizationPolicy::UseLoopClosures)
563 m_curr_used_consec_lcs += 1;
567 m_curr_ignored_consec_lcs += 1;
573 if (m_optimization_policy == OptimizationPolicy::IgnoreLoopClosures)
575 is_full_update =
false;
577 "*PARTIAL* graph optimization.. ignoring new loop closure");
581 is_full_update =
true;
584 return is_full_update;
588 template <
class GRAPH_T>
591 return m_just_fully_optimized_graph;
594 template <
class GRAPH_T>
596 const GRAPH_T& graph,
const size_t iter,
const size_t max_iter,
597 const double cur_sq_error)
601 template <
class GRAPH_T>
603 std::set<mrpt::graphs::TNodeID>* nodes_set,
612 nodeID < this->m_graph->nodeCount() - 1; ++nodeID)
614 double curr_distance = this->m_graph->nodes[nodeID].distanceTo(
615 this->m_graph->nodes[cur_nodeID]);
618 nodes_set->insert(nodeID);
624 this->m_graph->getAllNodes(*nodes_set);
630 template <
class GRAPH_T>
633 parent::printParams();
635 opt_params.dumpToConsole();
636 viz_params.dumpToConsole();
638 template <
class GRAPH_T>
642 parent::loadParams(source_fname);
644 opt_params.loadFromConfigFileName(source_fname,
"OptimizerParameters");
645 viz_params.loadFromConfigFileName(source_fname,
"VisualizationParameters");
650 m_max_used_consec_lcs = source.
read_int(
651 "OptimizerParameters",
"max_used_consecutive_loop_closures", 2,
false);
653 m_max_ignored_consec_lcs = source.
read_int(
654 "OptimizerParameters",
"max_ignored_consecutive_loop_closures", 15,
659 int min_verbosity_level =
660 source.
read_int(
"OptimizerParameters",
"class_verbosity", 1,
false);
664 m_has_read_config =
true;
669 template <
class GRAPH_T>
675 const std::string report_sep(2,
'\n');
676 const std::string header_sep(80,
'#');
679 stringstream class_props_ss;
680 class_props_ss <<
"Levenberg Marquardt Optimization Summary: " << std::endl;
681 class_props_ss << header_sep << std::endl;
684 const std::string time_res = this->m_time_logger.getStatsAsText();
685 const std::string output_res = this->getLogAsString();
689 parent::getDescriptiveReport(report_str);
691 *report_str += class_props_ss.str();
692 *report_str += report_sep;
694 *report_str += time_res;
695 *report_str += report_sep;
697 *report_str += output_res;
698 *report_str += report_sep;
703 template <
class GRAPH_T>
705 : optimization_distance_color(0, 201, 87),
706 keystroke_optimization_distance(
"u"),
707 keystroke_optimize_graph(
"w")
710 template <
class GRAPH_T>
712 template <
class GRAPH_T>
714 std::ostream&
out)
const
717 out <<
"-----------[ Levenberg-Marquardt Optimization ] -------\n";
718 out <<
"Optimization on second thread = "
719 << (optimization_on_second_thread ?
"TRUE" :
"FALSE") << std::endl;
720 out <<
"Optimize nodes in distance = " << optimization_distance <<
"\n";
721 out <<
"Min. node difference for LC = " << LC_min_nodeid_diff <<
"\n";
722 out << cfg.getAsString() << std::endl;
725 template <
class GRAPH_T>
730 optimization_on_second_thread = source.
read_bool(
731 section,
"optimization_on_second_thread",
false,
false);
732 LC_min_nodeid_diff = source.
read_int(
733 "GeneralConfiguration",
"LC_min_nodeid_diff", 30,
false);
734 optimization_distance =
735 source.
read_double(section,
"optimization_distance", 5,
false);
738 optimization_distance == 1 || optimization_distance > 0,
740 "Invalid value for optimization distance: %.2f",
741 optimization_distance));
744 cfg[
"verbose"] = source.
read_bool(section,
"verbose",
false,
false);
745 cfg[
"profiler"] = source.
read_bool(section,
"profiler",
false,
false);
746 cfg[
"max_iterations"] =
747 source.
read_double(section,
"max_iterations", 100,
false);
748 cfg[
"scale_hessian"] =
749 source.
read_double(
"Optimization",
"scale_hessian", 0.2,
false);
750 cfg[
"tau"] = source.
read_double(section,
"tau", 1e-3,
false);
755 template <
class GRAPH_T>
757 : keystroke_graph_toggle(
"s"), keystroke_graph_autofit(
"a")
760 template <
class GRAPH_T>
763 template <
class GRAPH_T>
765 std::ostream&
out)
const
769 out <<
"-----------[ Graph Visualization Parameters ]-----------\n";
771 "Visualize optimized graph = %s\n",
772 visualize_optimized_graph ?
"TRUE" :
"FALSE");
776 std::cout << std::endl;
780 template <
class GRAPH_T>
786 visualize_optimized_graph =
787 source.
read_bool(section,
"visualize_optimized_graph",
true,
false);
789 cfg[
"show_ID_labels"] =
790 source.
read_bool(section,
"optimized_show_ID_labels",
false,
false);
791 cfg[
"show_ground_grid"] =
792 source.
read_double(section,
"optimized_show_ground_grid", 1,
false);
794 source.
read_bool(section,
"optimized_show_edges",
true,
false);
796 source.
read_int(section,
"optimized_edge_color", 1500,
false);
798 source.
read_double(section,
"optimized_edge_width", 1.5,
false);
799 cfg[
"show_node_corners"] =
800 source.
read_bool(section,
"optimized_show_node_corners",
true,
false);
801 cfg[
"show_edge_rel_poses"] =
802 source.
read_bool(section,
"optimized_show_edge_rel_poses",
true,
false);
803 cfg[
"edge_rel_poses_color"] =
804 source.
read_int(section,
"optimized_edge_rel_poses_color", 2000,
false);
805 cfg[
"nodes_edges_corner_scale"] = source.
read_double(
806 section,
"optimized_nodes_edges_corner_scale", 0.4,
false);
807 cfg[
"nodes_corner_scale"] =
808 source.
read_double(section,
"optimized_nodes_corner_scale", 0.7,
false);
809 cfg[
"nodes_point_size"] =
810 source.
read_int(section,
"optimized_nodes_point_size", 5,
false);
811 cfg[
"nodes_point_color"] =
812 source.
read_int(section,
"optimized_nodes_point_color", 3000,
false);