Skip to content
Snippets Groups Projects
Commit 19fa2c71 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

changed errors to debug msgs

parent 8b5e59de
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
sm/csm/icp/icp.c
+ 4
− 4
View file @ 19fa2c71
...@@ -90,7 +90,7 @@ void sm_icp(struct sm_params*params, struct sm_result*res) { ...@@ -90,7 +90,7 @@ void sm_icp(struct sm_params*params, struct sm_result*res) {
int iterations; int iterations;
int nvalid; int nvalid;
if(!icp_loop(params, x_old->data, x_new->data, &error, &nvalid, &iterations)) { if(!icp_loop(params, x_old->data, x_new->data, &error, &nvalid, &iterations)) {
sm_error("icp: ICP failed for some reason. \n"); sm_debug("icp: ICP failed for some reason. \n");
res->valid = 0; res->valid = 0;
res->iterations = iterations; res->iterations = iterations;
res->nvalid = 0; res->nvalid = 0;
...@@ -127,7 +127,7 @@ void sm_icp(struct sm_params*params, struct sm_result*res) { ...@@ -127,7 +127,7 @@ void sm_icp(struct sm_params*params, struct sm_result*res) {
gsl_vector * x_a = gsl_vector_alloc(3); gsl_vector * x_a = gsl_vector_alloc(3);
double my_error; int my_valid; int my_iterations; 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)){ 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); sm_debug("Error during restart #%d/%d. \n", a, 6);
break; break;
} }
iterations+=my_iterations; iterations+=my_iterations;
...@@ -263,7 +263,7 @@ void sm_icp_xy(struct sm_params*params, struct sm_result*res) ...@@ -263,7 +263,7 @@ void sm_icp_xy(struct sm_params*params, struct sm_result*res)
int iterations; int iterations;
int nvalid; int nvalid;
if(!icp_loop(params, x_old->data, x_new->data, &error, &nvalid, &iterations)) { if(!icp_loop(params, x_old->data, x_new->data, &error, &nvalid, &iterations)) {
sm_error("icp: ICP failed for some reason. \n"); sm_debug("icp: ICP failed for some reason. \n");
res->valid = 0; res->valid = 0;
res->iterations = iterations; res->iterations = iterations;
res->nvalid = 0; res->nvalid = 0;
...@@ -298,7 +298,7 @@ void sm_icp_xy(struct sm_params*params, struct sm_result*res) ...@@ -298,7 +298,7 @@ void sm_icp_xy(struct sm_params*params, struct sm_result*res)
gsl_vector * x_a = gsl_vector_alloc(3); gsl_vector * x_a = gsl_vector_alloc(3);
double my_error; int my_valid; int my_iterations; 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)){ 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); sm_debug("Error during restart #%d/%d. \n", a, 6);
break; break;
} }
iterations+=my_iterations; iterations+=my_iterations;
......
sm/csm/icp/icp_debug.c
+ 3
− 3
View file @ 19fa2c71
...@@ -12,13 +12,13 @@ void debug_correspondences(struct sm_params * params) { ...@@ -12,13 +12,13 @@ void debug_correspondences(struct sm_params * params) {
find_correspondences(params); find_correspondences(params);
long hash2 = ld_corr_hash(laser_sens); long hash2 = ld_corr_hash(laser_sens);
if(hash1 != hash2) { if(hash1 != hash2) {
sm_error("find_correspondences_tricks might be buggy\n"); sm_debug("find_correspondences_tricks might be buggy\n");
int i = 0; for(i=0;i<laser_sens->nrays;i++) { int i = 0; for(i=0;i<laser_sens->nrays;i++) {
if( (c1[i].valid != c2[i].valid) || if( (c1[i].valid != c2[i].valid) ||
(c1[i].j1 != c2[i].j1) || (c1[i].j2 != c2[i].j2) ) { (c1[i].j1 != c2[i].j1) || (c1[i].j2 != c2[i].j2) ) {
sm_error("\t tricks: c1[%d].valid = %d j1 = %d j2 = %d dist2_j1 = %f\n", sm_debug("\t tricks: c1[%d].valid = %d j1 = %d j2 = %d dist2_j1 = %f\n",
i, c1[i].valid, c1[i].j1, c1[i].j2, c1[i].dist2_j1); i, c1[i].valid, c1[i].j1, c1[i].j2, c1[i].dist2_j1);
sm_error("\tno tricks: c2[%d].valid = %d j1 = %d j2 = %d dist2_j1 = %f\n", sm_debug("\tno tricks: c2[%d].valid = %d j1 = %d j2 = %d dist2_j1 = %f\n",
i, c2[i].valid, c2[i].j1, c2[i].j2, c2[i].dist2_j1); i, c2[i].valid, c2[i].j1, c2[i].j2, c2[i].dist2_j1);
} }
} }
......
sm/csm/icp/icp_loop.c
+ 10
− 10
View file @ 19fa2c71
...@@ -13,7 +13,7 @@ ...@@ -13,7 +13,7 @@
int icp_loop(struct sm_params*params, const double*q0, double*x_new, int icp_loop(struct sm_params*params, const double*q0, double*x_new,
double*total_error, int*valid, int*iterations) { double*total_error, int*valid, int*iterations) {
if(any_nan(q0,3)) { if(any_nan(q0,3)) {
sm_error("icp_loop: Initial pose contains nan: %s\n", friendly_pose(q0)); sm_debug("icp_loop: Initial pose contains nan: %s\n", friendly_pose(q0));
return 0; return 0;
} }
...@@ -56,7 +56,7 @@ int icp_loop(struct sm_params*params, const double*q0, double*x_new, ...@@ -56,7 +56,7 @@ int icp_loop(struct sm_params*params, const double*q0, double*x_new,
int num_corr = ld_num_valid_correspondences(laser_sens); int num_corr = ld_num_valid_correspondences(laser_sens);
double fail_perc = 0.05; double fail_perc = 0.05;
if(num_corr < fail_perc * laser_sens->nrays) { /* TODO: arbitrary */ if(num_corr < fail_perc * laser_sens->nrays) { /* TODO: arbitrary */
sm_error(" : before trimming, only %d correspondences.\n",num_corr); sm_debug(" : before trimming, only %d correspondences.\n",num_corr);
all_is_okay = 0; all_is_okay = 0;
egsl_pop_named("icp_loop iteration"); /* loop context */ egsl_pop_named("icp_loop iteration"); /* loop context */
break; break;
...@@ -91,7 +91,7 @@ int icp_loop(struct sm_params*params, const double*q0, double*x_new, ...@@ -91,7 +91,7 @@ int icp_loop(struct sm_params*params, const double*q0, double*x_new,
/* If not many correspondences, bail out */ /* If not many correspondences, bail out */
if(num_corr_after < fail_perc * laser_sens->nrays){ if(num_corr_after < fail_perc * laser_sens->nrays){
sm_error(" icp_loop: failed: after trimming, only %d correspondences.\n",num_corr_after); sm_debug(" icp_loop: failed: after trimming, only %d correspondences.\n",num_corr_after);
all_is_okay = 0; all_is_okay = 0;
egsl_pop_named("icp_loop iteration"); /* loop context */ egsl_pop_named("icp_loop iteration"); /* loop context */
break; break;
...@@ -99,7 +99,7 @@ int icp_loop(struct sm_params*params, const double*q0, double*x_new, ...@@ -99,7 +99,7 @@ int icp_loop(struct sm_params*params, const double*q0, double*x_new,
/* Compute next estimate based on the correspondences */ /* Compute next estimate based on the correspondences */
if(!compute_next_estimate(params, x_old, x_new)) { if(!compute_next_estimate(params, x_old, x_new)) {
sm_error(" icp_loop: Cannot compute next estimate.\n"); sm_debug(" icp_loop: Cannot compute next estimate.\n");
all_is_okay = 0; all_is_okay = 0;
egsl_pop_named("icp_loop iteration"); egsl_pop_named("icp_loop iteration");
break; break;
...@@ -274,9 +274,9 @@ int compute_next_estimate(struct sm_params*params, ...@@ -274,9 +274,9 @@ int compute_next_estimate(struct sm_params*params,
} else { } else {
static int warned_before = 0; static int warned_before = 0;
if(!warned_before) { if(!warned_before) {
sm_error("Param use_ml_weights was active, but not valid alpha[] or true_alpha[]." sm_debug("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"); "Perhaps, if this is a single ray not having alpha, you should mark it as inactive.\n");
sm_error("Writing laser_ref: \n"); sm_debug("Writing laser_ref: \n");
ld_write_as_json(laser_ref, stderr); ld_write_as_json(laser_ref, stderr);
warned_before = 1; warned_before = 1;
} }
...@@ -290,8 +290,8 @@ int compute_next_estimate(struct sm_params*params, ...@@ -290,8 +290,8 @@ int compute_next_estimate(struct sm_params*params,
} else { } else {
static int warned_before = 0; static int warned_before = 0;
if(!warned_before) { if(!warned_before) {
sm_error("Param use_sigma_weights was active, but the field readings_sigma[] was not filled in.\n"); sm_debug("Param use_sigma_weights was active, but the field readings_sigma[] was not filled in.\n");
sm_error("Writing laser_sens: \n"); sm_debug("Writing laser_sens: \n");
ld_write_as_json(laser_sens, stderr); ld_write_as_json(laser_sens, stderr);
} }
} }
...@@ -315,7 +315,7 @@ int compute_next_estimate(struct sm_params*params, ...@@ -315,7 +315,7 @@ int compute_next_estimate(struct sm_params*params,
int ok = gpc_solve(k, c, 0, inv_cov_x0, x_new); int ok = gpc_solve(k, c, 0, inv_cov_x0, x_new);
if(!ok) { if(!ok) {
sm_error("gpc_solve_valid failed\n"); sm_debug("gpc_solve_valid failed\n");
return 0; return 0;
} }
...@@ -328,7 +328,7 @@ int compute_next_estimate(struct sm_params*params, ...@@ -328,7 +328,7 @@ int compute_next_estimate(struct sm_params*params,
double epsilon = 0.000001; double epsilon = 0.000001;
if(new_error > old_error + epsilon) { 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]); sm_debug("\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; return 1;
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment