/**
* 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;
}