sm0.c
Go to the documentation of this file.
00001 #include <time.h>
00002 
00003 #include "../csm/csm_all.h"
00004 
00005 
00006 extern int distance_counter;
00007 int main(int argc, const char*argv[]) {
00008         FILE * file;
00009         if(argc==1) {
00010                 file = stdin;
00011         } else {
00012                 const char * filename = argv[1];
00013                 file = fopen(filename,"r");
00014                 if(file==NULL) {
00015                         fprintf(stderr, "Could not open '%s'.\n", filename); 
00016                         return -1;
00017                 }
00018         }
00019         
00020         struct sm_params params;
00021         struct sm_result result;
00022         
00023         params.max_angular_correction_deg = 5;
00024         params.max_linear_correction = 0.2;
00025         params.max_iterations = 30;
00026         params.epsilon_xy = 0.001;
00027         params.epsilon_theta = 0.001;
00028         params.max_correspondence_dist = 2;
00029         params.sigma = 0.01;
00030         params.restart = 1;
00031         params.restart_threshold_mean_error = 3.0 / 300.0;
00032         params.restart_dt= 0.1;
00033         params.restart_dtheta=    1.5 * 3.14 /180;
00034 
00035         params.clustering_threshold = 0.05;
00036         params.orientation_neighbourhood = 3;
00037         params.use_corr_tricks = 1;
00038 
00039         params.do_alpha_test = 0;
00040         params.outliers_maxPerc = 0.85;
00041 
00042         params.outliers_adaptive_order =0.7;
00043         params.outliers_adaptive_mult=2;
00044         params.do_visibility_test = 1;
00045         params.do_compute_covariance = 0;
00046 
00047         int num_matchings = 0;
00048         int num_iterations = 0;
00049         clock_t start = clock();
00050         
00051         if(!(params.laser_ref = ld_read_smart(file)) ) {
00052                 printf("Could not read first scan.\n");
00053                 return -1;
00054         }
00055         
00056         gsl_vector *u = gsl_vector_alloc(3);
00057         gsl_vector *x_old = gsl_vector_alloc(3);
00058         gsl_vector *x_new = gsl_vector_alloc(3);
00059         while(! (params.laser_sens = ld_read_smart(file)) ) {
00060                 copy_from_array(x_old, params.laser_ref->odometry);
00061                 copy_from_array(x_new, params.laser_sens->odometry);
00062                 pose_diff(x_new,x_old,u);
00063                 vector_to_array(u, params.first_guess);
00064         
00065                 sm_gpm(&params,&result);
00066                 /* sm_icp(&params,&result); */
00067                 
00068                 num_matchings++;
00069                 num_iterations += result.iterations;
00070         
00071                 ld_free(params.laser_ref);
00072                 params.laser_ref = params.laser_sens;
00073         }
00074 
00075         clock_t end = clock();
00076         float seconds = (end-start)/((float)CLOCKS_PER_SEC);
00077         
00078         sm_debug("sm0: CPU time = %f (seconds) (start=%d end=%d)\n", seconds,(int)start,(int)end);
00079         sm_debug("sm0: Total number of matchings = %d\n", num_matchings);
00080         sm_debug("sm0: Total number of iterations = %d\n", num_iterations);
00081         sm_debug("sm0: Avg. iterations per matching = %f\n", num_iterations/((float)num_matchings));
00082         sm_debug("sm0: Avg. seconds per matching = %f\n", seconds/num_matchings);
00083         sm_debug("sm0:   that is, %d matchings per second\n", (int)floor(num_matchings/seconds));
00084         sm_debug("sm0: Avg. seconds per iteration = %f (note: very imprecise)\n", seconds/num_iterations);
00085         sm_debug("sm0: Number of comparisons = %d \n", distance_counter);
00086         sm_debug("sm0: Avg. comparisons per ray = %f \n", 
00087                 (distance_counter/((float)num_iterations*params.laser_ref->nrays)));
00088 
00089         ld_free(params.laser_ref);
00090         
00091         gsl_vector_free(u);
00092         gsl_vector_free(x_old);
00093         gsl_vector_free(x_new);
00094         return 0;
00095 }


csm
Author(s): Andrea Censi
autogenerated on Mon Jan 16 2017 03:48:29