sm3.c
Go to the documentation of this file.
00001 #include <time.h>
00002 #include <string.h>
00003 
00004 #include "../csm/csm_all.h"
00005 
00006 #include <options/options.h>
00007 
00008 struct sm3_params {
00009         const char * input_filename;
00010         
00011 } p;
00012 extern void sm_options(struct sm_params*p, struct option*ops);
00013 
00014 extern int distance_counter;
00015 
00016 
00017 int main(int argc, const char*argv[]) {
00018         sm_set_program_name(argv[0]);
00019         
00020         struct sm_params params;
00021         struct sm_result result;
00022         
00023         struct option* ops = options_allocate(100);
00024         options_string(ops, "in", &p.input_filename, "stdin",
00025                 "Log file");
00026         
00027         sm_options(&params, ops);
00028         if(!options_parse_args(ops, argc, argv)) {
00029                 fprintf(stderr, "\n\nUsage:\n");
00030                 options_print_help(ops, stderr);
00031                 return -1;
00032         }
00033 
00034         FILE * file = open_file_for_reading(p.input_filename);
00035         if(!file) return -1;
00036         
00037         LDP *lds; int lds_size;
00038         if(!ld_read_all(file, &lds, &lds_size)) {
00039                 sm_error("Cannot read all laser scans.\n");
00040                 return -1;
00041         }
00042 
00043         sm_debug("Read %d scans.\n", lds_size);
00044 
00045 
00046         int num_matchings = 0;
00047         int num_iterations = 0;
00048         clock_t start = clock();
00049         
00050         int i;
00051         for(i=0;i<lds_size-1;i++) {
00052                 params.laser_ref = lds[i];
00053                 params.laser_sens = lds[i+1];
00054 
00055                 double odometry[3];
00056                 pose_diff_d(params.laser_sens->odometry, params.laser_ref->odometry, odometry);
00057                 double ominus_laser[3], temp[3];
00058                 ominus_d(params.laser, ominus_laser);
00059                 oplus_d(ominus_laser, odometry, temp);
00060                 oplus_d(temp, params.laser, params.first_guess);
00061 
00062                 sm_icp(&params,&result); 
00063                 
00064                 num_matchings++;
00065                 num_iterations += result.iterations;
00066                 
00067                 fprintf(stderr, "."); 
00068         }
00069         
00070         clock_t end = clock();
00071         float seconds = (end-start)/((float)CLOCKS_PER_SEC);
00072         
00073         if(num_matchings>0) {
00074                 printf("sm3: CPU time = %f (seconds) (start=%d end=%d)\n", seconds,(int)start,(int)end);
00075                 printf("sm3: Total number of matchings = %d\n", num_matchings);
00076                 printf("sm3: Total number of iterations = %d\n", num_iterations);
00077                 printf("sm3: Avg. iterations per matching = %f\n", num_iterations/((float)num_matchings));
00078                 printf("sm3: Avg. seconds per matching = %f\n", seconds/num_matchings);
00079                 printf("sm3:   that is, %d matchings per second\n", (int)floor(num_matchings/seconds));
00080                 printf("sm3: Avg. seconds per iteration = %f (note: very imprecise)\n", seconds/num_iterations);
00081                 printf("sm3: Number of comparisons = %d \n", distance_counter);
00082                 printf("sm3: Avg. comparisons per ray per iteration = %f \n", 
00083                         (distance_counter/((float)num_iterations*params.laser_ref->nrays)));
00084         } else {
00085                 sm_error("Empty file?\n");
00086                 return 1;
00087         }
00088         return 0;
00089 }


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