int main(int argc, const char * argv[]) { sm_set_program_name(argv[0]); options_banner("ld_purify: Makes sure that the file format is valid. \n * Sets valid=0 if reading is outside interval "); struct ld_purify_params p; struct option* ops = options_allocate(20); options_double(ops, "threshold_min", &p.threshold_min, 0.01, "Sets valid=0 if readings are less than this threshold."); options_double(ops, "threshold_max", &p.threshold_max, 79.0, "Sets valid=0 if readings are more than this threshold."); options_string(ops, "in", &p.file_input, "stdin", "Input file "); options_string(ops, "out", &p.file_output, "stdout", "Output file "); if(!options_parse_args(ops, argc, argv)) { options_print_help(ops, stderr); return -1; } FILE * in = open_file_for_reading(p.file_input); if(!in) return -3; FILE * out = open_file_for_writing(p.file_output); if(!out) return -2; LDP ld; int count = -1; while( (ld = ld_from_json_stream(in))) { purify(ld, p.threshold_min, p.threshold_max); if(!ld_valid_fields(ld)) { sm_error("Wait, we didn't purify enough (#%d in file)\n", count); continue; } ld_write_as_json(ld, out); ld_free(ld); } return 0; }
int main(int argc, const char * argv[]) { sm_set_program_name(argv[0]); csm_options_banner("ld_noise: Adds noise to readings in a scan"); struct ld_noise_params p; struct csm_option* ops = csm_options_allocate(20); csm_options_double(ops, "discretization", &p.discretization, 0.0, "Size of discretization (disabled if 0)"); csm_options_double(ops, "sigma", &p.sigma, 0.0, "Std deviation of gaussian noise (disabled if 0)"); csm_options_int(ops, "lambertian", &p.lambertian, 0, "Use lambertian model cov = sigma^2 / cos(beta^2) where beta is the incidence. Need have alpha or true_alpha."); csm_options_int(ops, "seed", &p.seed, 0, "Seed for random number generator (if 0, use GSL_RNG_SEED env. variable)."); csm_options_string(ops, "in", &p.file_input, "stdin", "Input file "); csm_options_string(ops, "out", &p.file_output, "stdout", "Output file "); if(!csm_options_parse_args(ops, argc, argv)) { fprintf(stderr, "A simple program for adding noise to sensor scans.\n\nUsage:\n"); csm_options_print_help(ops, stderr); return -1; } FILE * in = open_file_for_reading(p.file_input); if(!in) return -3; FILE * out = open_file_for_writing(p.file_output); if(!out) return -2; gsl_rng_env_setup(); gsl_rng * rng = gsl_rng_alloc (gsl_rng_ranlxs0); if(p.seed != 0) gsl_rng_set(rng, (unsigned int) p.seed); LDP ld; int count = 0; while( (ld = ld_from_json_stream(in))) { if(!ld_valid_fields(ld)) { sm_error("Invalid laser data (#%d in file)\n", count); continue; } int i; for(i=0;i<ld->nrays;i++) { if(!ld->valid[i]) continue; double * reading = ld->readings + i; if(p.sigma > 0) { double add_sigma = p.sigma; if(p.lambertian) { int have_alpha = 0; double alpha = 0; if(!is_nan(ld->true_alpha[i])) { alpha = ld->true_alpha[i]; have_alpha = 1; } else if(ld->alpha_valid[i]) { alpha = ld->alpha[i];; have_alpha = 1; } else have_alpha = 0; if(have_alpha) { /* Recall that alpha points outside the surface */ double beta = (alpha+M_PI) - ld->theta[i]; add_sigma = p.sigma / cos(beta); } else { sm_error("Because lambertian is active, I need either true_alpha[] or alpha[]"); ld_write_as_json(ld, stderr); return -1; } } *reading += gsl_ran_gaussian(rng, add_sigma); if(is_nan(ld->readings_sigma[i])) { ld->readings_sigma[i] = add_sigma; } else { ld->readings_sigma[i] = sqrt(square(add_sigma) + square(ld->readings_sigma[i])); } } if(p.discretization > 0) *reading -= fmod(*reading , p.discretization); } ld_write_as_json(ld, out); ld_free(ld); } return 0; }