int main(int argc, const char * argv[]) { sm_set_program_name(argv[0]); /* struct csm_option* ops = csm_options_allocate(3); csm_options_double(ops, "scale_deg", &p.scale_deg, 0.0, "Scale factor (degrees) "); csm_options_int(ops, "neighbours", &p.neighbours, 1, "How many neighbours to consider (regardless of scale)."); if(!csm_options_parse_args(ops, argc, argv)) { fprintf(stderr, "A simple program for smoothing a sensor scan.\n\nUsage:\n"); csm_options_print_help(ops, stderr); return -1; } */ /* jj_set_stream(open_file_for_writing("ld_cluster_curv.txt")); */ int errors = 0; int count = -1; LDP ld; while( (ld = ld_read_smart(stdin)) ) { count++; if(!ld_valid_fields(ld)) { sm_error("Invalid laser data (#%d in file)\n", count); return -1; } ld_cluster_curv(ld); ld_write_as_json(ld, stdout); ld_free(ld); } return errors; }
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; }
/** Write the laser data in CARMEN format */ void ld_write_as_carmen(LDP ld, FILE * stream) { int i; double timestamp; if(!ld_valid_fields(ld)) { sm_error("Writing bad data to the stream.\n"); } fprintf(stream, "FLASER %d ", ld->nrays); for(i=0; i<ld->nrays; i++){ fprintf(stream, "%g ", ld->readings[i]); } fprintf(stream, "%g %g %g ", ld->estimate[0], ld->estimate[1], ld->estimate[2]); fprintf(stream, "%g %g %g ", ld->odometry[0], ld->odometry[1], ld->odometry[2]); timestamp = ld->tv.tv_sec + ((double)ld->tv.tv_sec)/1e6; fprintf(stream, "%g %s %g", timestamp, ld->hostname, timestamp); fputs("\n", stream); }
int main(int argc, const char * argv[]) { sm_set_program_name(argv[0]); int errors = 0; int count = -1; LDP ld; while( (ld = ld_read_smart(stdin)) ) { count++; if(!ld_valid_fields(ld)) { sm_error("Invalid laser data (#%d in file)\n", count); errors++; continue; } ld_linearize(ld); ld_write_as_json(ld, stdout); ld_free(ld); } return errors; }
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; }
void sm_icp(struct sm_params*params, struct sm_result*res) { res->valid = 0; LDP laser_ref = params->laser_ref; LDP laser_sens = params->laser_sens; if(!ld_valid_fields(laser_ref) || !ld_valid_fields(laser_sens)) { return; } sm_debug("sm_icp: laser_sens has %d/%d; laser_ref has %d/%d rays valid\n", count_equal(laser_sens->valid, laser_sens->nrays, 1), laser_sens->nrays, count_equal(laser_ref->valid, laser_ref->nrays, 1), laser_ref->nrays); /** Mark as invalid the rays outside of (min_reading, max_reading] */ ld_invalid_if_outside(laser_ref, params->min_reading, params->max_reading); ld_invalid_if_outside(laser_sens, params->min_reading, params->max_reading); sm_debug("sm_icp: laser_sens has %d/%d; laser_ref has %d/%d rays valid (after removing outside interval [%f, %f])\n", count_equal(laser_sens->valid, laser_sens->nrays, 1), laser_sens->nrays, count_equal(laser_ref->valid, laser_ref->nrays, 1), laser_ref->nrays, params->min_reading, params->max_reading); if(JJ) jj_context_enter("sm_icp"); egsl_push_named("sm_icp"); if(params->use_corr_tricks || params->debug_verify_tricks) ld_create_jump_tables(laser_ref); ld_compute_cartesian(laser_ref); ld_compute_cartesian(laser_sens); if(params->do_alpha_test) { ld_simple_clustering(laser_ref, params->clustering_threshold); ld_compute_orientation(laser_ref, params->orientation_neighbourhood, params->sigma); ld_simple_clustering(laser_sens, params->clustering_threshold); ld_compute_orientation(laser_sens, params->orientation_neighbourhood, params->sigma); } if(JJ) jj_add("laser_ref", ld_to_json(laser_ref)); if(JJ) jj_add("laser_sens", ld_to_json(laser_sens)); gsl_vector * x_new = gsl_vector_alloc(3); gsl_vector * x_old = vector_from_array(3, params->first_guess); if(params->do_visibility_test) { sm_debug("laser_ref:\n"); visibilityTest(laser_ref, x_old); sm_debug("laser_sens:\n"); gsl_vector * minus_x_old = gsl_vector_alloc(3); ominus(x_old,minus_x_old); visibilityTest(laser_sens, minus_x_old); gsl_vector_free(minus_x_old); } double error; int iterations; int nvalid; if(!icp_loop(params, x_old->data, x_new->data, &error, &nvalid, &iterations)) { sm_error("icp: ICP failed for some reason. \n"); res->valid = 0; res->iterations = iterations; res->nvalid = 0; } else { /* It was succesfull */ int restarted = 0; double best_error = error; gsl_vector * best_x = gsl_vector_alloc(3); gsl_vector_memcpy(best_x, x_new); if(params->restart && (error/nvalid)>(params->restart_threshold_mean_error) ) { sm_debug("Restarting: %f > %f \n",(error/nvalid),(params->restart_threshold_mean_error)); restarted = 1; double dt = params->restart_dt; double dth = params->restart_dtheta; sm_debug("icp_loop: dt = %f dtheta= %f deg\n",dt,rad2deg(dth)); double perturb[6][3] = { {dt,0,0}, {-dt,0,0}, {0,dt,0}, {0,-dt,0}, {0,0,dth}, {0,0,-dth} }; int a; for(a=0;a<6;a++){ sm_debug("-- Restarting with perturbation #%d\n", a); struct sm_params my_params = *params; gsl_vector * start = gsl_vector_alloc(3); gvs(start, 0, gvg(x_new,0)+perturb[a][0]); gvs(start, 1, gvg(x_new,1)+perturb[a][1]); gvs(start, 2, gvg(x_new,2)+perturb[a][2]); gsl_vector * x_a = gsl_vector_alloc(3); double my_error; int my_valid; int my_iterations; if(!icp_loop(&my_params, start->data, x_a->data, &my_error, &my_valid, &my_iterations)){ sm_error("Error during restart #%d/%d. \n", a, 6); break; } iterations+=my_iterations; if(my_error < best_error) { sm_debug("--Perturbation #%d resulted in error %f < %f\n", a,my_error,best_error); gsl_vector_memcpy(best_x, x_a); best_error = my_error; } gsl_vector_free(x_a); gsl_vector_free(start); } } /* At last, we did it. */ res->valid = 1; vector_to_array(best_x, res->x); sm_debug("icp: final x = %s \n", gsl_friendly_pose(best_x)); if (restarted) { // recompute correspondences in case of restarts ld_compute_world_coords(laser_sens, res->x); if(params->use_corr_tricks) find_correspondences_tricks(params); else find_correspondences(params); } if(params->do_compute_covariance) { val cov0_x, dx_dy1, dx_dy2; compute_covariance_exact( laser_ref, laser_sens, best_x, &cov0_x, &dx_dy1, &dx_dy2); val cov_x = sc(square(params->sigma), cov0_x); /* egsl_v2da(cov_x, res->cov_x); */ res->cov_x_m = egsl_v2gslm(cov_x); res->dx_dy1_m = egsl_v2gslm(dx_dy1); res->dx_dy2_m = egsl_v2gslm(dx_dy2); if(0) { egsl_print("cov0_x", cov0_x); egsl_print_spectrum("cov0_x", cov0_x); val fim = ld_fisher0(laser_ref); val ifim = inv(fim); egsl_print("fim", fim); egsl_print_spectrum("ifim", ifim); } } res->error = best_error; res->iterations = iterations; res->nvalid = nvalid; gsl_vector_free(best_x); } gsl_vector_free(x_new); gsl_vector_free(x_old); egsl_pop_named("sm_icp"); if(JJ) jj_context_exit(); }
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]); struct sm_params params; struct sm_result result; struct option* ops = options_allocate(100); options_string(ops, "in", &p.file_in, "stdin", "Input file "); options_string(ops, "out", &p.file_out, "stdout", "Output file "); options_string(ops, "out_stats", &p.file_out_stats, "", "Output file (stats) "); options_string(ops, "file_jj", &p.file_jj, "", "File for journaling -- if left empty, journal not open."); options_int(ops, "algo", &p.algo, 0, "Which algorithm to use (0:(pl)ICP 1:gpm-stripped 2:HSM) "); options_int(ops, "debug", &p.debug, 0, "Shows debug information"); options_int(ops, "recover_from_error", &p.recover_from_error, 0, "If true, tries to recover from an ICP matching error"); p.format = 0; /* options_int(ops, "format", &p.format, 0, "Output format (0: log in JSON format, 1: log in Carmen format (not implemented))");*/ sm_options(¶ms, ops); if(!options_parse_args(ops, argc, argv)) { fprintf(stderr, "\n\nUsage:\n"); options_print_help(ops, stderr); return -1; } sm_debug_write(p.debug); /* Open input and output files */ FILE * file_in = open_file_for_reading(p.file_in); if(!file_in) return -1; FILE * file_out = open_file_for_writing(p.file_out); if(!file_out) return -1; if(strcmp(p.file_jj, "")) { FILE * jj = open_file_for_writing(p.file_jj); if(!jj) return -1; jj_set_stream(jj); } FILE * file_out_stats = 0; if(strcmp(p.file_out_stats, "")) { file_out_stats = open_file_for_writing(p.file_out_stats); if(!file_out_stats) return -1; } /* Read first scan */ LDP laser_ref; if(!(laser_ref = ld_read_smart(file_in))) { sm_error("Could not read first scan.\n"); return -1; } if(!ld_valid_fields(laser_ref)) { sm_error("Invalid laser data in first scan.\n"); return -2; } /* For the first scan, set estimate = odometry */ copy_d(laser_ref->odometry, 3, laser_ref->estimate); spit(laser_ref, file_out); int count=-1; LDP laser_sens; while( (laser_sens = ld_read_smart(file_in)) ) { count++; if(!ld_valid_fields(laser_sens)) { sm_error("Invalid laser data in (#%d in file).\n", count); return -(count+2); } params.laser_ref = laser_ref; params.laser_sens = laser_sens; /* Set first guess as the difference in odometry */ if( any_nan(params.laser_ref->odometry,3) || any_nan(params.laser_sens->odometry,3) ) { sm_error("The 'odometry' field is set to NaN so I don't know how to get an initial guess. I usually use the difference in the odometry fields to obtain the initial guess.\n"); sm_error(" laser_ref->odometry = %s \n", friendly_pose(params.laser_ref->odometry) ); sm_error(" laser_sens->odometry = %s \n", friendly_pose(params.laser_sens->odometry) ); sm_error(" I will quit it here. \n"); return -3; } double odometry[3]; pose_diff_d(laser_sens->odometry, laser_ref->odometry, odometry); double ominus_laser[3], temp[3]; ominus_d(params.laser, ominus_laser); oplus_d(ominus_laser, odometry, temp); oplus_d(temp, params.laser, params.first_guess); /* Do the actual work */ switch(p.algo) { case(0): sm_icp(¶ms, &result); break; case(1): sm_gpm(¶ms, &result); break; case(2): sm_hsm(¶ms, &result); break; default: sm_error("Unknown algorithm to run: %d.\n",p.algo); return -1; } if(!result.valid){ if(p.recover_from_error) { sm_info("One ICP matching failed. Because you passed -recover_from_error, I will try to recover." " Note, however, that this might not be good in some cases. \n"); sm_info("The recover is that the displacement is set to 0. No result stats is output. \n"); /* For the first scan, set estimate = odometry */ copy_d(laser_ref->estimate, 3, laser_sens->estimate); ld_free(laser_ref); laser_ref = laser_sens; } else { sm_error("One ICP matching failed. Because I process recursively, I will stop here.\n"); sm_error("Use the option -recover_from_error if you want to try to recover.\n"); ld_free(laser_ref); return 2; } } else { /* Add the result to the previous estimate */ oplus_d(laser_ref->estimate, result.x, laser_sens->estimate); /* Write the corrected log */ spit(laser_sens, file_out); /* Write the statistics (if required) */ if(file_out_stats) { JO jo = result_to_json(¶ms, &result); fputs(jo_to_string(jo), file_out_stats); fputs("\n", file_out_stats); jo_free(jo); } ld_free(laser_ref); laser_ref = laser_sens; } } ld_free(laser_ref); return 0; }