diff --git a/sm/apps/json_extract.c b/sm/apps/json_extract.c index dce118885e53ac19a49684ebaf584a3a04709dd6..e3c012e28c5922f55ec252e9dabe2dae8835a03a 100644 --- a/sm/apps/json_extract.c +++ b/sm/apps/json_extract.c @@ -27,7 +27,7 @@ int main(int argc, const char * argv[]) { int i; for(i=0;i<nth;i++) { if(!json_stream_skip(input_stream)) { - fprintf(stderr, "Could not skip %d-th object\n", i); + sm_error("Could not skip %d-th object\n", i); return -2; } } diff --git a/sm/apps/json_extract_field.c b/sm/apps/json_extract_field.c index 97b0f3b3be6eb1f75ccd5c2b770a87e3a2de246d..8ebda07ddbc42a7701934b946b7da3bc52de7eea 100644 --- a/sm/apps/json_extract_field.c +++ b/sm/apps/json_extract_field.c @@ -9,10 +9,12 @@ int main(int argc, const char * argv[]) { const char*input_filename; const char*output_filename; const char*field; + int exit_on_error; struct option* ops = options_allocate(3); options_string(ops, "in", &input_filename, "stdin", "input file (JSON)"); options_string(ops, "out", &output_filename, "stdout", "output file (JSON)"); + options_int(ops, "exit_on_error", &exit_on_error, 0, "if true, exit if object has no field"); options_string(ops, "field", &field, "field_name", "field to extract from structure"); if(!options_parse_args(ops, argc, argv)) { @@ -27,8 +29,10 @@ int main(int argc, const char * argv[]) { if(!input_stream || !output_stream) return -1; - + int n=0; while(1) { + n++; + JO jo = json_read_stream(input_stream); if(!jo) { if(feof(input_stream)) break; @@ -38,8 +42,13 @@ int main(int argc, const char * argv[]) { JO jo_field = jo_get(jo, field); if(!jo_field) { - sm_error("Field '%s' not found in structure.\n", field); - return -1; + if(exit_on_error) { + sm_error("object #%d: field '%s' not found in structure.\n", n, field); + return -1; + } else { + sm_error("object #%d: field '%s' not found in structure.\n", n, field); + continue; + } } jo_write_plain(jo_field, output_stream); @@ -64,7 +73,7 @@ void jo_write_plain(JO jo, FILE* out) { } case json_type_double: { double v = json_object_get_double(jo); - fprintf(out, "%f", v); + fprintf(out, "%g", v); break; } case json_type_string: { diff --git a/sm/apps/sm1.c b/sm/apps/sm1.c index f0ee1ae230651da24756efaf61447ef4b994eb95..f89ef64e152dfda51bc1d9770649ed251de5eb21 100644 --- a/sm/apps/sm1.c +++ b/sm/apps/sm1.c @@ -13,12 +13,14 @@ struct sm1_params { int debug; int algo; + int write_post_mortem; } p; extern int distance_counter; extern void sm_options(struct sm_params*p, struct option*ops); + const char *sm1_banner = "There are TWO ways to define the input to this program.\n" @@ -67,9 +69,8 @@ int main(int argc, const char*argv[]) { 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, "write_post_mortem", &p.write_post_mortem, 1, "In case of failure, writes a post mortem."); - - sm_options(¶ms, ops); if(!options_parse_args(ops, argc, argv)) { fprintf(stderr, "\n\nUsage:\n"); @@ -84,7 +85,6 @@ int main(int argc, const char*argv[]) { : open_file_for_reading(p.file2); if(!file1 || !file2) return -1; - FILE * out = open_file_for_writing(p.file_output); if(!out) return -2; @@ -95,7 +95,10 @@ int main(int argc, const char*argv[]) { } LDP ld1, ld2; + int count = 0; while(1) { + count++; + ld1 = ld_from_json_stream(file1); if(!ld1) { if(feof(file1)) break; @@ -112,6 +115,15 @@ int main(int argc, const char*argv[]) { params.laser_ref = ld1; params.laser_sens = ld2; + 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(" (file %s) laser_ref->odometry = %s \n", p.file1, friendly_pose(params.laser_ref->odometry) ); + sm_error(" (file %s) laser_sens->odometry = %s \n", p.file2, friendly_pose(params.laser_sens->odometry) ); + sm_error(" I will quit it here. \n"); + return 3; + } + pose_diff_d( params.laser_sens->odometry, /* o minus */ params.laser_ref->odometry, /* = */ params.first_guess); @@ -129,6 +141,40 @@ int main(int argc, const char*argv[]) { return -1; } + if(p.write_post_mortem && !result.valid) { + char casename[256]; + sprintf(casename, "sm1_failure_matching%d", count); + sm_error("sm1: matching #%d failed. Writing a special case %s.\n", count, casename ); +// save_testcase(¶ms) + + char file_config[256],file1[256],file2[256],file_jj[256],script[256]; + + sprintf(file_config, "%s.config", casename); + sprintf(file1, "%s_laser_ref.json", casename); + sprintf(file2, "%s_laser_sens.json", casename); + sprintf(file_jj, "%s.journal", casename); + sprintf(script, "%s.sh", casename); + + FILE * f = fopen(file_config, "w"); + options_dump(ops,f,0); + fclose(f); + + f = fopen(file1, "w"); + ld_write_as_json(params.laser_ref,f); + fclose(f); + + f = fopen(file2, "w"); + ld_write_as_json(params.laser_sens,f); + fclose(f); + + f = fopen(script, "w"); + fprintf(f, "#!/bin/bash\n"); + fprintf(f, "%s -config %s -file1 %s -file2 %s -debug 1 -file_jj %s -write_post_mortem 0 \n", + argv[0], file_config, file1, file2, file_jj); + fprintf(f, "sm_animate -in %s -out %s_anim.pdf \n", file_jj, casename); + fclose(f); + } + JO jo = result_to_json(¶ms, &result); @@ -137,7 +183,6 @@ int main(int argc, const char*argv[]) { params.laser_ref->true_pose, true_x); double true_e[3]; - pose_diff_d(result.x, true_x, true_e); /* int i=0;for(;i<3;i++) true_e[i] = result.x[i] - true_x[i];*/ diff --git a/sm/apps/sm2.c b/sm/apps/sm2.c index ec926405af93401d4fa42368d527800d080ca0ba..b8f282a627e15c136f5247f0740dc0a061d7cfd9 100644 --- a/sm/apps/sm2.c +++ b/sm/apps/sm2.c @@ -104,6 +104,16 @@ int main(int argc, const char*argv[]) { 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]; diff --git a/sm/apps/sm_animate.c b/sm/apps/sm_animate.c index d5b045cd859c5a221d9192aac89d58e1011a0b33..7b0ad3b821d4c606e54737cc1297e6b0792cba62 100644 --- a/sm/apps/sm_animate.c +++ b/sm/apps/sm_animate.c @@ -76,6 +76,7 @@ int main(int argc, const char** argv) while( (jo = json_read_stream(input)) ) { char filename[100]; sprintf(filename, p.file_output, count); + sm_info("Writing frame %s \n", p.file_output); if(!draw_animation(&p, jo, filename)) return -2; count++; @@ -151,31 +152,32 @@ int draw_animation(anim_params* p, JO jo, const char*filename) { jo_read_double_array(iteration, "x_old", x_old, 3, NAN); jo_read_double_array(iteration, "x_new", x_new, 3, NAN); - JO corr0 = jo_get(iteration, "corr0"); - JO corr1 = jo_get(iteration, "corr1"); - JO corr2 = jo_get(iteration, "corr2"); - if(!corr1 || !corr2 || !corr0) { - sm_error("Iteration %d: could not read correspondences (field 'corr<i>'). Probably ICP failed here?\n", it); - break; - } cairo_save(cr); cr_ld_draw(cr, laser_ref, &(p->laser_ref_s)); ld_compute_world_coords(laser_sens, x_old); - cr_set_style(cr, &(p->corr)); - cairo_set_source_rgb (cr, 1.0, 0.0, 0.0); - json_to_corr(corr0, laser_sens->corr, laser_sens->nrays); - cr_ld_draw_corr(cr, laser_ref, laser_sens); - - cairo_set_source_rgb (cr, 1.0, 0.0, 1.0); - json_to_corr(corr1, laser_sens->corr, laser_sens->nrays); - cr_ld_draw_corr(cr, laser_ref, laser_sens); - cairo_set_source_rgb (cr, 0.0, 1.0, 0.0); - json_to_corr(corr2, laser_sens->corr, laser_sens->nrays); - cr_ld_draw_corr(cr, laser_ref, laser_sens); + JO corr0 = jo_get(iteration, "corr0"); + JO corr1 = jo_get(iteration, "corr1"); + JO corr2 = jo_get(iteration, "corr2"); + if(!corr1 || !corr2 || !corr0) { + sm_error("Iteration %d: could not read correspondences (field 'corr<i>'). Probably ICP failed here?\n", it); + } else { + cr_set_style(cr, &(p->corr)); + cairo_set_source_rgb (cr, 1.0, 0.0, 0.0); + json_to_corr(corr0, laser_sens->corr, laser_sens->nrays); + cr_ld_draw_corr(cr, laser_ref, laser_sens); + + cairo_set_source_rgb (cr, 1.0, 0.0, 1.0); + json_to_corr(corr1, laser_sens->corr, laser_sens->nrays); + cr_ld_draw_corr(cr, laser_ref, laser_sens); + + cairo_set_source_rgb (cr, 0.0, 1.0, 0.0); + json_to_corr(corr2, laser_sens->corr, laser_sens->nrays); + cr_ld_draw_corr(cr, laser_ref, laser_sens); + } cr_set_reference(cr, x_old); cr_ld_draw(cr, laser_sens, &(p->laser_sens_s)); diff --git a/sm/csm/icp/icp.c b/sm/csm/icp/icp.c index 7ef7d22e9b577162161b059b751fc4c087ad36b4..a96838ca90973ae0e9e7c1d2abaef55f3d71d653 100644 --- a/sm/csm/icp/icp.c +++ b/sm/csm/icp/icp.c @@ -90,7 +90,7 @@ void sm_icp(struct sm_params*params, struct sm_result*res) { int iterations; int nvalid; if(!icp_loop(params, x_old->data, x_new->data, &error, &nvalid, &iterations)) { - sm_error("ICP failed for some reason. \n"); + sm_error("icp: ICP failed for some reason. \n"); res->valid = 0; res->iterations = iterations; res->nvalid = 0; @@ -171,7 +171,6 @@ void sm_icp(struct sm_params*params, struct sm_result*res) { } } - res->error = best_error; res->iterations = iterations; res->nvalid = nvalid; diff --git a/sm/csm/icp/icp_loop.c b/sm/csm/icp/icp_loop.c index 58ec34a8c0f370b1a905752813b71a3e9a5a5abe..f64d17a9a321c76e90c47f6494f68967bd10161a 100644 --- a/sm/csm/icp/icp_loop.c +++ b/sm/csm/icp/icp_loop.c @@ -12,6 +12,12 @@ 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); @@ -29,6 +35,7 @@ int icp_loop(struct sm_params*params, const double*q0, double*x_new, 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 */ @@ -80,11 +87,11 @@ int icp_loop(struct sm_params*params, const double*q0, double*x_new, *total_error = error; *valid = num_corr_after; - sm_debug("Total error: %f valid %d mean = %f\n", *total_error, *valid, *total_error/ *valid); + 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("Failed: after trimming, only %d correspondences.\n",num_corr_after); + 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; @@ -92,7 +99,7 @@ int icp_loop(struct sm_params*params, const double*q0, double*x_new, /* Compute next estimate based on the correspondences */ if(!compute_next_estimate(params, x_old, x_new)) { - sm_error("Cannot compute next estimate.\n"); + sm_error(" icp_loop: Cannot compute next estimate.\n"); all_is_okay = 0; egsl_pop_named("icp_loop iteration"); break; @@ -101,7 +108,7 @@ int icp_loop(struct sm_params*params, const double*q0, double*x_new, pose_diff_d(x_new, x_old, delta); { - sm_debug("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); + 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); @@ -111,7 +118,7 @@ int icp_loop(struct sm_params*params, const double*q0, double*x_new, hashes[iteration] = ld_corr_hash(laser_sens); { - sm_debug("icp_loop: it. %d hash=%d nvalid=%d mean error = %f, x_new= %s\n", + 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)); } @@ -171,6 +178,9 @@ int compute_next_estimate(struct sm_params*params, 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; @@ -203,6 +213,9 @@ int compute_next_estimate(struct sm_params*params, 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. */ @@ -309,12 +322,13 @@ int compute_next_estimate(struct sm_params*params, double old_error = gpc_total_error(c, k, x_old); double new_error = gpc_total_error(c, k, x_new); - sm_debug("Old error: %f x_old= %s \n", old_error, friendly_pose(x_old)); - sm_debug("New error: %f x_new= %s \n", old_error, friendly_pose(x_old)); + 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("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]); + 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; diff --git a/sm/csm/icp/icp_outliers.c b/sm/csm/icp/icp_outliers.c index d595babe5baf9d1062895ecaee86a9a1040448dc..b4202e3bf606c23b713abf41a364650690874212 100644 --- a/sm/csm/icp/icp_outliers.c +++ b/sm/csm/icp/icp_outliers.c @@ -18,7 +18,7 @@ void visibilityTest(LDP laser_ref, const gsl_vector*u) { gvg(u,0)-laser_ref->points[j].p[0]); } - sm_debug("visibility: Found outliers: "); + sm_debug("\tvisibility: Found outliers: "); int invalid = 0; for(j=1;j<laser_ref->nrays;j++) { if(!ld_valid_ray(laser_ref,j)||!ld_valid_ray(laser_ref,j-1)) continue; @@ -67,7 +67,7 @@ void kill_outliers_double(struct sm_params*params) { nkilled ++; } } - sm_debug("kill_outliers_double: killed %d correspondences\n",nkilled); + sm_debug("\tkill_outliers_double: killed %d correspondences\n",nkilled); } /** @@ -150,7 +150,7 @@ void kill_outliers_trim(struct sm_params*params, double*total_error) { if(JJ) jj_add_double("error_limit_adaptive", error_limit2); if(JJ) jj_add_double("error_limit", error_limit); - sm_debug("icp_outliers: maxPerc %f error_limit: fix %f adaptive %f \n", + sm_debug("\ticp_outliers: maxPerc %f error_limit: fix %f adaptive %f \n", params->outliers_maxPerc,error_limit1,error_limit2); *total_error = 0; diff --git a/sm/csm/logging.c b/sm/csm/logging.c index 80b72823b89875a88715890538ec31de11ceda8e..647a6dbcb6d8c1645266c59bf805db2635bc8be4 100644 --- a/sm/csm/logging.c +++ b/sm/csm/logging.c @@ -33,7 +33,8 @@ void check_for_xterm_color() { const char * term = getenv("TERM"); if(!term) term = "unavailable"; - xterm_color_available = !strcmp(term, "xterm-color") || !strcmp(term, "xterm");; + xterm_color_available = !strcmp(term, "xterm-color") || !strcmp(term, "xterm") + || !strcmp(term, "rxvt"); /* sm_info("Terminal type: '%s', colors: %d\n", term, xterm_color_available); */ }