49 template <
class GRAPH,
bool EDGES_ARE_PDF = GRAPH::edge_t::is_PDF_val>
56 template <
class GRAPH>
59 static const int DIM = GRAPH::edge_t::type_value::static_size;
64 const typename GRAPH::global_poses_t& real_poses, GRAPH& graph,
74 typename GRAPH::edge_t RelativePose(
75 real_poses.find(to)->second - real_poses.find(from)->second);
76 graph.insertEdge(from, to, RelativePose);
80 template <
class GRAPH>
83 static const int DIM = GRAPH::edge_t::type_value::static_size;
88 const typename GRAPH::global_poses_t& real_poses, GRAPH& graph,
91 typename GRAPH::edge_t RelativePose(
92 real_poses.find(to)->second - real_poses.find(from)->second,
94 graph.insertEdge(from, to, RelativePose);
103 template <
class my_graph_t>
106 template <
class GRAPH_T>
107 static void my_levmarq_feedback(
108 const GRAPH_T& graph,
const size_t iter,
const size_t max_iter,
109 const double cur_sq_error)
112 if ((iter % 100) == 0)
113 cout <<
"Progress: " << iter <<
" / " << max_iter
114 <<
", total sq err = " << cur_sq_error << endl;
120 void run(
bool add_extra_tightening_edge)
126 typename my_graph_t::global_poses_t real_node_poses;
137 const size_t N_VERTEX = 50;
138 const double DIST_THRES = 7;
139 const double NODES_XY_MAX = 20;
145 for (
TNodeID j = 0; j < N_VERTEX; j++)
148 static double ang = 2 *
M_PI / N_VERTEX;
149 const double R = NODES_XY_MAX + 2 * (j % 2 ? 1 : -1);
150 CPose2D p(
R * cos(ang * j),
R * sin(ang * j), ang);
153 real_node_poses[j] = p;
168 typename edge_adder_t::cov_t inf_matrix;
169 inf_matrix.setDiagonal(
170 edge_adder_t::cov_t::RowsAtCompileTime,
176 for (
TNodeID i = 0; i < N_VERTEX; i++)
178 for (
TNodeID j = i + 1; j < N_VERTEX; j++)
180 if (real_node_poses[i].distanceTo(real_node_poses[j]) <
183 i, j, real_node_poses, graph, inf_matrix);
188 if (add_extra_tightening_edge)
192 0, N_VERTEX / 2, real_node_poses, graph, inf_matrix);
196 typename my_graph_t::edge_t& ed =
207 const my_graph_t graph_GT = graph;
209 cout <<
"graph nodes: " << graph_GT.nodeCount() << endl;
210 cout <<
"graph edges: " << graph_GT.edgeCount() << endl;
213 for (
typename my_graph_t::edges_map_t::iterator itEdge =
215 itEdge != graph.edges.end(); ++itEdge)
217 const typename my_graph_t::edge_t::type_value delta_noise(
CPose3D(
224 itEdge->second.getPoseMean() +=
225 typename my_graph_t::edge_t::type_value(delta_noise);
228 for (
typename my_graph_t::global_poses_t::iterator itNode =
230 itNode != graph.nodes.end(); ++itNode)
231 if (itNode->first != graph.root)
232 itNode->second.getPoseMean() +=
233 typename my_graph_t::edge_t::type_value(
CPose3D(
248 const my_graph_t graph_initial = graph;
258 params[
"max_iterations"] = 500;
259 params[
"scale_hessian"] = 0.1;
272 cout <<
"Global graph RMS error / edge = "
273 << std::sqrt(graph.getGlobalSquareError(
false) / graph.edgeCount())
275 cout <<
"Global graph RMS error / edge = "
276 << std::sqrt(graph.getGlobalSquareError(
true) / graph.edgeCount())
277 <<
" (ignoring information matrices)." << endl;
284 params, &my_levmarq_feedback<my_graph_t>);
286 cout <<
"Global graph RMS error / edge = "
287 << std::sqrt(graph.getGlobalSquareError(
false) / graph.edgeCount())
289 cout <<
"Global graph RMS error / edge = "
290 << std::sqrt(graph.getGlobalSquareError(
true) / graph.edgeCount())
291 <<
" (ignoring information matrices)." << endl;
300 graph_render_params1[
"show_edges"] = 1;
301 graph_render_params1[
"edge_width"] = 1;
302 graph_render_params1[
"nodes_corner_scale"] = 1;
305 graph, graph_render_params1);
309 graph_render_params2[
"show_ground_grid"] = 0;
310 graph_render_params2[
"show_edges"] = 0;
311 graph_render_params2[
"show_node_corners"] = 0;
312 graph_render_params2[
"nodes_point_size"] = 7;
316 graph_initial, graph_render_params2);
318 graph_render_params2[
"nodes_point_size"] = 5;
321 graph, graph_render_params2);
325 graph_render_params3[
"show_ground_grid"] = 0;
326 graph_render_params3[
"show_ID_labels"] = 1;
327 graph_render_params3[
"show_edges"] = 1;
328 graph_render_params3[
"edge_width"] = 3;
329 graph_render_params3[
"nodes_corner_scale"] = 2;
332 graph_GT, graph_render_params3);
335 graph_initial, graph_render_params3);
338 5, 5,
"Ground truth: Big corners & thick edges",
341 5, 5 + 15,
"Initial graph: Gray thick points.",
344 5, 5 + 30,
"Final graph: Small corners & thin edges",
349 scene->insert(gl_graph1);
350 scene->insert(gl_graph3);
351 scene->insert(gl_graph2);
352 scene->insert(gl_graph5);
353 win.unlockAccess3DScene();
363 cout <<
"Close any window to end...\n";
366 std::this_thread::sleep_for(10ms);
377 cout <<
"Select the type of graph to optimize:\n"
378 "1. CNetworkOfPoses2D \n"
379 "2. CNetworkOfPoses2DInf \n"
380 "3. CNetworkOfPoses3D \n"
381 "4. CNetworkOfPoses3DInf \n";
388 std::getline(cin, l);
393 bool add_extra_tightening_edge;
394 cout <<
"Add an extra, incompatible tightening edge? [y/N] ";
397 std::getline(cin, l);
399 add_extra_tightening_edge = l[0] ==
'Y' || l[0] ==
'y';
407 demo.
run(add_extra_tightening_edge);
413 demo.
run(add_extra_tightening_edge);
419 demo.
run(add_extra_tightening_edge);
425 demo.
run(add_extra_tightening_edge);
430 std::this_thread::sleep_for(20ms);
435 cout <<
"MRPT exception caught: " << e.what() << endl;
440 printf(
"Another exception!!");