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(¶ms,&result);
00066
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 }