/**
* 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() {
// setup a simple graph
// known poses
Pose2d x0 (0, 0, M_PI/9.0);
Pose2d x1 (10, 10, 0.0 );
Pose2d x2 (20, 20, M_PI/6.0);
Pose2d x3 (30, 30, -M_PI/4.0);
Pose2d x4 (40, 40, -M_PI/8.0);
// known landmarks
Point2d la (100, 100);
// mesurements
Pose2d z0 = x0;
Pose2d z01 = x1.ominus(x0);
Pose2d z12 = x2.ominus(x1);
Pose2d z23 = x3.ominus(x2);
Pose2d z34 = x4.ominus(x3);
Pose2d z13 = x3.ominus(x1);
// landmark measurement
Point2d z1a = x1.transform_to(la);
// add noise to measurements
double sigma = 0.01;
MatrixXd Q = sigma*sigma*eye(3);
Noise Qsqinf = Information(Q.inverse());
MatrixXd Q2 = sigma*sigma*eye(2);
Noise Q2sqinf = Information(Q2.inverse());
z0 = add_noise(z0, sigma);
z01 = add_noise(z01, sigma);
z12 = add_noise(z12, sigma);
z23 = add_noise(z23, sigma);
z34 = add_noise(z34, sigma);
z13 = add_noise(z13, sigma);
z1a = add_noise(z1a, sigma);
// nodes
Pose2d_Node* n0 = new Pose2d_Node();
Pose2d_Node* n1 = new Pose2d_Node();
Pose2d_Node* n2 = new Pose2d_Node();
Pose2d_Node* n3 = new Pose2d_Node();
Pose2d_Node* n4 = new Pose2d_Node();
Point2d_Node* na = new Point2d_Node();
// make graph
Slam slam;
Properties prop;
prop.verbose = true; prop.quiet = false; prop.method = GAUSS_NEWTON;
slam.set_properties(prop);
// add nodes to graph
slam.add_node(n0);
slam.add_node(n1);
slam.add_node(n2),
slam.add_node(n3);
slam.add_node(n4);
slam.add_node(na);
// create factors and add them to the graph
Pose2d_Factor* z0f = new Pose2d_Factor(n0, z0, Qsqinf);
slam.add_factor(z0f);
Pose2d_Pose2d_Factor* z01f = new Pose2d_Pose2d_Factor(n0, n1, z01, Qsqinf);
slam.add_factor(z01f);
Pose2d_Pose2d_Factor* z12f = new Pose2d_Pose2d_Factor(n1, n2, z12, Qsqinf);
slam.add_factor(z12f);
Pose2d_Pose2d_Factor* z23f = new Pose2d_Pose2d_Factor(n2, n3, z23, Qsqinf);
slam.add_factor(z23f);
Pose2d_Pose2d_Factor* z34f = new Pose2d_Pose2d_Factor(n3, n4, z34, Qsqinf);
slam.add_factor(z34f);
Pose2d_Pose2d_Factor* z13f = new Pose2d_Pose2d_Factor(n1, n3, z13, Qsqinf);
slam.add_factor(z13f);
Pose2d_Point2d_Factor* z1af = new Pose2d_Point2d_Factor(n1, na, z1a, Q2sqinf);
slam.add_factor(z1af);
slam.batch_optimization();
slam.print_stats();
ofstream f; f.open ("before.graph"); slam.write(f); f.close();
//slam.print_graph();
//print_all(slam);
// get true marginal distribution over p(x0, x2, x3, x4)
list<Node*> nodes_subset;
nodes_subset.push_back(n0);
nodes_subset.push_back(n2);
nodes_subset.push_back(n3);
nodes_subset.push_back(n4);
MatrixXd P_true = slam.covariances().marginal(nodes_subset);
//MatrixXd L_true = get_info (&slam);
VectorXd mu_true(12);
mu_true.segment<3>(0) = n0->value().vector();
mu_true.segment<3>(3) = n2->value().vector();
mu_true.segment<3>(6) = n3->value().vector();
mu_true.segment<3>(9) = n4->value().vector();
// remove node 1 using GLC ========================================
bool sparse = true; // sparse approximate or dense exact
vector<Factor*> felim = glc_elim_factors (n1); //usefull for local managment of factors
//vector<Factor*> fnew = glc_remove_node (slam, n1, sparse); // not root shifted
GLC_RootShift rs;
vector<Factor*> fnew = glc_remove_node (slam, n1, sparse, &rs); // root shifted
// ================================================================
cout << felim.size() << " factor(s) removed." << endl;
cout << fnew.size() << " new GLC factor(s) added." << endl;
slam.batch_optimization();
slam.print_stats();
f.open ("after.graph"); slam.write(f); f.close();
// get glc marginal distribution over p(x0, x2, x3, x4)
MatrixXd P_glc = slam.covariances().marginal(nodes_subset);
//MatrixXd L_glc = get_info (&slam);
VectorXd mu_glc(12);
mu_glc.segment<3>(0) = n0->value().vector();
mu_glc.segment<3>(3) = n2->value().vector();
mu_glc.segment<3>(6) = n3->value().vector();
mu_glc.segment<3>(9) = n4->value().vector();
// calcualte the KLD
int n = mu_glc.size();
double A = (P_glc.inverse() * P_true).trace();
VectorXd du = mu_glc - mu_true;
// deal with difference in angles
for(int i=0; i<du.size(); i++) {
if(i % 3 == 2)
du(i) = standardRad(du(i));
}
double B = du.transpose() * P_glc.inverse() * du;
double C = log(P_true.determinant()) - log(P_glc.determinant());
double kld = 0.5*(A + B - C - n);
cout << "KLD: " << kld << endl;
return 0;
}