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 }