simulations/isam/isam/isam.cpp
/**
 * The actual processing of data, in separate thread if GUI enabled.
 */
The actual processing of data, in separate thread if GUI enabled.
The actual processing of data in separate thread if GUI enabled .
int process(void* unused) {
  slam.set_properties(prop);
  incremental_slam();
  toc("all");
char fname_result_features[FNAME_MAX];
strcpy(fname_result_features,fname_result);
strcat(fname_result_features,"_features.txt");
ofstream out_features(fname_result_features, ios::out | ios::binary);
require(out_features, "Slam.save: Cannot open output file.");
out_features << "\0";
 COV AMERS
char fname_result_cov_final[FNAME_MAX];
strcpy(fname_result_cov_final,fname_result);
strcat(fname_result_cov_final,"_cov_final.txt");
ofstream out_cov_final(fname_result_cov_final, ios::out | ios::binary );
require(out_cov_final, "Slam.save: Cannot open output file.");
out_cov_final << "\0";
std::list<Node*> node_i_as_list;
  const Covariances& covariances = slam.covariances().clone();
  cout << "Full covariance matrix:" << endl;
  MatrixXd cov_full ;
      for (list<Node*>::const_iterator it = slam.get_nodes().begin();
          it != slam.get_nodes().end(); it++) {
if ( strcmp ( (*it)->name(), "Point2d" )==0 ){
node_i_as_list.push_back(*it);
(*it)->write(out_features);
out_features << endl;
}
}
  cov_full = covariances.marginal(node_i_as_list);
  out_cov_final << cov_full << endl << endl;
out_cov_final.close();
out_features.close();
  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 process ( void * unused ) { slam . set_properties ( prop ) ; incremental_slam ( ) ; toc ( " " ) ; char fname_result_features [ FNAME_MAX ] ; strcpy ( fname_result_features , fname_result ) ; strcat ( fname_result_features , " " ) ; ofstream out_features ( fname_result_features , ios :: out | ios :: binary ) ; require ( out_features , " " ) ; out_features << " \0 " ; char fname_result_cov_final [ FNAME_MAX ] ; strcpy ( fname_result_cov_final , fname_result ) ; strcat ( fname_result_cov_final , " " ) ; ofstream out_cov_final ( fname_result_cov_final , ios :: out | ios :: binary ) ; require ( out_cov_final , " " ) ; out_cov_final << " \0 " ; std :: list < Node * > node_i_as_list ; const Covariances & covariances = slam . covariances ( ) . clone ( ) ; cout << " " << endl ; MatrixXd cov_full ; for ( list < Node * > :: const_iterator it = slam . get_nodes ( ) . begin ( ) ; it != slam . get_nodes ( ) . end ( ) ; it ++ ) { if ( strcmp ( ( * it ) -> name ( ) , " " ) == 0 ) { node_i_as_list . push_back ( * it ) ; ( * it ) -> write ( out_features ) ; out_features << endl ; } } cov_full = covariances . marginal ( node_i_as_list ) ; out_cov_final << cov_full << endl << endl ; out_cov_final . close ( ) ; out_features . close ( ) ; if ( ! prop . quiet ) { if ( ! batch_processing ) { cout << endl ; } double accumulated = tictoc ( " " ) + tictoc ( " " ) + tictoc ( " " ) ; cout << " " << accumulated << " " << endl ; cout << " " << tictoc ( " " ) << " " << endl ; slam . print_stats ( ) ; cout << endl ; } if ( save_stats ) { cout << " " << fname_stats << endl ; save_statistics ( fname_stats ) ; cout << endl ; } if ( write_result ) { cout << " " << 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 ) ; }
The actual processing of data, in separate thread if GUI enabled.
The actual processing of data in separate thread if GUI enabled .
// incrementally process data // AJOUT CODE // recovering the block-diagonals only of the full covariance matrix /*
  cout << "Block-diagonals only:" << endl;
  Covariances::node_lists_t node_lists;
  list<Node*> nodes;
  nodes.push_back(pose_node_1);
  node_lists.push_back(nodes);
  nodes.clear();
  nodes.push_back(pose_node_2);
  node_lists.push_back(nodes);
  list<MatrixXd> cov_blocks = covariances.marginal(node_lists);
  int i = 1;
  for (list<MatrixXd>::iterator it = cov_blocks.begin(); it!=cov_blocks.end(); it++, i++) {
    cout << "block " << i << endl;
    cout << *it << endl;
  }
*/ // AJOUT CODE // AJOUT CODE COV AMERS // Node* node_i(0); // noeud // last_node = *loader->nodes(step).begin(); // last_node_as_list.push_back(last_node); // int res; // recovering the full covariance matrix // cov_full = covariances.marginal(node_i_as_list); // cout << cov_full << endl << endl; // res = strcmp ( (*it)->name(), "Point2d" ); // cout << res << endl; // node_i_as_list.push_back(*it); // cout << (*it)->name() << endl; //  MatrixXd cov_full = covariances.marginal(slam.get_nodes());