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;
}
