From d01224736ca6aa0d4f757a9dde751a4ae3ab86c5 Mon Sep 17 00:00:00 2001
From: Andrea Censi <andrea@cds.caltech.edu>
Date: Sat, 6 Feb 2010 07:38:21 +0000
Subject: [PATCH] added purify command
---
sm/CMakeLists.txt | 1 +
sm/apps/ld_purify.c | 87 +++++++++++++++++++++++++++++++++++++++++++
sm/apps/ld_resample.c | 42 ++++++++-------------
sm/csm/laser_data.c | 2 +-
4 files changed, 105 insertions(+), 27 deletions(-)
create mode 100644 sm/apps/ld_purify.c
diff --git a/sm/CMakeLists.txt b/sm/CMakeLists.txt
index 56cb187..87a8818 100644
--- a/sm/CMakeLists.txt
+++ b/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)
diff --git a/sm/apps/ld_purify.c b/sm/apps/ld_purify.c
new file mode 100644
index 0000000..53de455
--- /dev/null
+++ b/sm/apps/ld_purify.c
@@ -0,0 +1,87 @@
+#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;
+ }
+
+ }
+
+}
+
+
+
diff --git a/sm/apps/ld_resample.c b/sm/apps/ld_resample.c
index e6b600b..95b8620 100644
--- a/sm/apps/ld_resample.c
+++ b/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;
}
diff --git a/sm/csm/laser_data.c b/sm/csm/laser_data.c
index 6e6820b..7f47fb7 100644
--- a/sm/csm/laser_data.c
+++ b/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;
}
--
GitLab