ld_correct.c
Go to the documentation of this file.
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 }


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