00001 #include <math.h> 00002 #include <gsl/gsl_math.h> 00003 00004 #include <options/options.h> 00005 00006 #include "../csm/csm_all.h" 00007 00008 int main(int argc, const char * argv[]) { 00009 sm_set_program_name(argv[0]); 00010 00011 00012 const char*input_filename; 00013 const char*output_filename; 00014 double diff[3] = {0, 0, 0}; 00015 00016 double laser[3] ={0,0,0}; 00017 00018 double omega[2] ={0,0}; 00019 double omega_vel = 0; 00020 double vel[2] ={0,0}; 00021 double vel_omega = 0; 00022 00023 00024 struct option* ops = options_allocate(15); 00025 options_string(ops, "in", &input_filename, "stdin", "input file"); 00026 options_string(ops, "out", &output_filename, "stdout", "output file"); 00027 options_double(ops, "l_x", &(laser[0]), 0.0, "laser x (m)"); 00028 options_double(ops, "l_y", &(laser[1]), 0.0, "laser y (m)"); 00029 options_double(ops, "l_theta", &(laser[2]), 0.0, "laser theta (rad)"); 00030 00031 options_double(ops, "omega0", &(omega[0]), 0.0, "omega (rad)"); 00032 options_double(ops, "omega1", &(omega[1]), 0.0, "omega (linear)"); 00033 options_double(ops, "omega_vel", &(omega_vel), 0.0, "omega x vel"); 00034 options_double(ops, "vel0", &(vel[0]), 0.0, "vel (m)"); 00035 options_double(ops, "vel1", &(vel[1]), 0.0, "vel (linear)"); 00036 options_double(ops, "vel_omega", &(vel_omega), 0.0, "vel x omega"); 00037 00038 00039 if(!options_parse_args(ops, argc, argv)) { 00040 fprintf(stderr, " Corrects bias in odometry.\n"); 00041 options_print_help(ops, stderr); 00042 return -1; 00043 } 00044 00045 FILE * input_stream = open_file_for_reading(input_filename); 00046 FILE *output_stream = open_file_for_writing(output_filename); 00047 00048 if(!input_stream || !output_stream) return -1; 00049 00050 LDP laser_ref = ld_read_smart(input_stream); 00051 if(!laser_ref) { 00052 sm_error("Cannot read first scan.\n"); 00053 return -2; 00054 } 00055 00056 copy_d(laser_ref->odometry, 3, laser_ref->estimate); 00057 00058 double old_odometry[3]; 00059 copy_d(laser_ref->odometry, 3, old_odometry); 00060 00061 LDP laser_sens; 00062 ld_write_as_json(laser_ref, output_stream); 00063 00064 while((laser_sens = ld_read_smart(input_stream))) { 00065 double guess[3], old_guess[3]; 00066 pose_diff_d(laser_sens->odometry, old_odometry, guess); 00067 pose_diff_d(laser_sens->odometry, old_odometry, old_guess); 00068 copy_d(laser_sens->odometry, 3, old_odometry); 00069 00070 if(fabs(guess[2]) > 1e-7) 00071 guess[2] = old_guess[2] * omega[1] + omega_vel * guess[0] + omega[0]; 00072 00073 if(fabs(guess[0]) > 1e-7) 00074 guess[0] = old_guess[0] * vel[1] + vel_omega * guess[2] + vel[0]; 00075 00076 fprintf(stderr, "odo: %f %f %f Corr: %f rad \t%f m \n", guess[0], guess[1], guess[2], guess[2]-old_guess[2], guess[0]-old_guess[0]); 00077 00078 oplus_d(laser_ref->odometry, guess, laser_sens->odometry); 00079 oplus_d(laser_sens->odometry, laser, laser_sens->estimate); 00080 00081 fprintf(stderr, "ref odo: %s ref est: %s \n", 00082 friendly_pose(laser_ref->odometry), 00083 friendly_pose(laser_ref->estimate)); 00084 fprintf(stderr, "sens odo: %s sens est: %s \n", 00085 friendly_pose(laser_sens->odometry), 00086 friendly_pose(laser_sens->estimate)); 00087 00088 00089 00090 ld_write_as_json(laser_sens, output_stream); 00091 00092 ld_free(laser_ref); 00093 laser_ref = laser_sens; 00094 } 00095 00096 return 0; 00097 }