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;
}
void ld_write_format(LDP ld, FILE*f, const char * out_format) {
if(!strncmp(out_format, "carmen", 6))
ld_write_as_carmen(ld, f);
else
ld_write_as_json(ld, f);
/* XXX: check validity of format string */
}
void spit(LDP ld, FILE * stream) {
switch(p.format) {
case(0): {
ld_write_as_json(ld, stream);
break;
}
case(1): {
/* XXX: to implement */
break;
}
}
}
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]);
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;
}
int compute_next_estimate(struct sm_params*params,
const double x_old[3], double x_new[3])
{
LDP laser_ref = params->laser_ref;
LDP laser_sens = params->laser_sens;
struct gpc_corr c[laser_sens->nrays];
int i; int k=0;
for(i=0;i<laser_sens->nrays;i++) {
if(!laser_sens->valid[i])
continue;
if(!ld_valid_corr(laser_sens,i))
continue;
int j1 = laser_sens->corr[i].j1;
int j2 = laser_sens->corr[i].j2;
c[k].valid = 1;
if(laser_sens->corr[i].type == corr_pl) {
c[k].p[0] = laser_sens->points[i].p[0];
c[k].p[1] = laser_sens->points[i].p[1];
c[k].q[0] = laser_ref->points[j1].p[0];
c[k].q[1] = laser_ref->points[j1].p[1];
/** TODO: here we could use the estimated alpha */
double diff[2];
diff[0] = laser_ref->points[j1].p[0]-laser_ref->points[j2].p[0];
diff[1] = laser_ref->points[j1].p[1]-laser_ref->points[j2].p[1];
double one_on_norm = 1 / sqrt(diff[0]*diff[0]+diff[1]*diff[1]);
double normal[2];
normal[0] = +diff[1] * one_on_norm;
normal[1] = -diff[0] * one_on_norm;
double cos_alpha = normal[0];
double sin_alpha = normal[1];
c[k].C[0][0] = cos_alpha*cos_alpha;
c[k].C[1][0] =
c[k].C[0][1] = cos_alpha*sin_alpha;
c[k].C[1][1] = sin_alpha*sin_alpha;
/* sm_debug("k=%d, i=%d sens_phi: %fdeg, j1=%d j2=%d, alpha_seg=%f, cos=%f sin=%f \n", k,i,
rad2deg(laser_sens->theta[i]), j1,j2, atan2(sin_alpha,cos_alpha), cos_alpha,sin_alpha);*/
#if 0
/* Note: it seems that because of numerical errors this matrix might be
not semidef positive. */
double det = c[k].C[0][0] * c[k].C[1][1] - c[k].C[0][1] * c[k].C[1][0];
double trace = c[k].C[0][0] + c[k].C[1][1];
int semidef = (det >= 0) && (trace>0);
if(!semidef) {
/* printf("%d: Adjusting correspondence weights\n",i);*/
double eps = -det;
c[k].C[0][0] += 2*sqrt(eps);
c[k].C[1][1] += 2*sqrt(eps);
}
#endif
} else {
c[k].p[0] = laser_sens->points[i].p[0];
c[k].p[1] = laser_sens->points[i].p[1];
projection_on_segment_d(
laser_ref->points[j1].p,
laser_ref->points[j2].p,
laser_sens->points_w[i].p,
c[k].q);
/* Identity matrix */
c[k].C[0][0] = 1;
c[k].C[1][0] = 0;
c[k].C[0][1] = 0;
c[k].C[1][1] = 1;
}
double factor = 1;
/* Scale the correspondence weight by a factor concerning the
information in this reading. */
if(params->use_ml_weights) {
int have_alpha = 0;
double alpha = 0;
if(!is_nan(laser_ref->true_alpha[j1])) {
alpha = laser_ref->true_alpha[j1];
have_alpha = 1;
} else if(laser_ref->alpha_valid[j1]) {
alpha = laser_ref->alpha[j1];;
have_alpha = 1;
} else have_alpha = 0;
if(have_alpha) {
double pose_theta = x_old[2];
/** Incidence of the ray
Note that alpha is relative to the first scan (not the world)
and that pose_theta is the angle of the second scan with
respect to the first, hence it's ok. */
double beta = alpha - (pose_theta + laser_sens->theta[i]);
factor = 1 / square(cos(beta));
} else {
static int warned_before = 0;
if(!warned_before) {
sm_error("Param use_ml_weights was active, but not valid alpha[] or true_alpha[]."
"Perhaps, if this is a single ray not having alpha, you should mark it as inactive.\n");
sm_error("Writing laser_ref: \n");
ld_write_as_json(laser_ref, stderr);
warned_before = 1;
}
}
}
/* Weight the points by the sigma in laser_sens */
if(params->use_sigma_weights) {
if(!is_nan(laser_sens->readings_sigma[i])) {
factor *= 1 / square(laser_sens->readings_sigma[i]);
} else {
static int warned_before = 0;
if(!warned_before) {
sm_error("Param use_sigma_weights was active, but the field readings_sigma[] was not filled in.\n");
sm_error("Writing laser_sens: \n");
ld_write_as_json(laser_sens, stderr);
}
}
}
c[k].C[0][0] *= factor;
c[k].C[1][0] *= factor;
c[k].C[0][1] *= factor;
c[k].C[1][1] *= factor;
k++;
}
/* TODO: use prior for odometry */
double std = 0.11;
const double inv_cov_x0[9] =
{1/(std*std), 0, 0,
0, 1/(std*std), 0,
0, 0, 0};
int ok = gpc_solve(k, c, 0, inv_cov_x0, x_new);
if(!ok) {
sm_error("gpc_solve_valid failed\n");
return 0;
}
double old_error = gpc_total_error(c, k, x_old);
double new_error = gpc_total_error(c, k, x_new);
sm_debug("\tcompute_next_estimate: old error: %f x_old= %s \n", old_error, friendly_pose(x_old));
sm_debug("\tcompute_next_estimate: new error: %f x_new= %s \n", new_error, friendly_pose(x_new));
sm_debug("\tcompute_next_estimate: new error - old_error: %g \n", new_error-old_error);
double epsilon = 0.000001;
if(new_error > old_error + epsilon) {
sm_error("\tcompute_next_estimate: something's fishy here! Old error: %lf new error: %lf x_old %lf %lf %lf x_new %lf %lf %lf\n",old_error,new_error,x_old[0],x_old[1],x_old[2],x_new[0],x_new[1],x_new[2]);
}
return 1;
}
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]);
const char *in_filename;
const char *ref_filename;
const char *out_filename;
const char *ref_field_string; ld_reference ref_field;
const char *out_field_string; ld_reference out_field;
struct option* ops = options_allocate(15);
options_string(ops, "in", &in_filename, "stdin", "scan matching log");
options_string(ops, "ref", &ref_filename, "ref.log", "slam log");
options_string(ops, "out", &out_filename, "stdout", "output file");
options_string(ops, "ref_field", &ref_field_string, "estimate", "What field to find in ref.");
options_string(ops, "out_field", &out_field_string, "true_pose", "What field to copy to.");
if(!options_parse_args(ops, argc, argv)) {
fprintf(stderr, " This program works on two logs: A and B. "
"For each scan in A, the program searches for the scan in B having the same timestamp. "
"Then, the true_pose field in B is copied to the scan form A, and it is written to the output.\n");
options_print_help(ops, stderr);
return -1;
}
ref_field = ld_string_to_reference(ref_field_string);
out_field = ld_string_to_reference(out_field_string);
FILE * in_stream = open_file_for_reading(in_filename);
FILE * ref_stream = open_file_for_reading(ref_filename);
FILE * out_stream = open_file_for_writing(out_filename);
if(!in_stream || !ref_stream || !out_stream) return -1;
LDP ld_in;
while((ld_in = ld_read_smart(in_stream))) {
int matched = 0;
while(1) {
LDP ld_ref = ld_read_smart(ref_stream);
if(!ld_ref) break;
if(same_scan(ld_in, ld_ref)) {
matched = 1;
const double *ref_pose = ld_get_reference_pose(ld_ref, ref_field);
double *out_pose = ld_get_reference_pose_silent(ld_in, out_field);
copy_d(ref_pose, 3, out_pose);
ld_write_as_json(ld_in, out_stream);
fputs("\n", out_stream);
break;
}
ld_free(ld_ref);
}
if(!matched) {
sm_error("Could not match %s. \n", short_desc(ld_in));
if(feof(ref_stream)) {
sm_error("..because ref stream has ended.\n");
break;
}
continue;
}
ld_free(ld_in);
}
return 0;
}
