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]);
int period; int phase;
const char*input_filename;
const char*output_filename;
struct csm_option* ops = csm_options_allocate(3);
csm_options_int(ops, "period", &period, 1, "Period of objects to extract.");
csm_options_int(ops, "phase", &phase, 0, "Phase (=0 starts with the first object)");
csm_options_string(ops, "in", &input_filename, "stdin", "input file (JSON)");
csm_options_string(ops, "out", &output_filename, "stdout", "output file (JSON)");
if(!csm_options_parse_args(ops, argc, argv)) {
fprintf(stderr, "%s : decimates a JSON stream."
"\n\ncsm_options:\n", argv[0]);
csm_options_print_help(ops, stderr);
return -1;
}
if(period < 1) {
sm_error("Period must be >= 1.\n");
return 2;
}
FILE * input_stream = open_file_for_reading(input_filename);
FILE *output_stream = open_file_for_writing(output_filename);
if(!input_stream || !output_stream) return -1;
int count = 0;
while(1) {
JO jo = json_read_stream(input_stream);
if(!jo) {
if(feof(input_stream)) break;
sm_error("Malformed JSON\n");
return -1;
}
if( (count - phase) % period == 0) {
const char * s = json_object_to_json_string(jo);
fputs(s,output_stream); fputs("\n",output_stream);
}
jo_free(jo);
count++;
}
return 0;
}
int bbfind_compute(bbfind*bbf, BB2 bbox) {
double ul[2], ur[2], ll[2], lr[2];
if(1) {
if(!getBoundingBox(bbf->buf, bbf->num, ul, ur, ll, lr)) {
sm_error("Could not compute bounding box.\n");
return 0;
}
bbox->pose[0] = ll[0];
bbox->pose[1] = ll[1];
bbox->pose[2] = atan2(lr[1]-ll[1], lr[0]-ll[0]);
bbox->size[0] = distance_d(lr, ll);
bbox->size[1] = distance_d(ll, ul);
} else {
double bb_min[2] = {bbf->buf[0].x,bbf->buf[0].y},
bb_max[2] = {bbf->buf[0].x,bbf->buf[0].y};
int i; for(i=0;i<bbf->num; i++) {
bb_min[0] = GSL_MIN(bb_min[0], bbf->buf[i].x);
bb_min[1] = GSL_MIN(bb_min[1], bbf->buf[i].y);
bb_max[0] = GSL_MAX(bb_max[0], bbf->buf[i].x);
bb_max[1] = GSL_MAX(bb_max[1], bbf->buf[i].y);
}
bbox->pose[0] = bb_min[0];
bbox->pose[1] = bb_min[1];
bbox->pose[2] = 0;
bbox->size[0] = bb_max[0] - bb_min[0];
bbox->size[1] = bb_max[1] - bb_min[1];
}
return 1;
}
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 cluster_convolve(const int*cluster,const double*original, int n, double*dest, double*filter, int filter_len, int negate_negative)
{
int i; /* index on the points */
int j; /* index on the filter */
for(i=0;i<n;i++) {
if(cluster[i] == -1) {
dest[i] = GSL_NAN;
continue;
}
dest[i] = 0;
for(j=-(filter_len-1);j<=(filter_len-1);j++) {
int i2 = i + j;
if(i2<0) i2=0; if(i2>=n) i2=n-1;
if(cluster[i2] != cluster[i]) i2 = i;
double coeff = filter[abs(j)];
if(j<0 && negate_negative) coeff *= -1;
dest[i] += original[i2] * coeff;
if(is_nan(dest[i]))
sm_error("i: %d; something wrong after processing i2: %d cluster[i2]=%d original[i2] = %f \n", i, i2, cluster[i2], original[i2]);
}
}
}
FILE * open_file(const char *filename, const char*mode) {
FILE*file = fopen(filename, mode);
if(file==NULL) {
sm_error("Could not open file '%s': %s.\n", filename, strerror(errno));
return 0;
}
return file;
}
/** Returns 0 on success */
int read_next_double(const char*line, size_t*cur, double*d) {
int inc;
int ret = sscanf(line+*cur, " %lf %n", d, &inc);
if(1 != ret) {
sm_error("Could not read double at %p + %d '%s'. ret: %d.\n", line, *cur, line+*cur, ret);
return -1;
}
*cur += inc;
return 0;
}
bool json2tuple(const JO jo, calib_tuple&tuple) {
int res =
jo_read_double(jo, "T", &tuple.T) &&
jo_read_double(jo, "phi_l", &tuple.phi_l) &&
jo_read_double(jo, "phi_r", &tuple.phi_r) &&
jo_read_double_array (jo, "sm", tuple.sm, 3, GSL_NAN);
if(!res)
sm_error("Cannot read tuple form JSON.\n");
return res;
}
/* Init Opus encoder, return it on success. Print error to stdout, release wave resource and return NULL on error. */
OpusSM* init_opus(WAVE* wave) {
OpusSM* sm = sm_init(wave->header.SampleRate, wave->header.NumChannels);
if (sm_error(sm) != SM_OK) {
fprintf(stderr, "Opus encoder returned error on opus_encoder_create(). Error code: %d\n", sm_error(sm));
sm = sm_destroy(sm);
return NULL;
}
return sm;
}
int bbfind_add_point2(bbfind*bbf, double x, double y) {
if(bbf->num > bbf->buf_size - 2) {
bbf->buf_size *= 2;
if(! (bbf->buf = (BB_Point*) realloc(bbf->buf, sizeof(BB_Point)*bbf->buf_size)) ) {
sm_error("Cannot allocate (size=%d)\n", bbf->buf_size);
return 0;
}
}
bbf->buf[bbf->num].x = x;
bbf->buf[bbf->num].y = y;
bbf->num++;
return 1;
}
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 nth;
const char*input_filename;
const char*output_filename;
struct csm_option* ops = csm_options_allocate(3);
csm_options_int(ops, "nth", &nth, 0, "Index of object to extract.");
csm_options_string(ops, "in", &input_filename, "stdin", "input file (JSON)");
csm_options_string(ops, "out", &output_filename, "stdout", "output file (JSON)");
if(!csm_options_parse_args(ops, argc, argv)) {
fprintf(stderr, "%s : extracts n-th JSON object from stream."
"\n\ncsm_options:\n", argv[0]);
csm_options_print_help(ops, stderr);
return -1;
}
FILE * input_stream = open_file_for_reading(input_filename);
FILE *output_stream = open_file_for_writing(output_filename);
if(!input_stream || !output_stream) return -1;
int i; for(i=0;i<nth;i++) {
if(!json_stream_skip(input_stream)) {
sm_error("Could not skip %d-th object\n", i);
return -2;
}
}
JO jo = json_read_stream(input_stream);
if(!jo) {
fprintf(stderr, "Could not read %d-th object (after skipping %d)\n",
nth, i);
return -2;
}
fputs(json_object_to_json_string(jo), output_stream);
fputs("\n", output_stream);
return 0;
}
static void nble_security_reply(struct bt_conn *conn,
struct nble_sm_passkey *par)
{
struct nble_sm_passkey_reply_req rsp = {
.conn = conn,
.conn_handle = conn->handle,
};
memcpy(&rsp.params, par, sizeof(*par));
nble_sm_passkey_reply_req(&rsp);
}
static int sm_error(struct bt_conn *conn, uint8_t reason)
{
struct nble_sm_passkey params;
params.type = NBLE_GAP_SM_REJECT;
params.reason = reason;
nble_security_reply(conn, ¶ms);
return 0;
}
static void legacy_passkey_entry(struct bt_smp *smp, unsigned int passkey)
{
struct nble_sm_passkey pkey = {
.type = NBLE_SM_PK_PASSKEY,
.passkey = passkey,
};
nble_security_reply(smp->conn, &pkey);
}
int bt_smp_auth_cancel(struct bt_conn *conn)
{
BT_DBG("");
return sm_error(conn, BT_SMP_ERR_PASSKEY_ENTRY_FAILED);
}
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 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 icp_loop(struct sm_params*params, const double*q0, double*x_new,
double*total_error, int*valid, int*iterations) {
if(any_nan(q0,3)) {
sm_error("icp_loop: Initial pose contains nan: %s\n", friendly_pose(q0));
return 0;
}
LDP laser_sens = params->laser_sens;
double x_old[3], delta[3], delta_old[3] = {0,0,0};
copy_d(q0, 3, x_old);
unsigned int hashes[params->max_iterations];
int iteration;
sm_debug("icp: starting at q0 = %s \n", friendly_pose(x_old));
if(JJ) jj_loop_enter("iterations");
int all_is_okay = 1;
for(iteration=0; iteration<params->max_iterations;iteration++) {
if(JJ) jj_loop_iteration();
if(JJ) jj_add_double_array("x_old", x_old, 3);
egsl_push_named("icp_loop iteration");
sm_debug("== icp_loop: starting iteration. %d \n", iteration);
/** Compute laser_sens's points in laser_ref's coordinates
by roto-translating by x_old */
ld_compute_world_coords(laser_sens, x_old);
/** Find correspondences (the naif or smart way) */
if(params->use_corr_tricks)
find_correspondences_tricks(params);
else
find_correspondences(params);
/** If debug_verify_tricks, make sure that find_correspondences_tricks()
and find_correspondences() return the same answer */
if(params->debug_verify_tricks)
debug_correspondences(params);
/* If not many correspondences, bail out */
int num_corr = ld_num_valid_correspondences(laser_sens);
double fail_perc = 0.05;
if(num_corr < fail_perc * laser_sens->nrays) { /* TODO: arbitrary */
sm_error(" : before trimming, only %d correspondences.\n",num_corr);
all_is_okay = 0;
egsl_pop_named("icp_loop iteration"); /* loop context */
break;
}
if(JJ) jj_add("corr0", corr_to_json(laser_sens->corr, laser_sens->nrays));
/* Kill some correspondences (using dubious algorithm) */
if(params->outliers_remove_doubles)
kill_outliers_double(params);
int num_corr2 = ld_num_valid_correspondences(laser_sens);
if(JJ) jj_add("corr1", corr_to_json(laser_sens->corr, laser_sens->nrays));
double error=0;
/* Trim correspondences */
kill_outliers_trim(params, &error);
int num_corr_after = ld_num_valid_correspondences(laser_sens);
if(JJ) {
jj_add("corr2", corr_to_json(laser_sens->corr, laser_sens->nrays));
jj_add_int("num_corr0", num_corr);
jj_add_int("num_corr1", num_corr2);
jj_add_int("num_corr2", num_corr_after);
}
*total_error = error;
*valid = num_corr_after;
sm_debug(" icp_loop: total error: %f valid %d mean = %f\n", *total_error, *valid, *total_error/ *valid);
/* If not many correspondences, bail out */
if(num_corr_after < fail_perc * laser_sens->nrays){
sm_error(" icp_loop: failed: after trimming, only %d correspondences.\n",num_corr_after);
all_is_okay = 0;
egsl_pop_named("icp_loop iteration"); /* loop context */
break;
}
/* Compute next estimate based on the correspondences */
if(!compute_next_estimate(params, x_old, x_new)) {
sm_error(" icp_loop: Cannot compute next estimate.\n");
all_is_okay = 0;
egsl_pop_named("icp_loop iteration");
break;
}
pose_diff_d(x_new, x_old, delta);
{
sm_debug(" icp_loop: killing. laser_sens has %d/%d rays valid, %d corr found -> %d after double cut -> %d after adaptive cut \n", count_equal(laser_sens->valid, laser_sens->nrays, 1), laser_sens->nrays, num_corr, num_corr2, num_corr_after);
if(JJ) {
jj_add_double_array("x_new", x_new, 3);
jj_add_double_array("delta", delta, 3);
}
}
/** Checks for oscillations */
hashes[iteration] = ld_corr_hash(laser_sens);
{
sm_debug(" icp_loop: it. %d hash=%d nvalid=%d mean error = %f, x_new= %s\n",
iteration, hashes[iteration], *valid, *total_error/ *valid,
friendly_pose(x_new));
}
/** PLICP terminates in a finite number of steps! */
if(params->use_point_to_line_distance) {
int loop_detected = 0; /* TODO: make function */
int a; for(a=iteration-1;a>=0;a--) {
if(hashes[a]==hashes[iteration]) {
sm_debug("icpc: oscillation detected (cycle length = %d)\n", iteration-a);
loop_detected = 1;
break;
}
}
if(loop_detected) {
egsl_pop_named("icp_loop iteration");
break;
}
}
/* This termination criterium is useless when using
the point-to-line-distance; however, we put it here because
one can choose to use the point-to-point distance. */
if(termination_criterion(params, delta)) {
egsl_pop_named("icp_loop iteration");
break;
}
copy_d(x_new, 3, x_old);
copy_d(delta, 3, delta_old);
egsl_pop_named("icp_loop iteration");
}
if(JJ) jj_loop_exit();
*iterations = iteration+1;
return all_is_okay;
}
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 log2pdf(log2pdf_params *p) {
/** First of all, we read the entire map into memory */
FILE *input_file = open_file_for_reading(p->input_filename);
if(!input_file) return 0;
LDP*scans; int nscans;
if(!ld_read_some_scans_distance(input_file, &scans, &nscans,
p->use_reference, p->distance_xy, deg2rad(p->distance_th_deg) ) ){
sm_error("Could not read map from file '%s'.\n", p->input_filename);
return 0;
}
if(nscans == 0) {
sm_error("I could not read any scan from file '%s'.\n", p->input_filename);
return 0;
}
sm_debug("Read map: %d scans in total.\n", nscans);
/** Let's find the bounding box for the map */
double bb_min[2], bb_max[2];
double offset[3] = {0,0,0};
lda_get_bounding_box(scans, nscans, bb_min, bb_max, offset, p->use_reference, p->laser.horizon);
bb_min[0] -= p->padding;
bb_min[1] -= p->padding;
bb_max[0] += p->padding;
bb_max[1] += p->padding;
sm_debug("Bounding box: %f %f -- %f %f.\n", bb_min[0], bb_min[1],
bb_max[0], bb_max[1]);
/* Create PDF surface and setup paper size and transformations */
int max_width_points = p->dimension;
int max_height_points = p->dimension;
cairo_surface_t *surface;
cairo_t *cr;
if(!create_pdf_surface(p->output_filename, max_width_points, max_height_points,
bb_min, bb_max, &surface, &cr)) return 0;
/* Draw pose path */
if(p->pose_path.draw) {
cairo_save(cr);
cr_set_style(cr, &(p->pose_path));
cr_lda_draw_pose_path(cr, scans, nscans, p->use_reference);
if(nscans > 0 && p->laser.pose.draw) {
cairo_set_source_rgb(cr, 0.3, 0.0, 1.0);
double *pose0 = ld_get_reference_pose(scans[0], p->use_reference);
cairo_arc(cr, pose0[0], pose0[1], p->start_pose_width, 0.0, 2*M_PI);
cairo_fill(cr);
}
cairo_restore(cr);
}
/* Draw map */
int k; for(k=0;k<nscans;k++) {
LDP ld = scans[k];
double *pose = ld_get_reference_pose(ld, p->use_reference);
if(!pose) continue;
double offset[3] = {0,0, deg2rad(p->offset_theta_deg) };
double world_pose[3];
oplus_d(offset, pose, world_pose);
cairo_save(cr);
cr_set_reference(cr, world_pose);
cr_ld_draw(cr, ld, &(p->laser));
cairo_restore(cr);
}
cairo_show_page (cr);
cairo_destroy (cr);
cairo_surface_destroy (surface);
return 1;
}
LDP ld_from_carmen_string(const char*line) {
if(0 != strncmp(line, carmen_prefix, strlen(carmen_prefix))) {
sm_error("This is not a Carmen line: \n-> %s\n", line);
return 0;
}
size_t cur = strlen(carmen_prefix);
int nrays=-1;
if(read_next_integer(line, &cur, &nrays)) {
sm_error("Could not get number of rays.\n");
goto error;
}
LDP ld = ld_alloc_new(nrays);
double fov = M_PI;
double min_reading = 0;
double max_reading = 80;
if(nrays == 769) {
min_reading = 0.001;
max_reading = 4;
fov = deg2rad(270.0);
static int print = 0;
if(!print) { print = 1;
sm_info("Assuming that 769 rays is an Hokuyo "
"with fov = %f deg, min_reading = %f m, max_reading = %fm\n",
rad2deg(fov), min_reading, max_reading);
}
}
ld->min_theta = -fov/2;
ld->max_theta = +fov/2;
int on_error = 0;
int i;
for(i=0;i<nrays;i++) {
double reading;
if(read_next_double(line,&cur,&reading)) {
sm_error("Could not read ray #%d / %d, \n", i, nrays);
on_error = 1;
break;
}
ld->valid[i] = (reading > min_reading) && (reading < max_reading);
ld->readings[i] = ld->valid[i] ? reading : NAN;
ld->theta[i] = ld->min_theta + i *
(ld->max_theta-ld->min_theta) / (ld->nrays-1);
/* bad hokuyo!! */
if(nrays == 769) {
if(i>725 || i<44) {
ld->valid[i] = 0;
ld->readings[i] = NAN;
}
}
}
if(on_error) goto error;
if(read_next_double(line,&cur,ld->estimate+0)) goto error;
if(read_next_double(line,&cur,ld->estimate+1)) goto error;
if(read_next_double(line,&cur,ld->estimate+2)) goto error;
if(read_next_double(line,&cur,ld->odometry+0)) goto error;
if(read_next_double(line,&cur,ld->odometry+1)) goto error;
if(read_next_double(line,&cur,ld->odometry+2)) goto error;
/* Following: ipc_timestamp hostname timestamp */
/* Two csm_options:
double string double:
the first is timestamp in seconds, the second is discarded
int string int:
the first is sec, the second is usec
*/
static int warn_format = 1;
int inc; int sec=-1, usec=-1;
int res = sscanf(line + cur, "%d %s %d%n", &sec, ld->hostname, &usec, &inc);
if(3 == res) {
ld->tv.tv_sec = sec;
ld->tv.tv_usec = usec;
if(warn_format)
sm_info("Reading timestamp as 'sec hostname usec'.\n");
} else {
double v1=-1, v2=-1;
res = sscanf(line + cur, "%lf %s %lf%n", &v1, ld->hostname, &v2, &inc);
if(3 == res) {
ld->tv.tv_sec = (int) floor(v1);
ld->tv.tv_usec = floor( (v1 - floor(v1)) * 1e6 );
if(warn_format)
sm_info("Reading timestamp as doubles (discarding second one).\n");
} else {
ld->tv.tv_sec = 0;
ld->tv.tv_usec = 0;
if(warn_format)
sm_info("I could not read timestamp+hostname; ignoring (I will warn only once for this).\n");
}
}
warn_format = 0;
fprintf(stderr, "l");
return ld;
error:
printf("Malformed line: '%s'\nat cur = %d\n\t-> '%s'\n", line,(int)cur,line+cur);
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;
}
// computes the minimal bounding box for a set of 2d-points
// ul = upper left point, ll = lower left point,
// ur = upper right point, ul = upper left point
int getBoundingBox(BB_Point* p, int nOfPoints,
double ul[2], double ur[2], double ll[2], double lr[2]) {
// calculate the center of all points (schwerpunkt)
// -------------------------------------------------
double centerx = 0;
double centery = 0;
for (int i=0; i < nOfPoints; i++) {
centerx += p[i].x;
centery += p[i].y;
}
centerx /= (double) nOfPoints;
centery /= (double) nOfPoints;
// calcutae the covariance matrix
// -------------------------------
// covariance matrix (x1 x2, x3 x4)
double x1 = 0.0;
double x2 = 0.0;
double x3 = 0.0;
double x4 = 0.0;
for (int i=0; i < nOfPoints; i++) {
double cix = p[i].x - centerx;
double ciy = p[i].y - centery;
x1 += cix*cix;
x2 += cix*ciy;
x4 += ciy*ciy;
}
x1 /= (double) nOfPoints;
x2 /= (double) nOfPoints;
x3 = x2;
x4 /= (double) nOfPoints;
// covariance & center done
// calculate the eigenvectors
// ---------------------------
// catch 1/0 or sqrt(<0)
if ((x3 == 0) || (x2 == 0)|| (x4*x4-2*x1*x4+x1*x1+4*x2*x3 < 0 )) {
sm_error("Cyrill: Could not compute bounding box.\n");
return 0;
}
// eigenvalues
double lamda1 = 0.5* (x4 + x1 + sqrt(x4*x4 - 2.0*x1*x4 + x1*x1 + 4.0*x2*x3));
double lamda2 = 0.5* (x4 + x1 - sqrt(x4*x4 - 2.0*x1*x4 + x1*x1 + 4.0*x2*x3));
// eigenvector 1 with (x,y)
double v1x = - (x4-lamda1) * (x4-lamda1) * (x1-lamda1) / (x2 * x3 * x3);
double v1y = (x4-lamda1) * (x1-lamda1) / (x2 * x3);
// eigenvector 2 with (x,y)
double v2x = - (x4-lamda2) * (x4-lamda2) * (x1-lamda2) / (x2 * x3 * x3);
double v2y = (x4-lamda2) * (x1-lamda2) / (x2 * x3);
// norm the eigenvectors
double lv1 = sqrt ( (v1x*v1x) + (v1y*v1y) );
double lv2 = sqrt ( (v2x*v2x) + (v2y*v2y) );
v1x /= lv1;
v1y /= lv1;
v2x /= lv2;
v2y /= lv2;
// eigenvectors done
// get the points with maximal dot-product
double x = 0.0;
double y = 0.0;
double xmin = 1e20;
double xmax = -1e20;
double ymin = 1e20;
double ymax = -1e20;
for(int i = 0; i< nOfPoints; i++) {
// dot-product of relativ coordinates of every point
x = (p[i].x - centerx) * v1x + (p[i].y - centery) * v1y;
y = (p[i].x - centerx) * v2x + (p[i].y - centery) * v2y;
if( x > xmax) xmax = x;
if( x < xmin) xmin = x;
if( y > ymax) ymax = y;
if( y < ymin) ymin = y;
}
// now we can compute the corners of the bounding box
if(ul) {
ul[0] = centerx + xmin * v1x + ymin * v2x;
ul[1] = centery + xmin * v1y + ymin * v2y;
}
if(ur) {
ur[0] = centerx + xmax * v1x + ymin * v2x;
ur[1] = centery + xmax * v1y + ymin * v2y;
}
if(ll) {
ll[0] = centerx + xmin * v1x + ymax * v2x;
ll[1] = centery + xmin * v1y + ymax * v2y;
}
if(lr) {
lr[0] = centerx + xmax * v1x + ymax * v2x;
lr[1] = centery + xmax * v1y + ymax * v2y;
}
return 1;
}
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 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;
}
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;
}
void hsm_match(struct hsm_params*p, hsm_buffer b1, hsm_buffer b2) {
sm_log_push("hsm_match");
/* Let's measure the time */
clock_t hsm_match_start = clock();
assert(b1->num_angular_cells == b2->num_angular_cells);
assert(p->max_translation > 0);
assert(b1->linear_cell_size > 0);
b1->num_valid_results = 0;
/* Compute cross-correlation of spectra */
hsm_circular_cross_corr_stupid(b1->num_angular_cells, b2->hs, b1->hs, b1->hs_cross_corr);
/* Find peaks in cross-correlation */
int peaks[p->num_angular_hypotheses], npeaks;
hsm_find_peaks_circ(b1->num_angular_cells, b1->hs_cross_corr, p->angular_hyp_min_distance_deg, 0, p->num_angular_hypotheses, peaks, &npeaks);
sm_debug("Found %d peaks (max %d) in cross correlation.\n", npeaks, p->num_angular_hypotheses);
if(npeaks == 0) {
sm_error("Cross correlation of spectra has 0 peaks.\n");
sm_log_pop();
return;
}
sm_log_push("loop on theta hypotheses");
/* lag e' quanto 2 si sposta a destra rispetto a 1 */
for(int np=0;np<npeaks;np++) {
int lag = peaks[np];
double theta_hypothesis = lag * (2*M_PI/b1->num_angular_cells);
sm_debug("Theta hyp#%d: lag %d, angle %fdeg\n", np, lag, rad2deg(theta_hypothesis));
/* Superimpose the two spectra */
double mult[b1->num_angular_cells];
for(int r=0;r<b1->num_angular_cells;r++)
mult[r] = b1->hs[r] * b2->hs[pos_mod(r-lag, b1->num_angular_cells)];
/* Find directions where both are intense */
int directions[p->xc_ndirections], ndirections;
hsm_find_peaks_circ(b1->num_angular_cells, b1->hs_cross_corr, p->xc_directions_min_distance_deg, 1, p->xc_ndirections, directions, &ndirections);
if(ndirections<2) {
sm_error("Too few directions.\n");
}
struct {
/* Direction of cross correlation */
double angle;
int nhypotheses;
struct {
double delta;
double value;
} hypotheses[p->linear_xc_max_npeaks];
} dirs[ndirections];
sm_debug("Using %d (max %d) correlations directions.\n", ndirections, p->xc_ndirections);
int max_lag = (int) ceil(p->max_translation / b1->linear_cell_size);
int min_lag = -max_lag;
sm_debug("Max lag: %d cells (max t: %f, cell size: %f)\n",
max_lag, p->max_translation, b1->linear_cell_size);
sm_log_push("loop on xc direction");
/* For each correlation direction */
for(int cd=0;cd<ndirections;cd++) {
dirs[cd].angle = theta_hypothesis + (directions[cd]) * (2*M_PI/b1->num_angular_cells);
printf(" cd %d angle = %d deg\n", cd, (int) rad2deg(dirs[cd].angle));
/* Do correlation */
int lags [2*max_lag + 1];
double xcorr [2*max_lag + 1];
int i1 = pos_mod(directions[cd] , b1->num_angular_cells);
int i2 = pos_mod(directions[cd] + lag , b1->num_angular_cells);
double *f1 = b1->ht[i1];
double *f2 = b2->ht[i2];
hsm_linear_cross_corr_stupid(
b2->num_linear_cells,f2,
b1->num_linear_cells,f1,
xcorr, lags, min_lag, max_lag);
/* Find peaks of cross-correlation */
int linear_peaks[p->linear_xc_max_npeaks], linear_npeaks;
hsm_find_peaks_linear(
2*max_lag + 1, xcorr, p->linear_xc_peaks_min_distance/b1->linear_cell_size,
p->linear_xc_max_npeaks, linear_peaks, &linear_npeaks);
sm_debug("theta hyp #%d: Found %d (max %d) peaks for correlation.\n",
cd, linear_npeaks, p->linear_xc_max_npeaks);
dirs[cd].nhypotheses = linear_npeaks;
sm_log_push("Considering each peak of linear xc");
for(int lp=0;lp<linear_npeaks;lp++) {
int linear_xc_lag = lags[linear_peaks[lp]];
double value = xcorr[linear_peaks[lp]];
double linear_xc_lag_m = linear_xc_lag * b1->linear_cell_size;
sm_debug("lag: %d delta: %f value: %f \n", linear_xc_lag, linear_xc_lag_m, value);
dirs[cd].hypotheses[lp].delta = linear_xc_lag_m;
dirs[cd].hypotheses[lp].value = value;
}
sm_log_pop();
if(p->debug_true_x_valid) {
double true_delta = cos(dirs[cd].angle) * p->debug_true_x[0] +
sin(dirs[cd].angle) * p->debug_true_x[1];
sm_debug("true_x delta = %f \n", true_delta );
}
} /* xc direction */
sm_log_pop();
sm_debug("Now doing all combinations. How many are there?\n");
int possible_choices[ndirections];
int num_combinations = 1;
for(int cd=0;cd<ndirections;cd++) {
possible_choices[cd] = dirs[cd].nhypotheses;
num_combinations *= dirs[cd].nhypotheses;
}
sm_debug("Total: %d combinations\n", num_combinations);
sm_log_push("For each combination..");
for(int comb=0;comb<num_combinations;comb++) {
int choices[ndirections];
hsm_generate_combinations(ndirections, possible_choices, comb, choices);
/* Linear least squares */
double M[2][2]={{0,0},{0,0}}; double Z[2]={0,0};
/* heuristic quality value */
double sum_values = 0;
for(int cd=0;cd<ndirections;cd++) {
double angle = dirs[cd].angle;
double c = cos(angle), s = sin(angle);
double w = dirs[cd].hypotheses[choices[cd]].value;
double y = dirs[cd].hypotheses[choices[cd]].delta;
M[0][0] += c * c * w;
M[1][0] += c * s * w;
M[0][1] += c * s * w;
M[1][1] += s * s * w;
Z[0] += w * c * y;
Z[1] += w * s * y;
sum_values += w;
}
double det = M[0][0]*M[1][1]-M[0][1]*M[1][0];
double Minv[2][2];
Minv[0][0] = M[1][1] * (1/det);
Minv[1][1] = M[0][0] * (1/det);
Minv[0][1] = -M[0][1] * (1/det);
Minv[1][0] = -M[1][0] * (1/det);
double t[2] = {
Minv[0][0]*Z[0] + Minv[0][1]*Z[1],
Minv[1][0]*Z[0] + Minv[1][1]*Z[1]};
/* copy result in results slot */
int k = b1->num_valid_results;
b1->results[k][0] = t[0];
b1->results[k][1] = t[1];
b1->results[k][2] = theta_hypothesis;
b1->results_quality[k] = sum_values;
b1->num_valid_results++;
}
sm_log_pop();
} /* theta hypothesis */
sm_log_pop();
/* for(int i=0;i<b1->num_valid_results;i++) {
printf("#%d %.0fdeg %.1fm %.1fm quality %f \n",i,
rad2deg(b1->results[i][2]),
b1->results[i][0],
b1->results[i][1],
b1->results_quality[i]);
}*/
/* Sorting based on values */
int indexes[b1->num_valid_results];
for(int i=0;i<b1->num_valid_results;i++)
indexes[i] = i;
qsort_descending(indexes, (size_t) b1->num_valid_results, b1->results_quality);
/* copy in the correct order*/
double*results_tmp[b1->num_valid_results];
double results_quality_tmp[b1->num_valid_results];
for(int i=0;i<b1->num_valid_results;i++) {
results_tmp[i] = b1->results[i];
results_quality_tmp[i] = b1->results_quality[i];
}
for(int i=0;i<b1->num_valid_results;i++) {
b1->results[i] = results_tmp[indexes[i]];
b1->results_quality[i] = results_quality_tmp[indexes[i]];
}
for(int i=0;i<b1->num_valid_results;i++) {
char near[256]="";
double *x = b1->results[i];
if(p->debug_true_x_valid) {
double err_th = rad2deg(fabs(angleDiff(p->debug_true_x[2],x[2])));
double err_m = hypot(p->debug_true_x[0]-x[0],
p->debug_true_x[1]-x[1]);
const char * ast = (i == 0) && (err_th > 2) ? " ***** " : "";
sprintf(near, "th err %4d err_m %5f %s",(int)err_th ,err_m,ast);
}
if(i<10)
printf("after #%d %3.1fm %.1fm %3.0fdeg quality %5.0f \t%s\n",i,
x[0],
x[1], rad2deg(x[2]), b1->results_quality[i], near);
}
/* How long did it take? */
clock_t hsm_match_stop = clock();
int ticks = hsm_match_stop-hsm_match_start;
double ctime = ((double)ticks) / CLOCKS_PER_SEC;
sm_debug("Time: %f sec (%d ticks)\n", ctime, ticks);
sm_log_pop();
}
