ld_slip.c
Go to the documentation of this file.
00001 #include <gsl/gsl_rng.h>
00002 #include <gsl/gsl_randist.h>
00003 #include <gsl/gsl_math.h>
00004 
00005 #include <math.h>
00006 #include <options/options.h>
00007 
00008 #include "../csm/csm_all.h"
00009 
00010 struct ld_noise_params {
00011         int seed;
00012 
00013         double sigma_xy;
00014         double sigma_theta_deg;
00015         
00016         const char* file_input;
00017         const char* file_output;
00018         
00019         int debug;
00020 };
00021 
00022 const char * banner = 
00023         "A simple program for adding slip to odometry \n\n"
00024                 "The 'odometry' field is set to 'true_pose' + noise.\n"
00025                 "If 'true_pose' is not available, then the 'odometry' \n"
00026                 "field is set to 'odometry' + noise.\n\n"
00027                 "Note: this program does *not* simulate the effect of \n"
00028                 "slip or odometry error in a realistic way; each scan \n"
00029                 "in the file is considered separately, the error does \n"
00030                 "not depend on the traveled distance, etc.\n\n";
00031 
00032 int main(int argc, const char ** argv) {
00033         sm_set_program_name(argv[0]);
00034         
00035         struct ld_noise_params p;
00036         
00037         options_banner(banner);
00038         
00039         struct option* ops = options_allocate(10);
00040         options_double(ops, "sigma_theta_deg", &p.sigma_theta_deg, 0.0, 
00041                 "Std deviation of gaussian noise for theta (deg) (disabled if 0)");
00042         options_double(ops, "sigma_xy", &p.sigma_xy, 0.0, 
00043                 "Std deviation of gaussian noise for x,y (disabled if 0)");
00044         options_int(ops, "seed", &p.seed, 0, 
00045                 "Seed for random number generator (if 0, use GSL_RNG_SEED env. variable).");
00046         options_string(ops, "in", &p.file_input, "stdin", "Input file ");
00047         options_string(ops, "out", &p.file_output, "stdout", "Output file ");
00048 
00049         options_int(ops, "debug", &p.debug, 0, "Shows debug information");
00050         
00051                 
00052         if(!options_parse_args(ops, argc, argv)) {
00053                 options_print_help(ops, stderr);
00054                 return -1;
00055         }
00056         
00057         sm_debug_write(p.debug);
00058 
00059         gsl_rng_env_setup();
00060         gsl_rng * rng = gsl_rng_alloc (gsl_rng_ranlxs0);
00061         if(p.seed != 0)
00062         gsl_rng_set(rng, (unsigned int) p.seed);
00063         
00064         
00065         FILE * in = open_file_for_reading(p.file_input);
00066         if(!in) return -3;
00067 
00068         FILE * out = open_file_for_writing(p.file_output);
00069         if(!out) return -2;
00070 
00071         LDP ld; int count=0;
00072         while( (ld = ld_read_smart(in))) {
00073                 count++;
00074                 if(!ld_valid_fields(ld))  {
00075                         sm_error("Invalid laser data (#%d in file)\n", count);
00076                         continue;
00077                 }
00078                 
00079                 if(!any_nan(ld->true_pose, 3))
00080                         copy_d( (const double*) ld->true_pose, 3, ld->odometry);
00081                 
00082                 double e[3] = {0,0,0};
00083                 
00084                 if(p.sigma_xy > 0) {
00085                         e[0]  = gsl_ran_gaussian(rng, p.sigma_xy);
00086                         e[1]  = gsl_ran_gaussian(rng, p.sigma_xy);
00087                 }
00088 
00089                 if(p.sigma_theta_deg > 0) {
00090                         e[2] = gsl_ran_gaussian(rng, deg2rad(p.sigma_theta_deg));
00091                 }
00092                 
00093                 ld->odometry[0] += e[0];
00094                 ld->odometry[1] += e[1];
00095                 ld->odometry[2] += e[2];
00096         
00097                 sm_debug("Adding noise %s.\n", friendly_pose(e));
00098                 
00099                 ld_write_as_json(ld, out);
00100                 
00101                 ld_free(ld);
00102         }
00103         
00104         return 0;
00105 }


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