diff --git a/sm/csm/icp/icp.c b/sm/csm/icp/icp.c
index ec9362ca5166d5eef4eb55b08903af59e00d1cd0..bd1ffeabdbc97c45273f592d7e7bc944c49c1371 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: ICP failed for some reason. \n");
+ sm_debug("icp: ICP failed for some reason. \n");
res->valid = 0;
res->iterations = iterations;
res->nvalid = 0;
@@ -127,7 +127,7 @@ void sm_icp(struct sm_params*params, struct sm_result*res) {
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);
+ sm_debug("Error during restart #%d/%d. \n", a, 6);
break;
}
iterations+=my_iterations;
@@ -263,7 +263,7 @@ void sm_icp_xy(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: ICP failed for some reason. \n");
+ sm_debug("icp: ICP failed for some reason. \n");
res->valid = 0;
res->iterations = iterations;
res->nvalid = 0;
@@ -298,7 +298,7 @@ void sm_icp_xy(struct sm_params*params, struct sm_result*res)
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);
+ sm_debug("Error during restart #%d/%d. \n", a, 6);
break;
}
iterations+=my_iterations;
diff --git a/sm/csm/icp/icp_debug.c b/sm/csm/icp/icp_debug.c
index f4d761b8cb1b041fc54f28a282117838c0371775..633ab3f276f5476903ebbbcff076e1ae47c8bd73 100644
--- a/sm/csm/icp/icp_debug.c
+++ b/sm/csm/icp/icp_debug.c
@@ -12,13 +12,13 @@ void debug_correspondences(struct sm_params * params) {
find_correspondences(params);
long hash2 = ld_corr_hash(laser_sens);
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++) {
if( (c1[i].valid != c2[i].valid) ||
(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);
- 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);
}
}
diff --git a/sm/csm/icp/icp_loop.c b/sm/csm/icp/icp_loop.c
index f64d17a9a321c76e90c47f6494f68967bd10161a..41ba54d4dd7e14782ee19df7cffd2c57dab9eaf3 100644
--- a/sm/csm/icp/icp_loop.c
+++ b/sm/csm/icp/icp_loop.c
@@ -13,7 +13,7 @@
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));
+ sm_debug("icp_loop: Initial pose contains nan: %s\n", friendly_pose(q0));
return 0;
}
@@ -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);
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);
+ sm_debug(" : before trimming, only %d correspondences.\n",num_corr);
all_is_okay = 0;
egsl_pop_named("icp_loop iteration"); /* loop context */
break;
@@ -91,7 +91,7 @@ int icp_loop(struct sm_params*params, const double*q0, double*x_new,
/* 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);
+ sm_debug(" 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;
@@ -99,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(" icp_loop: Cannot compute next estimate.\n");
+ sm_debug(" icp_loop: Cannot compute next estimate.\n");
all_is_okay = 0;
egsl_pop_named("icp_loop iteration");
break;
@@ -274,9 +274,9 @@ int compute_next_estimate(struct sm_params*params,
} else {
static int warned_before = 0;
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");
- sm_error("Writing laser_ref: \n");
+ sm_debug("Writing laser_ref: \n");
ld_write_as_json(laser_ref, stderr);
warned_before = 1;
}
@@ -290,8 +290,8 @@ int compute_next_estimate(struct sm_params*params,
} 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");
+ sm_debug("Param use_sigma_weights was active, but the field readings_sigma[] was not filled in.\n");
+ sm_debug("Writing laser_sens: \n");
ld_write_as_json(laser_sens, stderr);
}
}
@@ -315,7 +315,7 @@ int compute_next_estimate(struct sm_params*params,
int ok = gpc_solve(k, c, 0, inv_cov_x0, x_new);
if(!ok) {
- sm_error("gpc_solve_valid failed\n");
+ sm_debug("gpc_solve_valid failed\n");
return 0;
}
@@ -328,7 +328,7 @@ int compute_next_estimate(struct sm_params*params,
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]);
+ 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;