Better error handling with named contexts. Fixed bug in memory allocation.

sm/apps/sm1.c

@@ -101,28 +101,6 @@ int main(int argc, const char*argv[]) {
 	fclose(file2);
 	fclose(out);
 	
-	return 0;
-/*
-	int num_matchings = 0;
-	int num_iterations = 0;
-	clock_t start = clock();
-	
-	clock_t end = clock();
-	float seconds = (end-start)/((float)CLOCKS_PER_SEC);
 	
-	printf("sm0: CPU time = %f (seconds) (start=%d end=%d)\n", seconds,(int)start,(int)end);
-	printf("sm0: Total number of matchings = %d\n", num_matchings);
-	printf("sm0: Total number of iterations = %d\n", num_iterations);
-	printf("sm0: Avg. iterations per matching = %f\n", num_iterations/((float)num_matchings));
-	printf("sm0: Avg. seconds per matching = %f\n", seconds/num_matchings);
-	printf("sm0:   that is, %d matchings per second\n", (int)floor(num_matchings/seconds));
-	printf("sm0: Avg. seconds per iteration = %f (note: very imprecise)\n", seconds/num_iterations);
-	printf("sm0: Number of comparisons = %d \n", distance_counter);
-	printf("sm0: Avg. comparisons per ray = %f \n", 
-		(distance_counter/((float)num_iterations*params.laser_ref.nrays)));
-	
-	gsl_vector_free(u);
-	gsl_vector_free(x_old);
-	gsl_vector_free(x_new);
-	return 0;*/
+	return 0;
 }

sm/csm/icp/icp.c

@@ -44,7 +44,7 @@ void sm_icp(struct sm_params*params, struct sm_result*res) {
 	
 	if(JJ) jj_context_enter("sm_icp");
 	
-	egsl_push();
+	egsl_push_named("sm_icp");
 	
 			
 	if(params->use_corr_tricks || params->debug_verify_tricks)
@@ -172,7 +172,7 @@ void sm_icp(struct sm_params*params, struct sm_result*res) {
 		gsl_vector_free(best_x);
 	}
 
-	egsl_pop();
+	egsl_pop_named("sm_icp");
 
 	if(JJ) jj_context_exit();
 }

sm/csm/icp/icp_covariance.c

@@ -14,7 +14,7 @@ void compute_covariance_exact(
 	LDP laser_ref, LDP laser_sens, const gsl_vector*x,
 		val *cov0_x, val *dx_dy1, val *dx_dy2)
 {
-	egsl_push();
+	egsl_push_named("compute_covariance_exact");
 	
 	val d2J_dxdy1 = zeros(3,(size_t)laser_ref ->nrays);
 	val d2J_dxdy2 = zeros(3,(size_t)laser_sens->nrays);
@@ -30,7 +30,7 @@ void compute_covariance_exact(
 	int i; 
 	for(i=0;i<laser_sens->nrays;i++) {
 		if(!ld_valid_corr(laser_sens,i)) continue;
-		egsl_push();
+		egsl_push_named("compute_covariance_exact iteration");
 
 		int j1 = laser_sens->corr[i].j1;
 		int j2 = laser_sens->corr[i].j2;
@@ -81,7 +81,7 @@ void compute_covariance_exact(
 		val d2Jk_dtheta_drho_j2 = sc(2.0, m3( tr(v2), dC_drho_j2, v1));
 		add_to_col(d2J_dxdy1, (size_t)j2, comp_col(d2Jk_dt_drho_j2, d2Jk_dtheta_drho_j2));
 
-		egsl_pop();
+		egsl_pop_named("compute_covariance_exact iteration");
 	}
 
 	/* composes matrix  d2J_dx2  from the pieces*/
@@ -101,7 +101,7 @@ void compute_covariance_exact(
 	*dx_dy1 = egsl_promote(edx_dy1);
 	*dx_dy2 = egsl_promote(edx_dy2);
 	
-	egsl_pop();	
+	egsl_pop_named("compute_covariance_exact");	
 	/* now edx_dy1 is not valid anymore, by *dx_dy1 is. */
 }
 

sm/csm/icp/icp_loop.c

@@ -28,7 +28,7 @@ int icp_loop(struct sm_params*params, const double*q0, double*x_new,
 		if(JJ) jj_loop_iteration();
 		if(JJ) jj_add_double_array("x_old", x_old, 3);
 
-		egsl_push();
+		egsl_push_named("icp_loop iteration");
 
 		/** Compute laser_sens's points in laser_ref's coordinates
 		    by roto-translating by x_old */
@@ -51,7 +51,7 @@ int icp_loop(struct sm_params*params, const double*q0, double*x_new,
 		if(num_corr < fail_perc * laser_sens->nrays) { /* TODO: arbitrary */
 			sm_error("Failed: before trimming, only %d correspondences.\n",num_corr);
 			all_is_okay = 0;
-			egsl_pop(); /* loop context */
+			egsl_pop_named("icp_loop iteration"); /* loop context */
 			break;
 		}
 
@@ -127,14 +127,17 @@ int icp_loop(struct sm_params*params, const double*q0, double*x_new,
 					break;
 				}
 			}
-			if(loop_detected) 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();
+			egsl_pop_named("icp_loop iteration");
 			break;
 		}
 		
@@ -142,7 +145,7 @@ int icp_loop(struct sm_params*params, const double*q0, double*x_new,
 		copy_d(delta, 3, delta_old);
 		
 		
-		egsl_pop();
+		egsl_pop_named("icp_loop iteration");
 	}
 
 	if(JJ) jj_loop_exit();

sm/lib/egsl/egsl.c

@@ -4,13 +4,14 @@
 
 #include <assert.h>
 #include <math.h>
+#include <string.h>
 
 #include "egsl.h"
 #include "egsl_imp.h"
 #define MAX_VALS 1024
 #define MAX_CONTEXTS 1024
 
-#define INVALID (val_from_context_and_index(1000,1000))
+/*#define INVALID (val_from_context_and_index(1000,1000))*/
 
 
 struct egsl_variable {
@@ -18,12 +19,16 @@ struct egsl_variable {
 };
 
 struct egsl_context {
+	char name[256];
 	int nallocated;
 	int nvars;
 	struct egsl_variable vars[MAX_VALS]; 
 };
 
+/* Current context */
 int cid=0;
+/* Maximum number of contexts */
+int max_cid = 0; 
 static struct egsl_context egsl_contexts[MAX_CONTEXTS];
 
 
@@ -80,35 +85,73 @@ gsl_matrix * egsl_gslm(val v) {
 	return v.gslm;
 }
 
+void egsl_push() { egsl_push_named("unnamed context"); }
+void egsl_pop() { egsl_pop_named("unnamed context"); }
 
-void egsl_push() {
+void egsl_push_named(const char*name) {
 	if(egsl_first_time) {
 		int c;
 		for(c=0;c<MAX_CONTEXTS;c++) {
 			egsl_contexts[c].nallocated = 0;
 			egsl_contexts[c].nvars = 0;
+			sprintf(egsl_contexts[c].name, "not yet used");
 		}
 		egsl_first_time  = 0;
 	}
 	cid++;
-	assert(cid<MAX_CONTEXTS);/* "Maximum number of contexts reached"); */
+	if(max_cid < cid) max_cid = cid;
+	
+	if(name != 0) 
+		sprintf(egsl_contexts[cid].name, "%s", name);
+	else
+		sprintf(egsl_contexts[cid].name, "Unnamed context");
+		
+	if(cid >= MAX_CONTEXTS) {
+		fprintf(stderr, "egsl: maximum number of contexts reached \n");
+		egsl_print_stats();
+		assert(0);
+	}
 }
 
-void egsl_pop() {
+void egsl_pop_named(const char*name) {
 	assert(cid>=0);/*, "No egsl_push before?"); */
+	if(name != 0) {
+		if(strcmp(name, egsl_contexts[cid].name)) {
+			fprintf(stderr, "egsl: context mismatch. You want to pop '%s', you are still at '%s'\n",
+				name, egsl_contexts[cid].name);
+			egsl_print_stats();
+			assert(0);
+		}
+	}
+	
 	egsl_contexts[cid].nvars = 0;
+	sprintf(egsl_contexts[cid].name, "Popped");
 	cid--;
 }
+/*
+void egsl_pop_check(int assumed) {
+	if(assumed != cid) {
+		fprintf(stderr, "egsl: You think you are in context %d while you are %d. \n", assumed, cid);
+		if(assumed < cid) 
+			fprintf(stderr, "     It seems you miss %d egsl_pop() somewhere.\n", - assumed + cid);			
+		else
+			fprintf(stderr, "     It seems you did %d egsl_pop() more.\n",  + assumed - cid);			
+		egsl_print_stats();
+	}
+	assert(cid>=0);
+	egsl_contexts[cid].nvars = 0;
+	cid--;
+}*/
 
 void egsl_print_stats() {
 	fprintf(stderr, "egsl: total allocations: %d   cache hits: %d\n",	
 		egsl_total_allocations, egsl_cache_hits);
 /*	printf("egsl: sizeof(val) = %d\n",(int)sizeof(val)); */
-	int c; for(c=0;c<MAX_CONTEXTS;c++) {
-	/*	printf("egsl: context #%d\n ",c);
-	 	if(0==egsl_contexts[c].nallocated) break; */
-		fprintf(stderr, "egsl: context #%d allocations: %d active: %d\n",
-			c,	egsl_contexts[c].nallocated, 	egsl_contexts[c].nvars);
+	int c; for(c=0;c<=max_cid;c++) {
+	/*	printf("egsl: context #%d\n ",c); */
+/*	 	if(0==egsl_contexts[c].nallocated) break; */
+		fprintf(stderr, "egsl: context #%d allocations: %d active: %d name: '%s' \n",
+			c,	egsl_contexts[c].nallocated, egsl_contexts[c].nvars, egsl_contexts[c].name);
 	}
 }
 

sm/lib/egsl/egsl.h

@@ -13,8 +13,13 @@ struct egsl_val {
 typedef struct egsl_val val;
 
 /* Core functions */
-void egsl_push(void);
-void egsl_pop(void);
+
+/* Push a new context. */
+void egsl_push();
+void egsl_push_named(const char*name);
+/* Pops a context */
+void egsl_pop();
+void egsl_pop_named(const char*name);
 void egsl_free(void);
 
 double* egsl_atmp(val v, size_t i, size_t j);

sm/lib/gpc/gpc.c

@@ -21,8 +21,8 @@
 #include "gpc.h"
 #include "gpc_utils.h"
 
+/* Note that we use static values here so we don't need to evaluate that all the time */
 #define M(matrix, rows, col) static gsl_matrix*matrix=0; if(!matrix) matrix = gsl_matrix_alloc(rows,col);
-/*#define MF(matrix) gsl_matrix_free(matrix)*/
 #define MF(matrix) /*gsl_matrix_free(matrix)*/