added purify command

sm/CMakeLists.txt

@@ -98,6 +98,7 @@ new_executable(ld_remove_doubles)
 new_executable(ld_alternate)
 new_executable(ld_recover)
 
+new_executable(ld_purify)
 new_executable(ld_select)
 new_executable(ld_cluster_curv)
 new_executable(ld_linearize)

sm/apps/ld_purify.c

+#include <gsl/gsl_rng.h>
+#include <gsl/gsl_randist.h>
+
+#include <math.h>
+#include <libgen.h>
+
+#include <options/options.h>
+#include "../csm/csm_all.h"
+
+void purify(LDP ld, double threshold_min, double threshold_max);
+
+
+struct ld_purify_params {
+	double threshold_min, threshold_max;
+	const char* file_input;
+	const char* file_output;
+};
+
+
+int main(int argc, const char * argv[]) {
+	sm_set_program_name(argv[0]);
+	
+	
+	options_banner("ld_purify: Makes sure that the file format is valid. \n * Sets valid=0 if reading is outside interval ");
+	
+	struct ld_purify_params p;
+	
+	struct option* ops = options_allocate(20);
+	options_double(ops, "threshold_min", &p.threshold_min, 0.01, 
+		"Sets valid=0 if readings are less than this threshold.");
+	options_double(ops, "threshold_max", &p.threshold_max, 79.0, 
+		"Sets valid=0 if readings are more than this threshold.");
+	
+	options_string(ops, "in", &p.file_input, "stdin", "Input file ");
+	options_string(ops, "out", &p.file_output, "stdout", "Output file ");
+		
+		
+	if(!options_parse_args(ops, argc, argv)) {
+		options_print_help(ops, stderr);
+		return -1;
+	}
+
+	FILE * in = open_file_for_reading(p.file_input);
+	if(!in) return -3;
+
+	FILE * out = open_file_for_writing(p.file_output);
+	if(!out) return -2;
+
+
+
+	LDP ld; int count = -1;
+	while( (ld = ld_from_json_stream(in))) {
+		
+		purify(ld, p.threshold_min, p.threshold_max);
+		
+		if(!ld_valid_fields(ld))  {
+			sm_error("Wait, we didn't purify enough  (#%d in file)\n", count);
+			continue;
+		}
+		
+		ld_write_as_json(ld, out);
+		ld_free(ld);
+	}
+	
+	return 0;
+}
+
+
+
+void purify(LDP ld, double threshold_min, double threshold_max) {
+	for(int i=0;i<ld->nrays;i++) {
+		if(!ld->valid[i]) continue;
+		
+		double rho = ld->readings[i];
+		if( is_nan(rho) | (rho < threshold_min) | (rho > threshold_max) ) {
+			ld->readings[i] = GSL_NAN;
+			ld->valid[i] = 0;
+			ld->alpha[i] = GSL_NAN;
+			ld->alpha_valid[i] = 0;
+		}
+		
+	}
+	
+}
+
+
+

sm/apps/ld_resample.c

@@ -24,27 +24,11 @@ int main(int argc, const char ** argv) {
 	struct option* ops = options_allocate(3);
 	options_double(ops, "interval", &p.interval, sqrt(2.0), " 1 = no resampling");
 		
-	options_int(ops, "seed", &p.seed, 0, 
-		"Seed for random number generator (if 0, use GSL_RNG_SEED env. variable).");
-		
 	if(!options_parse_args(ops, argc, argv)) {
-/*		fprintf(stderr, "A simple program for adding slip to odometry \n\n"
-			"The 'odometry' field is set to 'true_pose' + noise.\n"
-			"If 'true_pose' is not available, then the 'odometry' \n"
-			"field is set to 'odometry' + noise.\n\n"
-			"Note: this program does *not* simulate the effect of \n"
-			"slip or odometry error in a realistic way; each scan \n"
-			"in the file is considered separately, the error does \n"
-			"not depend on the traveled distance, etc.\n\n");*/
 		options_print_help(ops, stderr);
 		return -1;
 	}
 
-	gsl_rng_env_setup();
-	rng = gsl_rng_alloc (gsl_rng_ranlxs0);
-	if(p.seed != 0)
-	gsl_rng_set(rng, (unsigned int) p.seed);
-
 	int count = -1;
 	LDP ld;
 	while( (ld = ld_read_smart(stdin))) {
@@ -73,27 +57,33 @@ int main(int argc, const char ** argv) {
 
 LDP ld_resample(LDP ld) {
 	/* FIXME magic number */
-	int n = (int) (floor(ld->nrays / p.interval))-4;
+	int n = (int) (floor(ld->nrays / p.interval));
 	
 	LDP ld2 = ld_alloc_new(n);	
 	int k;
 	for(k=0;k<n;k++) {
 		double index = k * p.interval;
 		int i = (int) floor(index);
+		int j = i + 1;
 		double a = 1 - (index - i);
-
-		ld2->theta[k] = a * ld->theta[i] + (1-a) * ld->theta[i+1];
-
-		if(is_nan(ld2->theta[k])) {
-			sm_debug("Hey, k=%d theta[%d]=%f theta[%d]=%f\n",
-				k,i,ld->theta[i],i+1,ld->theta[i+1]);
-		}
+	
 		
-		if(!ld->valid[i] || !ld->valid[i+1]) {
+		if(  (j>= ld->nrays) || !ld->valid[i] || !ld->valid[j]) {
 			ld2->valid[k] = 0;
 			ld2->readings[k] = NAN;
+			ld2->alpha_valid[k] = 0;
+			ld2->alpha[k] = NAN;
+			ld2->theta[k] = ld->theta[i];
 		} else {
-			ld2->readings[k] = a * ld->readings[i] + (1-a) * ld->readings[i+1];
+			ld2->theta[k] = a * ld->theta[i] + (1-a) * ld->theta[j];
+			
+	
+			if(is_nan(ld2->theta[k])) {
+				sm_debug("Hey, k=%d theta[%d]=%f theta[%d]=%f\n",
+					k,i,ld->theta[i],j,ld->theta[j]);
+			}
+
+			ld2->readings[k] = a * ld->readings[i] + (1-a) * ld->readings[j];
 			ld2->valid[k] = 1;
 		}
 		

sm/csm/laser_data.c

@@ -230,7 +230,7 @@ int ld_valid_fields(LDP ld)  {
 		} else {
 			/* ray not valid, but checking theta anyway */
 			if(is_nan(th)) {
-				sm_error("Ray #%d: valid = %d  but theta = %f",
+				sm_error("Ray #%d: valid = %d  but theta = %f\n",
 					i,  ld->valid[i], th);
 				return 0;
 			}