orientation.c
Go to the documentation of this file.
00001 #include <math.h>
00002 #include <gsl/gsl_nan.h>
00003 
00004 #include "csm_all.h"
00005 
00006 #include <egsl/egsl_macros.h>
00007 
00008 
00009 void find_neighbours(LDP ld, int i, int max_num, int*indexes, size_t*num_found);
00010 void filter_orientation(double theta0, double rho0, size_t n,
00011         const double*thetas, const double*rhos, double *alpha, double*cov0_alpha );
00012 
00014 void ld_compute_orientation(LDP ld, int size_neighbourhood, double sigma) {
00015         int i;
00016         for(i=0;i<ld->nrays;i++){
00017                 if(!ld_valid_ray(ld,i) || (ld->cluster[i] == -1)) {
00018                         ld->alpha[i] = GSL_NAN;
00019                         ld->cov_alpha[i] = GSL_NAN;
00020                         ld->alpha_valid[i] = 0;
00021                         continue;
00022                 }
00023                 
00024                 int neighbours[size_neighbourhood*2];
00025                 size_t num_neighbours;
00026                 find_neighbours(ld, i, size_neighbourhood, neighbours, &num_neighbours);
00027 
00028                 if(0==num_neighbours) {
00029                         ld->alpha[i] = GSL_NAN;
00030                         ld->cov_alpha[i] = GSL_NAN;
00031                         ld->alpha_valid[i] = 0;
00032                         continue;
00033                 }
00034 
00035 /*              printf("orientation for i=%d:\n",i); */
00036                 double thetas[num_neighbours];
00037                 double readings[num_neighbours];
00038                 size_t a=0; 
00039                 for(a=0;a<num_neighbours;a++) {
00040                         thetas[a] = ld->theta[neighbours[a]];
00041                         readings[a] = ld->readings[neighbours[a]];
00042                         /* printf(" j = %d theta = %f rho = %f\n", neighbours[a], thetas[a],readings[a]); */
00043                 }
00044                 
00045                 double alpha=42, cov0_alpha=32;
00046                 filter_orientation(ld->theta[i],ld->readings[i],num_neighbours,
00047                         thetas,readings,&alpha,&cov0_alpha);
00048                 
00049                 if(gsl_isnan(alpha)) {
00050                         ld->alpha[i] = GSL_NAN;
00051                         ld->cov_alpha[i] = GSL_NAN;
00052                         ld->alpha_valid[i] = 0;
00053                 } else {  
00054                         ld->alpha[i] = alpha;
00055                         ld->cov_alpha[i] = cov0_alpha * square(sigma);
00056                         ld->alpha_valid[i] = 1;
00057                 }
00058                 /* printf("---------- i = %d alpha = %f sigma=%f cov_alpha = %f\n", i, alpha, ld->cov_alpha[i]);*/
00059         }
00060 }
00061 
00064 void filter_orientation(double theta0, double rho0, size_t n,
00065         const double*thetas, const double*rhos, double *alpha, double*cov0_alpha ) {
00066         
00067         egsl_push();
00068         /* Y = L x + R epsilon */
00069         val Y = zeros(n,1);
00070         val L = ones(n,1);
00071         val R = zeros(n,n+1);
00072 
00073         size_t i; for(i=0;i<n;i++) {
00074                 *egsl_atmp(Y, i, 0)   = (rhos[i]-rho0)/(thetas[i]-theta0);
00075                 *egsl_atmp(R, i, 0)   =             -1/(thetas[i]-theta0);
00076                 *egsl_atmp(R, i, i+1) =             +1/(thetas[i]-theta0);
00077         }
00078 
00079         val eRinv = inv(m(R, tr(R)));
00080         val vcov_f1 = inv(m3(tr(L),eRinv, L));
00081         val vf1 =   m4(vcov_f1, tr(L), eRinv, Y);
00082         
00083         double cov_f1 = *egsl_atmp(vcov_f1,0,0);
00084         double f1 = *egsl_atmp(vf1,0,0);
00085 
00086         *alpha = theta0 - atan(f1/rho0);
00087 
00088         if(cos(*alpha)*cos(theta0)+sin(*alpha)*sin(theta0)>0)
00089                 *alpha = *alpha + M_PI;
00090         
00091         double dalpha_df1  = rho0 / (square(rho0) + square(f1));
00092         double dalpha_drho = -f1 /  (square(rho0) + square(f1));
00093         
00094         *cov0_alpha     = square(dalpha_df1) * cov_f1 + square(dalpha_drho);
00095 
00096 
00097         if(gsl_isnan(*alpha)) {
00098                 egsl_print("Y",Y);
00099                 egsl_print("L",L);
00100                 egsl_print("R",R);
00101                 egsl_print("eRinv",eRinv);
00102                 egsl_print("vcov_f1",vcov_f1);
00103                 
00104                 printf("   f1 = %f cov =%f \n", f1,cov_f1);
00105                 printf("   f1/rho = %f \n", f1/rho0);
00106                 printf("   atan = %f \n", atan(f1/rho0));
00107                 printf("   theta0= %f \n", theta0);
00108         }
00109         
00110         egsl_pop();
00111 /*
00112 //      printf("dalpha_df1 = %f dalpha_drho = %f\n",dalpha_df1,dalpha_drho);
00113 //      printf("f1 = %f covf1 = %f alpha = %f cov_alpha = %f\n ",f1,cov_f1,*alpha,*cov0_alpha);
00114 //      printf("sotto = %f\n ",(square(rho0) + square(f1)));
00115         
00116 //      printf("   alpha = %f sigma= %f°\n", *alpha, rad2deg(0.01*sqrt(*cov0_alpha)));
00117 
00118         printf("l= ");
00119         gsl_matrix_fprintf(stdout, l, "%f");
00120         printf("\ny= ");
00121         gsl_matrix_fprintf(stdout, y, "%f");
00122         printf("\nr= ");
00123         gsl_matrix_fprintf(stdout, r, "%f");
00124         printf("\ninv(r*r)= ");
00125         gsl_matrix_fprintf(stdout, Rinv, "%f");
00126         printf("\nf1 = %lf ",f1);
00127         printf("\ncov_f1 = %lf ",cov_f1);
00128 */
00129 }
00130 
00131 /* indexes: an array of size "max_num*2" */
00132 void find_neighbours(LDP ld, int i, int max_num, int*indexes, size_t*num_found) {
00133         *num_found = 0;
00134         
00135         int up = i; 
00136         while ((up+1 <= i+max_num) && (up+1<ld->nrays) && ld_valid_ray(ld,up+1)
00137                         && (ld->cluster[up+1] == ld->cluster[i])) {
00138                 up+=1; 
00139                 indexes[(*num_found)++] = up;
00140         }
00141         int down = i; 
00142         while ((down >= i-max_num) && (down-1>=0) && ld_valid_ray(ld,down-1) && 
00143                         (ld->cluster[down-1] == ld->cluster[i])) {
00144                 down-=1;
00145                 indexes[(*num_found)++] = down;
00146         }
00147 }
00148 
00149 


csm_ros
Author(s): Gaël Ecorchard , Ivan Dryanovski, William Morris, Andrea Censi
autogenerated on Sat Jun 8 2019 19:52:42