int main(int argc, const char* argv[]) { sm_set_program_name(argv[0]); log2pdf_params p; lds_set_defaults(&(p.laser)); ls_set_defaults(&(p.pose_path)); p.laser.rays.draw = 0; p.laser.points.draw = 0; p.laser.normals.draw = 0; p.laser.countour.width = 0.1; p.pose_path.width = 0.1; p.pose_path.color = "#f00"; options_banner(banner); struct option * ops = options_allocate(100); options_string(ops, "in", &p.input_filename, "stdin", "input file (Carmen or JSON)"); options_string(ops, "out", &p.output_filename, "", "output file (if empty, input file + '.pdf')"); options_double(ops, "padding", &p.padding, 0.2, "padding around bounding box (m)"); options_double(ops, "dimension", &p.dimension, 500.0, "dimension of the image (points)"); options_double(ops, "offset_theta_deg", &p.offset_theta_deg, 0.0, " rotate entire map by this angle (deg) "); options_string(ops, "use", &p.use, "estimate", "One in 'odometry','estimate','true_pose'"); options_double(ops, "distance_xy", &p.distance_xy, 5.0, " Minimum distance between scans (m) "); options_double(ops, "distance_th_deg", &p.distance_th_deg, 45.0, " Minimum distance between scans (deg) "); options_double(ops, "start_pose_width", &p.start_pose_width, 0.4, "First pose | Circle width"); lds_add_options(&(p.laser), ops, "laser_", ""); ls_add_options(&(p.pose_path), ops, "path_", ""); if(!options_parse_args(ops, argc, argv)) { sm_error("Could not parse arguments.\n"); options_print_help(ops, stderr); return -1; } /* If out not specified */ if(strlen(p.output_filename)==0) { char buf[PATH_MAX]; sprintf(buf, "%s.pdf", p.input_filename); p.output_filename = my_strdup(buf); /* sm_info("Writing on file '%s'.\n", p.output_filename);*/ } p.use_reference = ld_string_to_reference(p.use); if(Invalid == p.use_reference) { sm_error("Invalid reference '%s'. " "Use one in 'odometry','estimate','true_pose'.\n", p.use); return -1; } /* sm_info("Using reference: %s.\n", ld_reference_to_string(p.use_reference));*/ return !log2pdf(&p); }
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]); struct ld_exp_tro1_params p; options_banner(banner); struct option* ops = options_allocate(10); options_double(ops, "max_xy_error", &p.max_xy_error, 10.0, "Maximum error for x,y (m)"); options_double(ops, "max_theta_error_deg", &p.max_theta_error_deg, 10.0, "Maximum error for orientation (deg)"); options_int (ops, "seed", &p.seed, 0, "Seed for random number generator (if 0, use GSL_RNG_SEED env. variable)."); options_int(ops, "num_per_scan", &p.num_per_scan, 10, "Number of trials for each scan."); options_string(ops, "in", &p.file_input, "stdin", "Input file "); options_string(ops, "out1", &p.file_output1, "stdout", "Output file for first scan"); options_string(ops, "out2", &p.file_output2, "stdout", "Output file for second scan"); options_int(ops, "debug", &p.debug, 0, "Shows debug information"); if(!options_parse_args(ops, argc, argv)) { options_print_help(ops, stderr); return -1; } sm_debug_write(p.debug); 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); /* Open the two output files (possibly the same one) */ FILE * in = open_file_for_reading(p.file_input); if(!in) return -3; FILE * out1 = open_file_for_writing(p.file_output1); if(!out1) return -2; FILE * out2; if(!strcmp(p.file_output1, p.file_output2)) { out1 = out2; } else { out2 = open_file_for_writing(p.file_output2); if(!out2) return -2; } /* Read laser data from input file */ LDP ld; int count=0; while( (ld = ld_read_smart(in))) { count++; if(!ld_valid_fields(ld)) { sm_error("Invalid laser data (#%d in file)\n", count); continue; } for(int n=0; n < p.num_per_scan; n++) { ld->true_pose[0] = 0; ld->true_pose[1] = 0; ld->true_pose[2] = 0; ld->odometry[0] = 0; ld->odometry[1] = 0; ld->odometry[2] = 0; ld_write_as_json(ld, out1); ld->odometry[0] = 2*(gsl_rng_uniform(rng)-0.5) * p.max_xy_error; ld->odometry[1] = 2*(gsl_rng_uniform(rng)-0.5) * p.max_xy_error; ld->odometry[2] = 2*(gsl_rng_uniform(rng)-0.5) * deg2rad(p.max_theta_error_deg); ld_write_as_json(ld, out2); } ld_free(ld); } return 0; }
int main(int argc, const char * argv[]) { sm_set_program_name(argv[0]); options_banner("ld_noise: Adds noise to readings in a scan"); struct ld_noise_params p; struct option* ops = options_allocate(20); options_double(ops, "discretization", &p.discretization, 0.0, "Size of discretization (disabled if 0)"); options_double(ops, "sigma", &p.sigma, 0.0, "Std deviation of gaussian noise (disabled if 0)"); 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."); options_int(ops, "seed", &p.seed, 0, "Seed for random number generator (if 0, use GSL_RNG_SEED env. variable)."); 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)) { fprintf(stderr, "A simple program for adding noise to sensor scans.\n\nUsage:\n"); 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; }