ld_noise.c
Go to the documentation of this file.
00001 #include <gsl/gsl_rng.h>
00002 #include <gsl/gsl_randist.h>
00003 
00004 #include <math.h>
00005 #include <libgen.h>
00006 
00007 #include <options/options.h>
00008 #include "../csm/csm_all.h"
00009 
00010 struct ld_noise_params {
00011         int seed;
00012         double discretization;
00013         double sigma;
00014         const char* file_input;
00015         const char* file_output;
00016         int lambertian;
00017 };
00018 
00019 int main(int argc, const char * argv[]) {
00020         sm_set_program_name(argv[0]);
00021         
00022         options_banner("ld_noise: Adds noise to readings in a scan");
00023         
00024         struct ld_noise_params p;
00025         
00026         struct option* ops = options_allocate(20);
00027         options_double(ops, "discretization", &p.discretization, 0.0, 
00028                 "Size of discretization (disabled if 0)");
00029         options_double(ops, "sigma", &p.sigma, 0.0, 
00030                 "Std deviation of gaussian noise (disabled if 0)");
00031         options_int(ops, "lambertian", &p.lambertian, 0, 
00032                 "Use lambertian model cov = sigma^2 / cos(beta^2) where beta is the incidence. Need have alpha or true_alpha.");
00033         options_int(ops, "seed", &p.seed, 0, 
00034                 "Seed for random number generator (if 0, use GSL_RNG_SEED env. variable).");
00035         options_string(ops, "in", &p.file_input, "stdin", "Input file ");
00036         options_string(ops, "out", &p.file_output, "stdout", "Output file ");
00037                 
00038                 
00039         if(!options_parse_args(ops, argc, argv)) {
00040                 fprintf(stderr, "A simple program for adding noise to sensor scans.\n\nUsage:\n");
00041                 options_print_help(ops, stderr);
00042                 return -1;
00043         }
00044 
00045         FILE * in = open_file_for_reading(p.file_input);
00046         if(!in) return -3;
00047 
00048         FILE * out = open_file_for_writing(p.file_output);
00049         if(!out) return -2;
00050 
00051 
00052         gsl_rng_env_setup();
00053         gsl_rng * rng = gsl_rng_alloc (gsl_rng_ranlxs0);
00054         if(p.seed != 0)
00055         gsl_rng_set(rng, (unsigned int) p.seed);
00056 
00057         LDP ld; int count = 0;
00058         while( (ld = ld_from_json_stream(in))) {
00059                 if(!ld_valid_fields(ld))  {
00060                         sm_error("Invalid laser data (#%d in file)\n", count);
00061                         continue;
00062                 }
00063                 
00064                 int i;
00065                 for(i=0;i<ld->nrays;i++) {
00066                         if(!ld->valid[i]) continue;
00067                         
00068                         double * reading = ld->readings + i;
00069                         if(p.sigma > 0) {
00070                                 double add_sigma = p.sigma;
00071                                 
00072                                 if(p.lambertian) {
00073 
00074                                         int have_alpha = 0;
00075                                         double alpha = 0;
00076                                         if(!is_nan(ld->true_alpha[i])) {
00077                                                 alpha = ld->true_alpha[i];
00078                                                 have_alpha = 1;
00079                                         } else if(ld->alpha_valid[i]) {
00080                                                 alpha = ld->alpha[i];;
00081                                                 have_alpha = 1;
00082                                         } else have_alpha = 0;
00083 
00084                                         if(have_alpha) {
00085                                                 /* Recall that alpha points outside the surface */
00086                                                 double beta = (alpha+M_PI) - ld->theta[i];
00087                                             add_sigma = p.sigma / cos(beta);
00088                                         } else {
00089                                                 sm_error("Because lambertian is active, I need either true_alpha[] or alpha[]");
00090                                                 ld_write_as_json(ld, stderr);
00091                                                 return -1;
00092                                         }
00093                                         
00094                                 } 
00095                                 
00096                            *reading += gsl_ran_gaussian(rng, add_sigma);
00097                                 
00098                                 if(is_nan(ld->readings_sigma[i])) {
00099                                         ld->readings_sigma[i] = add_sigma;
00100                                 } else {
00101                                         ld->readings_sigma[i] = sqrt(square(add_sigma) + square(ld->readings_sigma[i]));
00102                                 }
00103                         }
00104                         if(p.discretization > 0)
00105                                 *reading -= fmod(*reading , p.discretization);
00106                 }
00107         
00108                 ld_write_as_json(ld, out);
00109                 ld_free(ld);
00110         }
00111         
00112         return 0;
00113 }


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