10 #ifndef CLEVMARQGSO_IMPL_H
11 #define CLEVMARQGSO_IMPL_H
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)
39 template <
class GRAPH_T>
48 template <
class GRAPH_T>
57 if (this->m_graph->nodeCount() > m_last_total_num_of_nodes)
59 m_last_total_num_of_nodes = this->m_graph->nodeCount();
60 registered_new_node =
true;
62 if (m_first_time_call)
64 opt_params.last_pair_nodes_to_edge = this->m_graph->edges;
65 m_first_time_call =
true;
68 if (opt_params.optimization_on_second_thread)
71 m_thread_optimize.join();
78 bool is_full_update = this->checkForFullOptimization();
79 this->_optimizeGraph(is_full_update);
87 template <
class GRAPH_T>
92 parent::initializeVisuals();
94 this->initGraphVisualization();
95 this->initOptDistanceVisualization();
100 template <
class GRAPH_T>
104 parent::updateVisuals();
106 if (opt_params.optimization_distance > 0)
108 this->updateOptDistanceVisualization();
111 this->updateGraphVisualization();
116 template <
class GRAPH_T>
118 const std::map<std::string, bool>& events_occurred)
122 parent::notifyOfWindowEvents(events_occurred);
127 if (opt_params.optimization_distance > 0)
129 if (events_occurred.find(opt_params.keystroke_optimization_distance)
132 this->toggleOptDistanceVisualization();
135 if (events_occurred.find(opt_params.keystroke_optimize_graph)->second)
137 this->_optimizeGraph(
true);
142 if (events_occurred.find(viz_params.keystroke_graph_toggle)->second)
144 this->toggleGraphVisualization();
148 if (events_occurred.find(
"mouse_clicked")->second)
151 m_autozoom_active =
false;
155 if (events_occurred.find(viz_params.keystroke_graph_autofit)->second)
158 this->fitGraphInView();
164 template <
class GRAPH_T>
168 ASSERTDEBMSG_(this->m_win_manager,
"No CWindowManager* is given");
170 if (viz_params.visualize_optimized_graph)
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");
177 this->m_win_manager->assignTextMessageParameters(
178 &viz_params.offset_y_graph,
179 &viz_params.text_index_graph);
184 template <
class GRAPH_T>
188 ASSERTDEBMSG_(this->m_win_manager,
"No CWindowManager* is given");
200 bool prev_visibility =
true;
203 prev_visibility = prev_object->isVisible();
205 scene->removeObject(prev_object);
210 this->m_graph->getAs3DObject(graph_obj, viz_params.cfg);
212 graph_obj->setName(
"optimized_graph");
213 graph_obj->setVisibility(prev_visibility);
214 scene->insert(graph_obj);
215 this->m_win->unlockAccess3DScene();
217 this->m_win_manager->addTextMessage(
218 5, -viz_params.offset_y_graph,
220 "Optimized Graph: #nodes %d",
221 static_cast<int>(this->m_graph->nodeCount())),
223 viz_params.text_index_graph);
225 this->m_win->forceRepaint();
227 if (m_autozoom_active)
229 this->fitGraphInView();
235 template <
class GRAPH_T>
244 graph_obj->setVisibility(!graph_obj->isVisible());
246 this->m_win->unlockAccess3DScene();
247 this->m_win->forceRepaint();
252 template <
class GRAPH_T>
260 "\nVisualization of data was requested but no CDisplayWindow3D pointer "
267 std::dynamic_pointer_cast<CSetOfObjects>(
obj);
268 this->m_win->unlockAccess3DScene();
269 this->m_win->forceRepaint();
276 float 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)));
284 this->m_win->setCameraAzimuthDeg(60);
285 this->m_win->setCameraElevationDeg(75);
286 this->m_win->setCameraProjective(
true);
291 template <
class GRAPH_T>
297 if (opt_params.optimization_distance > 0)
299 this->m_win_observer->registerKeystroke(
300 opt_params.keystroke_optimization_distance,
301 "Toggle optimization distance on/off");
303 this->m_win_observer->registerKeystroke(
304 opt_params.keystroke_optimize_graph,
305 "Manually trigger a full graph optimization");
311 obj->setPose(initial_pose);
312 obj->setName(
"optimization_distance_obj");
316 this->m_win->unlockAccess3DScene();
317 this->m_win->forceRepaint();
320 this->m_win_manager->assignTextMessageParameters(
321 &opt_params.offset_y_optimization_distance,
322 &opt_params.text_index_optimization_distance);
324 this->m_win_manager->addTextMessage(
325 5, -opt_params.offset_y_optimization_distance,
326 format(
"Radius for graph optimization"),
328 opt_params.text_index_optimization_distance);
332 template <
class GRAPH_T>
341 opt_params.optimization_distance,
342 opt_params.optimization_distance - 0.1);
343 obj->setColor_u8(opt_params.optimization_distance_color);
347 template <
class GRAPH_T>
355 obj->setRadius(opt_params.optimization_distance);
357 opt_params.optimization_distance_color.R,
358 opt_params.optimization_distance_color.G,
359 opt_params.optimization_distance_color.B,
365 template <
class GRAPH_T>
369 ASSERTDEBMSG_(this->m_win_manager,
"No CWindowManager* is given");
376 obj->setPose(this->m_graph->nodes.rbegin()->second);
378 this->m_win->unlockAccess3DScene();
379 this->m_win->forceRepaint();
384 template <
class GRAPH_T>
393 obj->setVisibility(!
obj->isVisible());
395 this->m_win->unlockAccess3DScene();
396 this->m_win->forceRepaint();
401 template <
class GRAPH_T>
408 "optimizeGraph:: ThreadID:" << endl
409 <<
"\t" << std::this_thread::get_id()
412 <<
"Trying to grab lock... ");
414 std::lock_guard<std::mutex> graph_lock(*this->m_graph_section);
415 this->_optimizeGraph();
422 template <
class GRAPH_T>
426 this->m_time_logger.enter(
"CLevMarqGSO::_optimizeGraph");
429 if (m_min_nodes_for_optimization > this->m_graph->nodes.size())
435 optimization_timer.
Tic();
438 std::set<mrpt::graphs::TNodeID>* nodes_to_optimize;
447 nodes_to_optimize = NULL;
451 nodes_to_optimize =
new std::set<mrpt::graphs::TNodeID>;
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);
467 *(this->m_graph), levmarq_info, nodes_to_optimize, opt_params.cfg,
472 m_just_fully_optimized_graph =
true;
476 m_just_fully_optimized_graph =
false;
479 double elapsed_time = optimization_timer.
Tac();
485 delete nodes_to_optimize;
486 nodes_to_optimize =
nullptr;
488 this->m_time_logger.leave(
"CLevMarqGSO::_optimizeGraph");
493 template <
class GRAPH_T>
498 bool is_loop_closure =
false;
499 auto curr_pair_nodes_to_edge = this->m_graph->edges;
509 curr_pair_nodes_to_edge.begin();
510 it != curr_pair_nodes_to_edge.end(); ++it)
512 search = opt_params.last_pair_nodes_to_edge.find(it->first);
514 if (search == opt_params.last_pair_nodes_to_edge.end())
516 curr_pair = it->first;
519 static_cast<int>(curr_pair.first) -
520 static_cast<int>(curr_pair.second)) >
521 opt_params.LC_min_nodeid_diff)
525 is_loop_closure =
true;
532 opt_params.last_pair_nodes_to_edge = curr_pair_nodes_to_edge;
533 return is_loop_closure;
538 template <
class GRAPH_T>
541 bool is_full_update =
false;
543 if (opt_params.optimization_distance == -1)
548 bool added_lc = this->checkForLoopClosures();
553 if (m_curr_used_consec_lcs != 0 || m_curr_ignored_consec_lcs != 0)
558 m_curr_used_consec_lcs = 0;
559 m_curr_ignored_consec_lcs = 0;
560 m_optimization_policy = FOP_USE_LC;
562 return is_full_update;
567 bool use_limit_reached =
568 m_curr_used_consec_lcs == m_max_used_consec_lcs;
570 bool ignore_limit_reached =
571 m_curr_ignored_consec_lcs == m_max_ignored_consec_lcs;
574 if (ignore_limit_reached || use_limit_reached)
576 m_curr_ignored_consec_lcs = 0;
577 m_curr_used_consec_lcs = 0;
580 if (ignore_limit_reached)
582 m_optimization_policy = FOP_USE_LC;
584 if (use_limit_reached)
586 m_optimization_policy = FOP_IGNORE_LC;
591 if (m_optimization_policy == FOP_USE_LC)
593 m_curr_used_consec_lcs += 1;
597 m_curr_ignored_consec_lcs += 1;
603 if (m_optimization_policy == FOP_IGNORE_LC)
605 is_full_update =
false;
607 "*PARTIAL* graph optimization.. ignoring new loop closure");
611 is_full_update =
true;
614 return is_full_update;
618 template <
class GRAPH_T>
621 return m_just_fully_optimized_graph;
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)
631 template <
class GRAPH_T>
633 std::set<mrpt::graphs::TNodeID>* nodes_set,
642 nodeID < this->m_graph->nodeCount() - 1; ++nodeID)
644 double curr_distance = this->m_graph->nodes[nodeID].distanceTo(
645 this->m_graph->nodes[cur_nodeID]);
648 nodes_set->insert(nodeID);
654 this->m_graph->getAllNodes(*nodes_set);
660 template <
class GRAPH_T>
663 parent::printParams();
665 opt_params.dumpToConsole();
666 viz_params.dumpToConsole();
668 template <
class GRAPH_T>
672 parent::loadParams(source_fname);
674 opt_params.loadFromConfigFileName(source_fname,
"OptimizerParameters");
675 viz_params.loadFromConfigFileName(source_fname,
"VisualizationParameters");
680 m_max_used_consec_lcs =
source.read_int(
681 "OptimizerParameters",
"max_used_consecutive_loop_closures", 2,
false);
683 m_max_ignored_consec_lcs =
source.read_int(
684 "OptimizerParameters",
"max_ignored_consecutive_loop_closures", 15,
689 int min_verbosity_level =
690 source.read_int(
"OptimizerParameters",
"class_verbosity", 1,
false);
694 m_has_read_config =
true;
699 template <
class GRAPH_T>
709 stringstream class_props_ss;
710 class_props_ss <<
"Levenberg Marquardt Optimization Summary: " << std::endl;
711 class_props_ss << header_sep << std::endl;
714 const std::string time_res = this->m_time_logger.getStatsAsText();
715 const std::string output_res = this->getLogAsString();
719 parent::getDescriptiveReport(report_str);
721 *report_str += class_props_ss.str();
722 *report_str += report_sep;
724 *report_str += time_res;
725 *report_str += report_sep;
727 *report_str += output_res;
728 *report_str += report_sep;
735 template <
class GRAPH_T>
737 : optimization_distance_color(0, 201, 87),
738 keystroke_optimization_distance(
"u"),
739 keystroke_optimize_graph(
"w")
742 template <
class GRAPH_T>
746 template <
class GRAPH_T>
748 std::ostream& out)
const
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;
759 template <
class GRAPH_T>
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);
772 optimization_distance == 1 || optimization_distance > 0,
774 "Invalid value for optimization distance: %.2f",
775 optimization_distance));
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);
791 template <
class GRAPH_T>
793 : keystroke_graph_toggle(
"s"), keystroke_graph_autofit(
"a")
796 template <
class GRAPH_T>
800 template <
class GRAPH_T>
802 std::ostream& out)
const
807 "-----------[ Graph Visualization Parameters ]-----------\n");
809 "Visualize optimized graph = %s\n",
810 visualize_optimized_graph ?
"TRUE" :
"FALSE");
814 std::cout << std::endl;
818 template <
class GRAPH_T>
824 visualize_optimized_graph =
825 source.read_bool(section,
"visualize_optimized_graph", 1,
false);
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);
832 source.read_bool(section,
"optimized_show_edges", 1,
false);
834 source.read_int(section,
"optimized_edge_color", 4286611456,
false);
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);