23 template <
class my_graph_t>
31 const typename my_graph_t::global_poses_t& real_poses,
35 real_poses.find(to)->second - real_poses.find(from)->second;
38 if constexpr (my_graph_t::edge_t::is_PDF())
40 const auto N = my_graph_t::edge_t::state_length;
47 from, to,
typename my_graph_t::edge_t(RelativePose, mat));
51 graph.insertEdge(from, to, RelativePose);
57 my_graph_t& graph,
size_t N_VERTEX = 50,
double DIST_THRES = 7,
58 double NODES_XY_MAX = 20)
61 typename my_graph_t::global_poses_t real_node_poses;
74 for (
TNodeID j = 0; j < N_VERTEX; j++)
76 static double ang = 2 *
M_PI / N_VERTEX;
77 const double R = NODES_XY_MAX + 2 * (j % 2 ? 1 : -1);
78 CPose2D p(
R * cos(ang * j),
R * sin(ang * j), ang);
81 real_node_poses[j] = p;
88 for (
TNodeID i = 0; i < N_VERTEX; i++)
90 for (
TNodeID j = i + 1; j < N_VERTEX; j++)
92 if (real_node_poses[i].distanceTo(real_node_poses[j]) <
94 addEdge(i, j, real_node_poses, graph);
99 addEdge(0, N_VERTEX / 2, real_node_poses, graph);
105 const my_graph_t graph_GT = graph;
108 for (
auto& edge : graph.edges)
123 if constexpr (my_graph_t::edge_t::is_PDF())
125 const auto N = my_graph_t::edge_t::state_length;
129 edge.second +=
typename my_graph_t::edge_t(noise, mat);
133 edge.second +=
typename my_graph_t::edge_t(noise);
142 if constexpr (my_graph_t::edge_t::is_PDF())
144 const auto N = my_graph_t::edge_t::state_length;
148 edge.second +=
typename my_graph_t::edge_t(noise, mat);
152 edge.second +=
typename my_graph_t::edge_t(noise);
157 for (
auto itNode = graph.nodes.begin(); itNode != graph.nodes.end();
159 if (itNode->first != graph.root)
161 ->second +=
typename my_graph_t::edge_t::type_value(
CPose3D(