No commit message

No commit message

sm/CMakeLists.txt

@@ -29,19 +29,22 @@ PROJECT (scan_matching_lib C)
 	ENDIF(APPLE)
 
 
-########### Easy GSL ########## 
-	ADD_LIBRARY(egsl STATIC 
-		lib/egsl/egsl.c 
-		lib/egsl/egsl_ops.c
-		lib/egsl/egsl_conversions.c
-		lib/egsl/egsl_misc.c
-	)
+	########### Easy GSL ########## 
+		ADD_LIBRARY(egsl STATIC 
+			lib/egsl/egsl.c 
+			lib/egsl/egsl_ops.c
+			lib/egsl/egsl_conversions.c
+			lib/egsl/egsl_misc.c
+		)
 
-	ADD_EXECUTABLE(test_egsl lib/egsl/egsl_test.c)
-	TARGET_LINK_LIBRARIES(test_egsl egsl  ${GSL_LIBRARIES})
+		ADD_EXECUTABLE(test_egsl lib/egsl/egsl_test.c)
+		TARGET_LINK_LIBRARIES(test_egsl egsl  ${GSL_LIBRARIES})
 
-	ADD_EXECUTABLE(egsl_test_allocation lib/egsl/egsl_test_allocation.c)
-	TARGET_LINK_LIBRARIES(egsl_test_allocation egsl  ${GSL_LIBRARIES})
+		ADD_EXECUTABLE(egsl_test_allocation lib/egsl/egsl_test_allocation.c)
+		TARGET_LINK_LIBRARIES(egsl_test_allocation egsl  ${GSL_LIBRARIES})
+
+	########### Options ########## 
+		ADD_LIBRARY(options STATIC lib/options/options.c)
 
 ########### SM ########## 
 
@@ -78,10 +81,18 @@ PROJECT (scan_matching_lib C)
 	INSTALL(TARGETS sm ARCHIVE DESTINATION lib)
 	INSTALL(FILES src/sm.h  DESTINATION include)
 
-########### SM applications ########## 
+	########### SM applications ########## 
+
+		ADD_EXECUTABLE(sm0 apps/sm0.c)
+		TARGET_LINK_LIBRARIES(sm0 sm ${GSL_LIBRARIES})
+
+		ADD_EXECUTABLE(test_math_utils src/math_utils_test.c)
+		TARGET_LINK_LIBRARIES(test_math_utils sm ${GSL_LIBRARIES})
+
+	########### Carmen2pdf ########## 
 
-	ADD_EXECUTABLE(sm0 apps/sm0.c)
-	TARGET_LINK_LIBRARIES(sm0 sm ${GSL_LIBRARIES})
+	ADD_EXECUTABLE(carmen2pdf apps/carmen2pdf.c)
+	TARGET_LINK_LIBRARIES(carmen2pdf sm ${GSL_LIBRARIES})
+	SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -I../deploy/include/cairo")
+	TARGET_LINK_LIBRARIES(carmen2pdf options -lcairo)
 
-	ADD_EXECUTABLE(test_math_utils src/math_utils_test.c)
-	TARGET_LINK_LIBRARIES(test_math_utils sm ${GSL_LIBRARIES})

sm/apps/carmen2pdf.c

+#include <time.h>
+
+#include <cairo.h>
+#include <cairo-pdf.h>
+
+#include "../lib/options/options.h"
+
+
+#include "../src/math_utils.h"
+#include "../src/sm.h"
+#include "../src/laser_data.h"
+
+struct params {
+	int interval;
+	int use_odometry;
+	double horizon;
+	double line_threshold;
+	double dimension;
+	const char*output_filename;
+	const char*input_filename;
+
+	FILE*input_file;
+};
+
+void carmen2pdf(struct params p);
+
+void ld_getbb(struct laser_data* ld, double*x0, double*y0, double*x1, double*y1, 
+	int use_odometry, double horizon);
+
+int main(int argc, const char*argv[]) {
+	struct params p;
+	p.interval = 10;
+	p.input_filename = "in.log";
+	p.output_filename = "out.pdf";
+	p.use_odometry = 0;
+	p.line_threshold = 0.2;
+	p.horizon = 8;
+	p.dimension=500;
+	
+	#define noptions 7
+	struct option options[noptions] = 
+		{ {"--interval", "how many to ignore", OPTION_INT, &(p.interval)},
+		  {"--in", "input filename (Carmen format)",OPTION_STRING, &(p.input_filename)},
+		  {"--out", "output filename (pdf)",OPTION_STRING, &(p.output_filename)},
+		  {"--lt", "threshold for linking points (m)",OPTION_DOUBLE, &(p.line_threshold)},
+		  {"--horizon", "horizon of the laser (m)",OPTION_DOUBLE, &(p.horizon)},
+		  {"--dimension", "dimension of image (points)",OPTION_DOUBLE, &(p.dimension)},
+			{"--use_odometry", "if 1 uses odometry, else estimate",
+				OPTION_INT, &(p.use_odometry)}};
+	
+	if(!auto_option(argc, argv, noptions, options)) {
+		printf("error.");
+		print_help(stderr, noptions, options);
+		return -1;
+	}
+	
+	p.input_file = fopen(p.input_filename,"r");
+	if(p.input_file==NULL) {
+		fprintf(stderr, "Could not open '%s'.\n", p.input_filename); 
+		return -1;
+	}
+	
+	carmen2pdf(p);
+	return 0;
+}
+
+int should_consider(struct params *p, int counter) {
+	return counter%p->interval == 0;
+}
+
+void ld_get_world(struct laser_data*ld, int i, double*x, double*y,int use_odometry);
+
+struct bounding_box {
+	// World frame
+	double x0,y0,x1,y1;
+	// Paper size
+	double width, height;
+};
+
+void bb_w2b(struct bounding_box*bb, double wx, double wy, double*bx, double*by){
+	double scale = GSL_MIN(bb->width / (bb->x1-bb->x0), bb->height / (bb->y1-bb->y0));
+	*bx = (wx-bb->x0) * scale;
+	*by = bb->height- (wy-bb->y0) * scale;
+}
+
+void get_bb(struct params*p, struct bounding_box*bb) {
+	struct laser_data ld;	
+	int counter=0;
+	if(!ld_read_next_laser_carmen(p->input_file, &ld)) {
+		ld_getbb(&ld,&bb->x0,&bb->y0,&bb->x1,&bb->y1,p->use_odometry, p->horizon);
+	}
+	ld_free(&ld);
+	while(!ld_read_next_laser_carmen(p->input_file, &ld)) {
+		if(should_consider(p, counter))  {
+			double x0,y0,x1,y1;
+			ld_getbb(&ld,&x0,&y0,&x1,&y1,p->use_odometry, p->horizon);
+			bb->x0 = GSL_MIN(x0, bb->x0);
+			bb->x1 = GSL_MAX(x1, bb->x1);
+			bb->y0 = GSL_MIN(y0, bb->y0);
+			bb->y1 = GSL_MAX(y1, bb->y1);
+		}
+		counter++;
+		ld_free(&ld);
+	}
+	rewind(p->input_file);
+}
+
+void carmen2pdf(struct params p) {
+	
+	struct bounding_box bb;
+	get_bb(&p, &bb);
+
+	double wwidth = bb.x1-bb.x0;
+	double wheight= bb.y1-bb.y0;
+	if(wwidth > wheight) {
+		bb.width = p.dimension;
+		bb.height = bb.width / wwidth * wheight;
+	} else {
+		bb.height = p.dimension;
+		bb.width = bb.height / wheight * wwidth;
+	}
+	
+	printf("Bounding box: %f %f, %f %f\n",bb.x0,bb.y0,bb.x1,bb.y1);
+	
+	cairo_surface_t *surface;
+	cairo_t *cr;
+	cairo_status_t status;
+	
+	surface = cairo_pdf_surface_create(p.output_filename, bb.width, bb.height);
+	cr = cairo_create (surface);
+	status = cairo_status (cr);
+
+	if (status) {
+		printf("Failed to create pdf surface for file %s: %s\n",
+			p.output_filename, cairo_status_to_string (status));
+		return;
+	}
+	
+
+//	double scale = GSL_MIN(bb.width / (bb.x1-bb.x0), bb.height / (bb.y1-bb.y0));
+
+//	cairo_scale(cr, 50, 1);
+	//cairo_translate(cr, 0, -0.5*bb.height);
+	
+	struct laser_data ld;	
+	int counter=0; 
+	int first_pose=1; double old_pose_bx,old_pose_by;
+	while(!ld_read_next_laser_carmen(p.input_file, &ld)) {
+	
+		{
+			double bx,by;
+			if(p.use_odometry)
+				bb_w2b(&bb, ld.odometry[0], ld.odometry[1], &bx,&by);
+			else
+				bb_w2b(&bb, ld.estimate[0], ld.estimate[1], &bx,&by);
+
+			if(first_pose) { first_pose = 0; 
+			} else {
+				cairo_set_line_width(cr, 0.5);
+				cairo_set_source_rgb (cr, 1, 0, 0);
+				cairo_move_to(cr, old_pose_bx, old_pose_by);
+				cairo_line_to(cr, bx, by);
+				cairo_close_path(cr);
+				cairo_stroke(cr);
+			}
+
+			old_pose_bx = bx;
+			old_pose_by = by;
+		}
+		
+		if(should_consider(&p, counter))  {
+				
+	
+			cairo_set_source_rgb (cr, 0, 0, 0);
+			cairo_set_line_width(cr, 0.1);
+			int first = 1;
+			double last_x,last_y;
+			int i; for(i=0;i<ld.nrays;i++) {
+				if(ld.valid[i]==0) continue;
+				if(ld.readings[i]>p.horizon) continue;
+				
+				double x,y;
+				ld_get_world(&ld, i, &x, &y, p.use_odometry);
+				double bx,by;
+				bb_w2b(&bb, x,y,&bx,&by);
+				
+				if(first) { first = 0;
+					cairo_move_to(cr, bx, by);
+					last_x=x; last_y=y;
+				} else {
+					int near = square(p.line_threshold) > 
+						square(x-last_x)+square(y-last_y);
+					
+					if(near) {
+						cairo_line_to(cr, bx, by);
+					} else {
+						cairo_close_path(cr);
+						cairo_stroke(cr);
+						cairo_move_to(cr, bx, by);
+						last_x=x; last_y=y;
+					}
+				}
+				
+			}
+			cairo_close_path(cr);
+			cairo_stroke(cr);
+		}
+		counter++;
+		ld_free(&ld);
+	}
+
+	cairo_show_page (cr);
+
+	cairo_destroy (cr);
+	cairo_surface_destroy (surface);
+}
+
+void ld_get_world(struct laser_data*ld, int i, double*x, double*y, int use_odometry) {
+	gsl_vector * p = gsl_vector_alloc(2);
+	gsl_vector * pw = gsl_vector_alloc(2);
+	gsl_vector * pose = gsl_vector_alloc(3);
+	
+	gsl_vector_set(p, 0, cos(ld->theta[i]) * ld->readings[i]);
+	gsl_vector_set(p, 1, sin(ld->theta[i]) * ld->readings[i]);
+	
+	if(use_odometry)
+		copy_from_array(pose, ld->odometry);
+	else
+		copy_from_array(pose, ld->estimate);
+	
+	transform(p, pose, pw);
+	
+	*x = gsl_vector_get(pw, 0);
+	*y = gsl_vector_get(pw, 1);
+
+	gsl_vector_free(p);
+	gsl_vector_free(pw);
+	gsl_vector_free(pose);
+}
+
+void ld_getbb(struct laser_data* ld, double*x0, double*y0, double*x1, double*y1,
+ 	int use_odometry, double horizon) {
+	
+	int first=1;
+	int i; for(i=0;i<ld->nrays;i++) {
+		if(!ld->valid[i]) continue;
+		if(ld->readings[i]>horizon) continue;
+		double x,y;
+		ld_get_world(ld, i, &x, &y, use_odometry);
+		
+		if(first) {
+			*x0 = *x1 = x;
+			*y0 = *y1 = y;
+			first = 0;
+		} else {
+			*x0 = GSL_MIN(*x0, x);
+			*y0 = GSL_MIN(*y0, y);
+			*x1 = GSL_MAX(*x1, x);
+			*y1 = GSL_MAX(*y1, y);
+		}
+	}
+}
+
+
+
+

sm/lib/egsl/egsl.h

@@ -15,6 +15,8 @@ typedef struct egsl_val val;
 /// Core functions
 void egsl_push();
 void egsl_pop();
+void egsl_free();
+
 double* egsl_atmp(val v, size_t i, size_t j);
 val egsl_alloc(size_t rows, size_t columns);
 val egsl_alloc_in_context(int cid, size_t rows, size_t cols);

sm/lib/options/options.c

+/* $Id: options.cpp 764 2005-04-26 08:45:06Z andrea $ */
+#include <errno.h>
+#include <stdlib.h>
+#include <stdio.h>
+
+/* Cavillo (non impatta la portabilit. */
+#ifndef _GNU_SOURCE
+#define _GNU_SOURCE
+#endif
+
+#include <string.h>
+#include "options.h"
+
+/** Our version of strdup. */
+char * strdup_(const char *s) {
+	size_t len = strlen(s) + 1; /* null byte */
+	//char * t = (char*) malloc(len);
+	char * t = (char*)malloc(len);
+	memcpy(t,s,len);
+	return t;
+}
+
+/** Return 1 if ok. */
+int get_int(int*p, const char*s) {
+	int value;
+	errno = 0;
+	value = strtol(s, (char **)NULL, 10);
+	if(0 == errno) {
+		*p = value;
+		return 1;
+	} else return 0;
+}
+
+/** Return 1 if ok. */
+int get_double(double*p, const char*s) {
+	char *endptr;
+	*p = strtod(s, &endptr);
+	return endptr != s;
+}
+
+int auto_option(int argc, const char* argv[], int noptions, struct option*op) {
+	int i;
+	for (i=1; i<argc; i++) {
+		int j, found = 0;
+		for (j=0;j<noptions && !found;j++) {
+			const char * name;
+			int * set_pointer;
+			int requires_argument;
+
+			name = op[j].name;
+			if (strcmp(argv[i],name))
+				continue;
+
+			found = 1;
+
+			set_pointer = op[j].set_pointer;
+			if(NULL!=set_pointer)
+				*set_pointer = 1;
+
+			
+			requires_argument = NULL != op[j].value_pointer;
+
+			if(requires_argument) {
+				if(i>=argc-1) {
+					fprintf(stderr, "Argument %s needs value.\n", op[j].name);
+					return 0;
+				}
+
+				switch(op[j].type) {
+					case(OPTION_INT): {
+						int * value_pointer = (int*) op[j].value_pointer;
+						int ok = get_int(value_pointer, argv[i+1]);
+						if(!ok) {
+							fprintf(stderr, "Could not parse int: %s = %s.\n", 
+								op[j].name, argv[i+1]);
+							return 0;
+						}
+						break;
+					}
+
+					case(OPTION_STRING): {
+						char** value_pointer = (char**) op[j].value_pointer;
+						*value_pointer = (char*) strdup_(argv[i+1]);
+/*						fprintf(stderr, 
+							"String %s, value_pointer=%p void pointer=%p *value_pointer=%p result=%s\n"
+						 ,argv[i+1], value_pointer, o->op[j].value_pointer, *value_pointer,
+						 *value_pointer);*/
+						 break;
+					}
+					
+					case(OPTION_DOUBLE): {
+						double * value_pointer = (double*) op[j].value_pointer;
+						int ok = get_double(value_pointer, argv[i+1]);
+						if(!ok) {
+						fprintf(stderr, "Could not parse double: %s = %s.\n", 
+								op[j].name, argv[i+1]);
+							return 0;
+						}
+						break;
+					}
+				} /* switch */
+				i++;
+			} else { /* doesn't require argument */
+
+			}
+		} /* for */
+
+		if(!found) {
+			fprintf(stderr, "Argument '%s' not found.\n", argv[i]);
+			return 0;
+		}
+	} /* for */
+	
+	return 1;
+}
+
+
+void print_help(FILE*f, int noptions, struct option * options) {
+	int j;
+	for (j=0;j<noptions;j++) {
+		fprintf(f, "%s\t\t\t%s ", options[j].name, options[j].desc);
+			
+		if(options[j].value_pointer)
+		switch(options[j].type) {
+			case(OPTION_INT): {
+				int * value_pointer = (int*) options[j].value_pointer;
+				fprintf(f, "default: '%d'", *value_pointer);
+				break;
+			}
+
+			case(OPTION_STRING): {
+				char** value_pointer = (char**) options[j].value_pointer;
+				fprintf(f, "default: '%s'", *value_pointer);
+				break;
+			}
+			
+			case(OPTION_DOUBLE): {
+				double * value_pointer = (double*) options[j].value_pointer;
+				fprintf(f, "default: '%g'", *value_pointer);
+				break;
+			}
+		} /* switch */
+		
+		fprintf(f,"\n");
+
+	}
+}

sm/lib/options/options.h

+/* $Id: options.h 764 2005-04-26 08:45:06Z andrea $ */
+#ifndef H_OPTIONS
+#define H_OPTIONS
+
+#include <stdio.h>
+
+/**
+ *  Utility functions to parse command line arguments.
+ *
+ *  See options_example.c.
+ */
+
+ enum option_type { OPTION_STRING, OPTION_INT, OPTION_DOUBLE };
+ 
+struct option {
+	/** Name of the option. */
+	const char * name;
+	const char * desc;
+
+	/** Value type (if any, otherwise ignored). */
+	enum option_type type;
+	
+	/** Pointer to store param value. If value_pointer ==NULL then
+	 *  the option has no parameters. Ex: in "--port 2000", "--port"
+	 *  is the name and "2000" is the value. value_pointer is interpreted
+	 *  according to the value of "type".
+	 *   type=   INT:	value_pointer is a "int *"
+	 *   type=STRING:	value_pointer is a "char **"
+	 *   type=DOUBLE:	value_pointer is a "double *"
+	 *      A new string is allocated using malloc():
+	 *          *(value_pointer) = malloc( ... )
+	 */
+	void * value_pointer;
+
+
+	/** If not NULL, it is set to 1 if the option is found. */
+	int * set_pointer;
+};
+
+
+/* 0 on error */
+int auto_option(int argc, const char* argv[], int noptions, struct option * options);
+
+void print_help(FILE*where, int noptions, struct option * options);
+
+#endif

sm/src/icp/icp_corr_tricks.c

@@ -195,7 +195,8 @@ void find_correspondences_tricks(struct sm_params*params, gsl_vector* x_old) {
 		last_best = j1;
 		ld_set_correspondence(laser_sens, i, j1, j2);
 	}
-	
+
+	gsl_vector_free(p_i_w);
 }
 
 

sm/src/math_utils.h

@@ -11,7 +11,7 @@ void possible_interval(
 	const gsl_vector*p_i_w, struct laser_data*laser_sens, 
 	double maxAngularCorrectionDeg, double maxLinearCorrection, int*from, int*to, int*start_cell);
 
-void transform(const gsl_vector* point, const gsl_vector* x, gsl_vector*result);
+void transform(const gsl_vector* point2d, const gsl_vector* pose, gsl_vector*result2d);
 
 void gsl_vector_set_nan(gsl_vector*v);