Merge pull request #1 from aeolusbot/manuel/error-messages-spam

Suppressing error messages inside the sm_icp method

include/csm/algos.h

@@ -129,6 +129,11 @@ struct sm_params {
 };
 
 
+struct sm_reason {
+	/** Human readable description of outcomes */
+	char what[1024];
+};
+
 struct sm_result {
 	/** 1 if the result is valid */
 	int valid;
@@ -142,7 +147,10 @@ struct sm_result {
 	int nvalid;
 	/** Total correspondence error */
 	double error;
-
+
+	/** Human readable description of failures, empty if no one */
+	sm_reason reason;
+
 	/** Fields used for covariance computation */
 	#ifndef RUBY
 		gsl_matrix *cov_x_m;	

src/icp/icp.cpp

@@ -28,7 +28,9 @@ void ld_invalid_if_outside(LDP ld, double min_reading, double max_reading) {
 }
 
 void sm_icp(struct sm_params*params, struct sm_result*res) {
+	sm_reason reason;
 	res->valid = 0;
+	res->reason.what[0] = 0;
 
 	LDP laser_ref  = params->laser_ref;
 	LDP laser_sens = params->laser_sens;
@@ -85,11 +87,12 @@ void sm_icp(struct sm_params*params, struct sm_result*res) {
 	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");
+	if(!icp_loop(params, x_old->data(), x_new->data(), &error, &nvalid, &iterations, &reason)) {
+		//sm_error("icp: ICP failed for some reason. \n");
 		res->valid = 0;
 		res->iterations = iterations;
 		res->nvalid = 0;
+		snprintf(res->reason.what, sizeof(res->reason.what), "ICP failed: %s", reason.what);
 	} else {
 		/* It was succesfull */
 
@@ -121,8 +124,9 @@ void sm_icp(struct sm_params*params, struct sm_result*res) {
 					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);
+				if(!icp_loop(&my_params, start->data(), x_a->data(), &my_error, &my_valid, &my_iterations, &reason)){
+					//sm_error("Error during restart #%d/%d. \n", a, 6);
+					snprintf(res->reason.what, sizeof(res->reason.what), "Error during restart #%d/%d, due: %s", a, 6, reason.what);
 					break;
 				}
 				iterations+=my_iterations;

src/icp/icp.h

@@ -10,7 +10,7 @@ void sm_icp(struct sm_params*params, struct sm_result*res);
 
 /** This is the meat */
 int icp_loop(struct sm_params*params, const double*q0, double*x_new, 
- 	double*total_error, int*nvalid, int*iterations);
+	double*total_error, int*nvalid, int*iterations, sm_reason*reason);
 
 /** This is the beef: computing in closed form the next estimate 
     given the correspondences. */

src/icp/icp_loop.cpp

@@ -12,7 +12,7 @@
 #include <vector>
 
 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, sm_reason*reason) {
 	if(any_nan(q0,3)) {
 		sm_error("icp_loop: Initial pose contains nan: %s\n", friendly_pose(q0));
 		return 0;
@@ -52,7 +52,8 @@ int icp_loop(struct sm_params*params, const double*q0, double*x_new,
 		/* If not many correspondences, bail out */
 		int num_corr = ld_num_valid_correspondences(laser_sens);
 		if(num_corr < params->min_num_correspondences) {
-			sm_error("	: before trimming, only %d correspondences.\n",num_corr);
+			//sm_error("	: before trimming, only %d correspondences.\n",num_corr);
+			snprintf(reason->what, sizeof(reason->what), "before trimming, only %d correspondences.", num_corr);
 			all_is_okay = 0;
 			egsl_pop_named("icp_loop iteration"); /* loop context */
 			break;
@@ -76,15 +77,17 @@ int icp_loop(struct sm_params*params, const double*q0, double*x_new,
 
 		/* If not many correspondences, bail out */
 		if(num_corr_after < params->min_num_correspondences){
-			sm_error("  icp_loop: failed: after trimming, only %d correspondences.\n",num_corr_after);
+			//sm_error("  icp_loop: failed: after trimming, only %d correspondences.\n",num_corr_after);
+			snprintf(reason->what, sizeof(reason->what), "after trimming, only %d correspondences.", num_corr);
 			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");
+			//sm_error("  icp_loop: Cannot compute next estimate.\n");
+			snprintf(reason->what, sizeof(reason->what), "cannot compute next estimate.");
 			all_is_okay = 0;
 			egsl_pop_named("icp_loop iteration");
 			break;