ld_stats.c
Go to the documentation of this file.
00001 #include <options/options.h>
00002 
00003 #include <csm/csm_all.h>
00004 
00005 int main(int argc, const char * argv[]) {
00006         sm_set_program_name(argv[0]);
00007         
00008         const char*input_filename;
00009         const char*output_filename;
00010         double max_xy;
00011         double max_theta_deg;
00012         
00013         struct option* ops = options_allocate(10);
00014         options_string(ops, "in", &input_filename, "stdin", "input file (log)");
00015         options_string(ops, "out", &output_filename, "stdout", "output file ");
00016         options_double(ops, "max_xy", &max_xy, 100.0, "Max admissible xy displacement");
00017         options_double(ops, "max_theta_deg", &max_theta_deg, 360.0, 
00018                 "Max admissible theta displacement (deg)");
00019         
00020         if(!options_parse_args(ops, argc, argv)) {
00021                 sm_info("computes odometry statistics.\n\nOptions:\n");
00022                 options_print_help(ops, stderr);
00023                 return -1;
00024         }
00025         
00026         FILE * input_stream = open_file_for_reading(input_filename);
00027         FILE *output_stream = open_file_for_writing(output_filename);
00028         
00029         if(!input_stream || !output_stream) return -1;
00030         
00031         LDP laser_ref = ld_read_smart(input_stream);
00032         if(!laser_ref) {
00033                 sm_error("Cannot read first scan.\n.");
00034                 return -2;
00035         }
00036 
00037         int count = 0, valid = 0;
00038         LDP laser_sens;
00039         
00040         double avg_correction[3] = {0,0,0};
00041         double max_correction[3] = {0,0,0};
00042         while((laser_sens = ld_read_smart(input_stream))) {
00043                 double diff_true_pose[3], diff_odometry[3], diff_estimate[3];
00044                 pose_diff_d(laser_sens->odometry, laser_ref->odometry, diff_odometry);
00045                 pose_diff_d(laser_sens->true_pose, laser_ref->true_pose, diff_true_pose);
00046                 pose_diff_d(laser_sens->estimate, laser_ref->estimate, diff_estimate);
00047                 double diff[3];
00048                 pose_diff_d(diff_estimate, diff_odometry, diff);
00049                 
00050                 JO jo = jo_new();
00051                 jo_add_double_array(jo, "diff_odometry", diff_odometry, 3);
00052                 jo_add_double_array(jo, "diff_true_pose", diff_true_pose, 3);
00053                 jo_add_double_array(jo, "diff_estimate", diff_estimate, 3);
00054                 jo_add_double_array(jo, "correction", diff, 3);
00055                 
00056                 fputs(jo_to_string(jo), output_stream);
00057                 fputs("\n", output_stream);
00058                 jo_free(jo);
00059                 ld_free(laser_ref);
00060                 laser_ref = laser_sens;
00061                 
00062                 int use_it = (fabs(diff[0]) < max_xy) && (fabs(diff[1]) < max_xy) &&
00063                         (fabs(diff[2]) < deg2rad(max_theta_deg));
00064                         
00065                 if(use_it) {
00066                         int i; for(i=0;i<3;i++) {
00067                                 avg_correction[i] += diff[i];
00068                                 max_correction[i] = GSL_MAX(max_correction[i], fabs(diff[i]));
00069                         }
00070                         valid ++;
00071                 }
00072                 
00073                 count ++;
00074         }
00075 
00076         int i; for(i=0;i<3;i++) 
00077                 avg_correction[i] /= valid;
00078                 
00079         fprintf(stderr, "Used %d/%d  rays (%.1f %%)\n", valid, count, (100.0 *valid)/count);
00080         
00081         fprintf(stderr, "Avg: %f %f %f      = %fdeg \n",
00082                 avg_correction[0], avg_correction[1], avg_correction[2], rad2deg(avg_correction[2]));
00083         fprintf(stderr, "Max: %f %f %f      = %fdeg \n",
00084                 max_correction[0], max_correction[1], max_correction[2], rad2deg(max_correction[2]));
00085                 
00086         return 0;
00087 }


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