/** * The actual processing of data, in separate thread if GUI enabled. */ int process(void* unused) { // incrementally process data slam.set_properties(prop); incremental_slam(); toc("all"); if (!prop.quiet) { if (!batch_processing) { cout << endl; } double accumulated = tictoc("setup") + tictoc("incremental") + tictoc("batch"); cout << "Accumulated computation time: " << accumulated << "s" << endl; cout << "(Overall execution time: " << tictoc("all") << "s)" << endl; slam.print_stats(); cout << endl; } if (save_stats) { cout << "Saving statistics to " << fname_stats << endl; save_statistics(fname_stats); cout << endl; } if (write_result) { cout << "Saving result to " << fname_result << endl; slam.save(fname_result); cout << endl; } #ifdef USE_GUI if (use_gui) { while (true) { if (viewer.exit_requested()) { exit(0); } SDL_Delay(100); } } #endif exit(0); }
int main() { // locally defined, as nodes and factors below are allocated on the // stack, and therefore will go out of scope after the end of this // function; note that you can (and typically have to ) dynamically // allocate, see demo.cpp, but I used local variables here to make // the example simpler. Slam slam; Noise noise = SqrtInformation(10. * eye(3)); // first pose graph Pose2d prior_origin(0., 0., 0.); Pose2d_Node a0; slam.add_node(&a0); Pose2d_Factor p_a0(&a0, prior_origin, noise); slam.add_factor(&p_a0); Pose2d odo(1., 0., 0.); Pose2d_Node a1; slam.add_node(&a1); Pose2d_Pose2d_Factor o_a01(&a0, &a1, odo, noise); slam.add_factor(&o_a01); // second pose graph Pose2d_Node b0; slam.add_node(&b0); Pose2d_Factor p_b0(&b0, prior_origin, noise); slam.add_factor(&p_b0); Pose2d_Node b1; slam.add_node(&b1); Pose2d_Pose2d_Factor o_b01(&b0, &b1, odo, noise); slam.add_factor(&o_b01); cout << "two independent pose graphs:" << endl; cout << "batch optimization... (nothing changes)" << endl; slam.batch_optimization(); cout << "...done" << endl; cout << "a0: " << a0.value() << endl; cout << "a1: " << a1.value() << endl; cout << "b0: " << b0.value() << endl; cout << "b1: " << b1.value() << endl; // constraint between two pose graphs: // first pose graph now also has a anchor node, requires an arbitrary // prior (here origin); also initializes the anchor node Anchor2d_Node anchor0(&slam); slam.add_node(&anchor0); //Pose2d_Factor p_anchor0(&anchor0, prior_origin, noise); //slam.add_factor(&p_anchor0); // anchor node for second trajectory (as before) Anchor2d_Node anchor1(&slam); slam.add_node(&anchor1); Pose2d measure0(0., 1., 0.); // now we need 2 optional parameters to refer to both anchor nodes - // the order determines which trajectory the measurement originates // from; also, the first one already needs to be initialized (done // using prior above), the second one gets initialized if needed Pose2d_Pose2d_Factor d_a1_b1(&a1, &b1, measure0, noise, &anchor0, &anchor1); slam.add_factor(&d_a1_b1); cout << "\nfirst constraint added:" << endl; cout << "batch optimization... (nothing changes)" << endl; slam.batch_optimization(); cout << "...done" << endl; cout << "a0: " << a0.value() << endl; cout << "a1: " << a1.value() << endl; cout << "b0: " << b0.value() << endl; cout << "b1: " << b1.value() << endl; cout << "anchor0: " << anchor0.value() << endl; cout << "anchor1: " << anchor1.value() << endl; Pose2d measure1(0., 0.5, 0.); // conflicting measurement, want least squares Pose2d_Pose2d_Factor d_a1_b1_2(&a1, &b1, measure1, noise, &anchor0, &anchor1); slam.add_factor(&d_a1_b1_2); cout << "\nsecond conflicting constraint added (least squares is 0.75):" << endl; cout << "batch optimization... (least squares solution)" << endl; slam.batch_optimization(); cout << "...done" << endl; cout.precision(2); cout << fixed; cout << "a0: " << a0.value() << endl; cout << "a1: " << a1.value() << endl; cout << "b0: " << b0.value() << endl; cout << "b1: " << b1.value() << endl; cout << "anchor0: " << anchor0.value() << endl; cout << "anchor1: " << anchor1.value() << endl; slam.save("anchorNodes.graph"); return 0; }