da isa

sm/CMakeLists.txt

@@ -31,8 +31,8 @@
 	
 
 
-	SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wno-unused -Wall -W -Wconversion  -Wunreachable-code ")
-	SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -ggdb")
+	SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wno-unused -Wall  -Wconversion  -Wunreachable-code ")
+#	SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -ggdb")
 	SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fstrict-aliasing")
 
 	
@@ -49,8 +49,11 @@
 #		SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -mtune=7450 -mcpu=7450 -O3")
 #		SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -ffast-math -fomit-frame-pointer -mtune=7450 -mcpu=7450 -O3")
 	ELSE(APPLE)
-		#SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -pg")
-		SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O2")
+		SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -pg")
+		SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O3 -pipe -fomit-frame-pointer")
+
+		SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -march=pentium3 -msse -O3 -pipe -fomit-frame-pointer")
+		SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS}  ")
 
 	ENDIF(APPLE)
 
@@ -164,4 +167,5 @@ IF(CAIRO_FOUND)
 	SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -I../deploy/include/cairo")
 	SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${CAIRO_CFLAGS}")
 	TARGET_LINK_LIBRARIES(carmen2pdf options -lcairo)
+
 ENDIF(CAIRO_FOUND)

sm/lib/egsl/egsl.c

@@ -6,11 +6,13 @@
 #include <math.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))
 
+
 struct egsl_variable {
 	gsl_matrix * gsl_m;
 };
@@ -30,7 +32,7 @@ int egsl_first_time = 1;
 int egsl_total_allocations = 0;
 int egsl_cache_hits = 0;
 
-void error() {
+void egsl_error(void) {
 	/* TODO: better handling of errors */
 	
 	egsl_print_stats();
@@ -62,13 +64,13 @@ void check_valid_val(val v) {
 	int context = its_context(v);
 	if(context>cid) {
 		fprintf(stderr, "Val is from invalid context (%d>%d)\n",context,cid);
-		error();
+		egsl_error();
 	}
 	int var_index = its_var_index(v);
 	if(var_index >= egsl_contexts[context].nvars) {
 		fprintf(stderr, "Val is invalid  (%d>%d)\n",var_index, 
 			egsl_contexts[context].nvars);		
-		error();
+		egsl_error();
 	}
 }
 #endif
@@ -118,7 +120,7 @@ val egsl_alloc(size_t rows, size_t columns) {
 	
 	if(c->nvars>=MAX_VALS) {
 		fprintf(stderr,"Limit reached, in context %d, nvars is %d\n",cid,c->nvars);
-		error();
+		egsl_error();
 	}
 	int index = c->nvars;
 	if(index<c->nallocated) {
@@ -155,7 +157,7 @@ val egsl_alloc_in_context(int context, size_t rows, size_t columns) {
 /** Creates a copy of v in the previous context. */
 val egsl_promote(val v) {
 	if(cid==0) {
-		error();
+		egsl_error();
 	}
 
 	gsl_matrix * m = egsl_gslm(v);
@@ -176,7 +178,7 @@ void egsl_expect_size(val v, size_t rows, size_t cols) {
 		fprintf(stderr, "Matrix size is %d,%d while I expect %d,%d",
 			(int)m->size1,(int)m->size2,(int)rows,(int)cols);
 		
-		error();
+		egsl_error();
 	}
 }
 

sm/lib/egsl/egsl.h

@@ -13,9 +13,9 @@ struct egsl_val {
 typedef struct egsl_val val;
 
 /* Core functions */
-void egsl_push();
-void egsl_pop();
-void egsl_free();
+void egsl_push(void);
+void egsl_pop(void);
+void egsl_free(void);
 
 double* egsl_atmp(val v, size_t i, size_t j);
 val egsl_alloc(size_t rows, size_t columns);
@@ -75,11 +75,11 @@ val egsl_rot(double theta);
 void egsl_print(const char*str, val);	
 /** Prints eigenvalues and eigenvectors of a symmetric matrix */
 void egsl_print_spectrum(const char*s, val v);
-void egsl_print_stats();
+void egsl_print_stats(void);
 
 
 	
 /** Private implementations things */
 void egsl_expect_size(val v, size_t rows, size_t cols);
-
+void egsl_error(void);
 #endif

sm/lib/gpc/gpc.c

@@ -31,12 +31,15 @@ int gpc_solve_valid(int K, const struct gpc_corr*c, const int*valid,
 {
 	M(bigM,    4,4); M(g,  4,1); M(bigM_k,2,4);
 	M(bigM_k_t,4,2); M(C_k,2,2); M(q_k,   2,1);
-	M(temp42,  4,2); M(temp44,4,4);	M(temp21, 2,1);
 	M(temp41,  4,1); M(temp22,2,2);	M(temp22b,2,2);
+	M(temp42,  4,2); M(temp44,4,4);	M(temp21, 2,1);
 	M(temp22c, 2,2); M(temp12,1,2);
-		
+	M(temp42b,4,2);
+	
 	gsl_matrix_set_zero(bigM);
 	gsl_matrix_set_zero(g);
+	gsl_matrix_set_zero(temp42);
+	
 	int k;
 	for(k=0;k<K;k++) {
 		if(valid && !valid[k]) continue;
@@ -50,6 +53,11 @@ int gpc_solve_valid(int K, const struct gpc_corr*c, const int*valid,
 		gms(q_k,0,0,c[k].q[0]);gms(q_k,1,0,c[k].q[1]);
 		
 		m_trans(bigM_k, bigM_k_t);
+		if(0) {
+		m_display("before t42 is ",temp42);
+		m_display("bigM_k_t",bigM_k_t);
+		m_display("C_k",C_k);
+		}
 		m_mult(bigM_k_t, C_k, temp42);
 		m_mult(temp42, bigM_k, temp44);
 		m_scale(2.0, temp44);
@@ -61,9 +69,14 @@ int gpc_solve_valid(int K, const struct gpc_corr*c, const int*valid,
 		m_add_to(temp41, g);	
 
 		if(0) {
+			printf("C[k].p = %f %f \n", c[k].p[0], c[k].p[1]);
+			printf("C[k].q = %f %f \n", c[k].q[0], c[k].q[1]);
 			m_display("bigM_k",bigM_k);
-			m_display("q_k",q_k);
+			m_display("bigM_k_t",bigM_k_t);
 			m_display("C_k",C_k);
+			m_display("temp42",temp42);
+			m_display("temp44",temp44);
+			m_display("q_k",q_k);
 			m_display("now M is ",bigM);
 			m_display("now g is ",g);
 		}
@@ -102,7 +115,7 @@ int gpc_solve_valid(int K, const struct gpc_corr*c, const int*valid,
 		m_display("now g is ",g);
 	}
 	
-	if(0) {
+	if(1) {
 		m_display("bigM",bigM);
 		m_display("g",g);
 	}
@@ -180,18 +193,18 @@ int gpc_solve_valid(int K, const struct gpc_corr*c, const int*valid,
 	double q[5] = {p[0]-(l[0]*l[0]), p[1]-(2*l[1]*l[0]), 
 		p[2]-(l[1]*l[1]+2*l[0]*l[2]), -(2*l[2]*l[1]), -(l[2]*l[2])};
 	
-	
-	if(0) {
+	if(1) {
 		printf("p = %f %f %f \n", p[2], p[1], p[0]);
 		printf("l = %f %f %f \n", l[2], l[1], l[0]);
 		printf("q = %f %f %f %f %f \n", q[4],  q[3],  q[2], q[1], q[0]);
-	}	
-	
-	double lambda = poly_greatest_real_root(5,q);
+	}
 
-	if(0) {
+	double lambda = poly_greatest_real_root(5,q);
+	
+	if(1) {
 		printf("lambda = %f \n", lambda);
-	}
+	}	
+	
 	M(W,4,4); gsl_matrix_set_zero(W); gms(W,2,2,1.0); gms(W,3,3,1.0);
 	M(x,4,1);
 	

sm/lib/gpc/gpc_utils.c

@@ -80,7 +80,7 @@ void m_display(const char*str, gsl_matrix*m) {
 	for(i=0;i<m->size1;i++) {
 		printf("   ");
 		for(j=0;j<m->size2;j++)
-			printf("%f ", gmg(m,i,j));
+			printf("%e ", gmg(m,i,j));
 		printf("\n");
 	}
 }

sm/lib/json-c/config.h

-/* Define to 1 if you don't have `vprintf' but do have `_doprnt.' */
-#define HAVE_DOPRNT 
-
-/* Define to 1 if you have the <fcntl.h> header file. */
-#define HAVE_FCNTL_H 1
-
-/* Define to 1 if you have the <inttypes.h> header file. */
-#define HAVE_INTTYPES_H 1
-
-/* Define to 1 if you have the <limits.h> header file. */
-#define HAVE_LIMITS_H 1
-
-/* Define to 1 if your system has a GNU libc compatible `malloc' function, and
-   to 0 otherwise. */
-#define HAVE_MALLOC 1
-
-/* Define to 1 if you have the <memory.h> header file. */
-#define HAVE_MEMORY_H 1
-
-/* Define to 1 if you have the `open' function. */
-#define HAVE_OPEN 1
-
-/* Define to 1 if your system has a GNU libc compatible `realloc' function,
-   and to 0 otherwise. */
-#define HAVE_REALLOC 1
-
-/* Define to 1 if you have the <stdarg.h> header file. */
-#define HAVE_STDARG_H 1
-
-/* Define to 1 if you have the <stdint.h> header file. */
-#define HAVE_STDINT_H 1
-
-/* Define to 1 if you have the <stdlib.h> header file. */
-#define HAVE_STDLIB_H 1
-
-/* Define to 1 if you have the `strerror' function. */
-#define HAVE_STRERROR 1
-
-/* Define to 1 if you have the <strings.h> header file. */
-#define HAVE_STRINGS_H 1
-
-/* Define to 1 if you have the <string.h> header file. */
-#define HAVE_STRING_H 1
-
-/* Define to 1 if you have the `strncasecmp' function. */
-#define HAVE_STRNCASECMP 1
-
-/* Define to 1 if you have the `strndup' function. */
-#define HAVE_STRNDUP 0
-
-/* Define to 1 if you have the <syslog.h> header file. */
-#define HAVE_SYSLOG_H 1
-
-/* Define to 1 if you have the <sys/param.h> header file. */
-#define HAVE_SYS_PARAM_H 1
-
-/* Define to 1 if you have the <sys/stat.h> header file. */
-#define HAVE_SYS_STAT_H 1
-
-/* Define to 1 if you have the <sys/types.h> header file. */
-#define HAVE_SYS_TYPES_H 1
-
-/* Define to 1 if you have the <unistd.h> header file. */
-#define HAVE_UNISTD_H 1
-
-/* Define to 1 if you have the `vasprintf' function. */
-#define HAVE_VASPRINTF 1
- 
-/* Define to 1 if you have the `vprintf' function. */
-#define HAVE_VPRINTF 1
-
-/* Define to 1 if you have the `vsnprintf' function. */
-#define HAVE_VSNPRINTF 1
-
-/* Define to 1 if you have the `vsyslog' function. */
-#define HAVE_VSYSLOG 1
-
-
-/* Define to rpl_malloc if the replacement function should be used. */
-#if !HAVE_MALLOC
-#define malloc rpl_malloc
-#endif
-
-/* Define to rpl_realloc if the replacement function should be used. */
-#if !HAVE_REALLOC
-#define realloc rpl_realloc
-#endif
-
-
-
-
-
-
-/* Define to 1 if you have the ANSI C header files. */
-#define STDC_HEADERS 1
-
-/* Define to empty if `const' does not conform to ANSI C. */
-/* #undef const */
-
-/* Define to `unsigned int' if <sys/types.h> does not define. */
-/* #undef size_t */

sm/src/icp/icp_corr_tricks.c

@@ -29,6 +29,13 @@ void ld_create_jump_tables(struct laser_data* ld) {
 		ld->down_bigger[i] = j-i;
 	}	
 }
+extern int distance_counter;
+inline double distance2(const gsl_vector* a, const gsl_vector* b) {
+        distance_counter++;
+        double x = gvg(a,0)-gvg(b,0);
+        double y = gvg(a,1)-gvg(b,1);
+        return sqrt(x*x+y*y);
+}
 
 void find_correspondences_tricks(struct sm_params*params, gsl_vector* x_old) {
 	LDP laser_ref  = &(params->laser_ref);
@@ -107,6 +114,7 @@ void find_correspondences_tricks(struct sm_params*params, gsl_vector* x_old) {
 				
 				/* This is the distance from p_i_w to the "up" point*/
 				last_dist_up = distance(p_i_w, laser_ref->p[up]);
+				
 				/* If it is less than the best point, it is our new j1 */
 				if( (last_dist_up<params->max_correspondence_dist) && 
 					((j1==-1)||(last_dist_up < best_dist))) {
@@ -150,7 +158,7 @@ void find_correspondences_tricks(struct sm_params*params, gsl_vector* x_old) {
 				if(down < from) { down_stopped = 1; continue; }
 				if(!laser_ref->valid[down]) { --down; continue; }
 		
-				last_dist_down = distance(p_i_w, laser_ref->p[down]);
+				last_dist_down = distance2(p_i_w, laser_ref->p[down]);
 				if( (last_dist_down<params->max_correspondence_dist) && 
 				    ((j1==-1)||(last_dist_down < best_dist))) {
 						j1 = down; best_dist = last_dist_down;
@@ -195,8 +203,8 @@ void find_correspondences_tricks(struct sm_params*params, gsl_vector* x_old) {
 		}
 		if(j2up  ==-1) { j2 = j2down; } else
 		if(j2down==-1) { j2 = j2up; } else {
-			double dist_up   = distance(p_i_w, laser_ref->p[j2up  ]);
-			double dist_down = distance(p_i_w, laser_ref->p[j2down]);
+			double dist_up   = distance2(p_i_w, laser_ref->p[j2up  ]);
+			double dist_down = distance2(p_i_w, laser_ref->p[j2down]);
 			j2 = dist_up < dist_down ? j2up : j2down;
 		}