ld_linearize.c
Go to the documentation of this file.
00001 #include <options/options.h>
00002 #include "../csm/csm_all.h"
00003 
00004 void ld_linearize(LDP ld);
00005 double weighted_mean(double *x, double *weight, int n);
00006 
00007 int main(int argc, const char * argv[]) {
00008         sm_set_program_name(argv[0]);
00009         
00010         int errors = 0;
00011         int count = -1;
00012         LDP ld;
00013         while( (ld = ld_read_smart(stdin)) ) {
00014                 count++;
00015                 if(!ld_valid_fields(ld))  {
00016                         sm_error("Invalid laser data (#%d in file)\n", count);
00017                         errors++;
00018                         continue;
00019                 }
00020                 
00021                 ld_linearize(ld);
00022 
00023                 ld_write_as_json(ld, stdout);
00024 
00025                 ld_free(ld);
00026         }
00027         
00028         return errors;
00029 }
00030 
00031 double weighted_mean(double *x, double *weight, int n) {
00032         double sum_weight = 0;
00033         double sum_x = 0;
00034         int i;
00035         for(i=0;i<n;i++) {
00036                 sum_x += weight[i] * x[i];
00037                 sum_weight += weight[i];
00038         }
00039         return sum_x / sum_weight;
00040 }
00041 
00042 
00043 void ld_linearize(LDP ld) {
00044         int i;
00045         for(i=0;i<ld->nrays;i++) {
00046                 if(-1 == ld->cluster[i]) continue;
00047                 
00048                 int this_cluster = ld->cluster[i];
00049                 int indexes[ld->nrays];
00050                 int nindexes = 0;
00051                 
00052                 int j;
00053                 for(j=i;j<ld->nrays;j++)
00054                         if(ld->cluster[j]==this_cluster) 
00055                                 indexes[nindexes++] = j;
00056                 
00057                 double alpha[nindexes];
00058                 double alpha_weight[nindexes];
00059                 for(j=0;j<nindexes;j++) { 
00060                         alpha[j] = ld->alpha[indexes[j]];
00061                         alpha_weight[j] = 1 / ld->cov_alpha[indexes[j]];
00062                 }
00063         
00064                 double est_alpha = weighted_mean(alpha, alpha_weight, nindexes);
00065         
00066                 double rho[nindexes];
00067                 double rho_weight[nindexes];
00068                 for(j=0;j<nindexes;j++) { 
00069                         int i = indexes[j];
00070                         double theta = ld->theta[i];
00071                         double x = cos(theta)  * ld->readings[i];
00072                         double y = sin(theta)  * ld->readings[i];
00073                         rho[j] = cos(est_alpha) * x + sin(est_alpha) * y;
00074                         rho_weight[j] = 1 / ( cos(est_alpha) * cos(theta) 
00075                                 + sin(est_alpha) * sin(theta) );
00076                 }
00077         
00078                 double est_rho = weighted_mean(rho, rho_weight, nindexes);
00079         
00080                 for(j=0;j<nindexes;j++) { 
00081                         int i = indexes[j];
00082                         double theta = ld->theta[i];
00083                         ld->readings[i] = est_rho / (cos(est_alpha) * cos(theta) 
00084                                 + sin(est_alpha) * sin(theta));
00085                 }
00086         
00087                 i = indexes[nindexes-1];
00088         }
00089         
00090 }
00091 
00092 
00093 
00094 


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