ld_resample.c
Go to the documentation of this file.
00001 #include <gsl/gsl_rng.h>
00002 #include <gsl/gsl_randist.h>
00003 #include <gsl/gsl_math.h>
00004 
00005 #include <math.h>
00006 #include <options/options.h>
00007 
00008 
00009 #include "../csm/csm_all.h"
00010 
00011 struct {
00012         double interval;
00013         int seed;
00014 }p ;
00015 
00016 LDP ld_resample(LDP ld);
00017 
00018 gsl_rng * rng;
00019 
00020 int main(int argc, const char ** argv) {
00021         sm_set_program_name(argv[0]);
00022         
00023         
00024         struct option* ops = options_allocate(3);
00025         options_double(ops, "interval", &p.interval, sqrt(2.0), " 1 = no resampling");
00026                 
00027         if(!options_parse_args(ops, argc, argv)) {
00028                 options_print_help(ops, stderr);
00029                 return -1;
00030         }
00031 
00032         int count = -1;
00033         LDP ld;
00034         while( (ld = ld_read_smart(stdin))) {
00035                 count++;
00036                 
00037                 if(!ld_valid_fields(ld))  {
00038                         sm_error("Invalid laser data (#%d in file)\n", count);
00039                         continue;
00040                 }
00041                 
00042 /*              if(count & 1) {*/
00043                         LDP ld2 = ld_resample(ld);
00044                         ld_write_as_json(ld2, stdout);
00045                         ld_free(ld2);
00046                         ld_free(ld);
00047 /*              } else {
00048                         ld_write_as_json(ld, stdout);
00049                         ld_free(ld);
00050                 }*/
00051 
00052                 count++;
00053         }
00054         
00055         return 0;
00056 }
00057 
00058 LDP ld_resample(LDP ld) {
00059         /* FIXME magic number */
00060         int n = (int) (floor(ld->nrays / p.interval));
00061         
00062         LDP ld2 = ld_alloc_new(n);      
00063         int k;
00064         for(k=0;k<n;k++) {
00065                 double index = k * p.interval;
00066                 int i = (int) floor(index);
00067                 int j = i + 1;
00068                 double a = 1 - (index - i);
00069         
00070                 
00071                 if(  (j>= ld->nrays) || !ld->valid[i] || !ld->valid[j]) {
00072                         ld2->valid[k] = 0;
00073                         ld2->readings[k] = NAN;
00074                         ld2->alpha_valid[k] = 0;
00075                         ld2->alpha[k] = NAN;
00076                         ld2->theta[k] = ld->theta[i];
00077                 } else {
00078                         ld2->theta[k] = a * ld->theta[i] + (1-a) * ld->theta[j];
00079                         
00080         
00081                         if(is_nan(ld2->theta[k])) {
00082                                 sm_debug("Hey, k=%d theta[%d]=%f theta[%d]=%f\n",
00083                                         k,i,ld->theta[i],j,ld->theta[j]);
00084                         }
00085 
00086                         ld2->readings[k] = a * ld->readings[i] + (1-a) * ld->readings[j];
00087                         ld2->valid[k] = 1;
00088                 }
00089                 
00090 /*              sm_debug("k=%d index=%f i=%d a=%f valid %d reading %f\n", k,index,i,a,ld2->valid[k],ld2->readings[k]);*/
00091         }
00092         
00093         ld2->min_theta = ld2->theta[0];
00094         ld2->max_theta = ld2->theta[n-1];
00095         ld2->tv = ld->tv;
00096 
00097         copy_d(ld->odometry, 3, ld2->odometry);
00098         copy_d(ld->estimate, 3, ld2->estimate);
00099         
00100         
00101         return ld2;
00102 }
00103 


csm
Author(s): Andrea Censi
autogenerated on Mon Jan 16 2017 03:48:29