void prepareGraphIncrementaly(graph_t &graph) { obj_t obj; P::Pose p; P::InformMatrix inform; graph.addNode(node_t(p, obj)); for (size_t i = 0; i < 19; ++i) { graph.addNode(node_t(p, obj)); graph.addEdge(edge_t(&graph.getNode(i), &graph.getNode(i + 1), p, inform)); } }