ld_cluster_curv.c
Go to the documentation of this file.
00001 #include <options/options.h>
00002 #include "../csm/csm_all.h"
00003 
00004 struct {
00006         double scale_deg; 
00007         
00009         int neighbours;
00010 } p;
00011 
00012 void ld_cluster_curv(LDP ld) ;
00013 
00014 int main(int argc, const char * argv[]) {
00015         sm_set_program_name(argv[0]);
00016 /*      
00017         struct option* ops = options_allocate(3);
00018         options_double(ops, "scale_deg", &p.scale_deg, 0.0, "Scale factor (degrees) ");
00019         options_int(ops, "neighbours", &p.neighbours, 1, "How many neighbours to consider (regardless of scale).");
00020                 
00021         if(!options_parse_args(ops, argc, argv)) {
00022                 fprintf(stderr, "A simple program for smoothing a sensor scan.\n\nUsage:\n");
00023                 options_print_help(ops, stderr);
00024                 return -1;
00025         }
00026 */      
00027 /* jj_set_stream(open_file_for_writing("ld_cluster_curv.txt")); */
00028 
00029         int errors = 0;
00030         int count = -1;
00031         LDP ld;
00032         while( (ld = ld_read_smart(stdin)) ) {
00033                 count++;
00034                 if(!ld_valid_fields(ld))  {
00035                         sm_error("Invalid laser data (#%d in file)\n", count);
00036                         return -1;
00037                 }
00038                 
00039                 ld_cluster_curv(ld);
00040 
00041                 ld_write_as_json(ld, stdout);
00042 
00043                 ld_free(ld);
00044         }
00045         
00046         return errors;
00047 }
00048 
00049 void cluster_convolve(const int*cluster,const double*original, int n, double*dest, double*filter, int filter_len, int negate_negative) 
00050 {
00051         int i; /* index on the points */
00052         int j; /* index on the filter */
00053         
00054         for(i=0;i<n;i++) {
00055                 if(cluster[i] == -1) {
00056                         dest[i] = GSL_NAN;
00057                         continue;
00058                 }
00059                 
00060                 dest[i] = 0;
00061                 for(j=-(filter_len-1);j<=(filter_len-1);j++) {
00062                         int i2 = i + j;
00063                         if(i2<0) i2=0; if(i2>=n) i2=n-1;
00064                         if(cluster[i2] != cluster[i]) i2 = i; 
00065                         double coeff = filter[abs(j)];
00066                         if(j<0 && negate_negative) coeff *= -1;
00067                         dest[i] += original[i2] * coeff;
00068 
00069                         if(is_nan(dest[i]))  
00070                                 sm_error("i: %d; something wrong after processing i2: %d  cluster[i2]=%d original[i2] = %f \n", i, i2, cluster[i2], original[i2]);
00071                         
00072                 }
00073                 
00074         }
00075 }
00076 
00077 int cluster_find_max(int*cluster, double*v, int n) {
00078         int i, max = -1;
00079         for(i=0;i<n;i++) {
00080                 if(cluster[i] == -1) continue;
00081                 if( (max == -1) || (v[i] > v[max]) )
00082                         max = i;
00083         }
00084         return max;
00085 }
00086 
00087 int find_max(int *v, int n) {
00088         int i, max = -1;
00089         for(i=0;i<n;i++) {
00090                 if( (max == -1) || (v[i] > v[max]) )
00091                         max = i;
00092         }
00093         return max;     
00094 }
00095 
00096 int ld_max_cluster_id(LDP ld) {
00097         return ld->cluster[ find_max(ld->cluster, ld->nrays)];
00098 }
00099 
00100 int ld_cluster_size(LDP ld, int i0) {
00101         int this_cluster = ld->cluster[i0];
00102         int num = 0; int i;
00103         
00104         for(i=i0;i<ld->nrays;i++)
00105                 if(ld->cluster[i] == this_cluster)
00106                         num++;
00107                 else if(ld->cluster[i] != -1) break;
00108 
00109         return num;
00110 }
00111 
00112 void ld_remove_small_clusters(LDP ld, int min_size) {
00113         int i;
00114         for(i=0;i<ld->nrays;) {
00115                 int this_cluster = ld->cluster[i];
00116 
00117                 if(this_cluster == -1) { i++; continue; }
00118                 int cluster_size = ld_cluster_size(ld, i);
00119 
00120                 if(cluster_size < min_size)  {
00121                         for(;i<ld->nrays;i++)
00122                                 if(ld->cluster[i] == this_cluster)
00123                                         ld->cluster[i] = -1;
00124                                 else if(ld->cluster[i] != -1) break;
00125                 } else i++;
00126         }
00127 }
00128 
00129 void ld_mark_cluster_as_invalid(LDP ld, int cluster) {
00130         int i;
00131         for(i=0;i<ld->nrays;i++) {
00132                 if(ld->cluster[i] == cluster)
00133                         ld->valid[i] = 0;
00134         }
00135 }
00136 
00137 void array_abs(double*v, int n) {
00138         int i=0; for(i=0;i<n;i++) v[i] = fabs(v[i]);
00139 }
00140 
00141 
00142 void ld_cluster_curv(LDP ld) {
00143         int min_cluster_size = 10;
00144         double sigma = 0.005; 
00145         int orientation_neighbours = 4;
00146         int npeaks = 5;
00147         double near_peak_threshold = 0.4;
00148 
00149         if(JJ) jj_context_enter("ld_cluster_curv");
00150         int n = ld->nrays;
00151         
00152         
00153         if(JJ) jj_add_int_array("a00valid", ld->valid, n);
00154         if(JJ) jj_add_double_array("a01theta", ld->theta, n);
00155         if(JJ) jj_add_double_array("a02readings", ld->readings, n);
00156         
00157         
00158         ld_simple_clustering(ld, sigma*5);
00159 /*      int i=0; for(i=0;i<n;i++)
00160                 ld->cluster[i] = ld->valid[i] ? 1 : -1;*/
00161         
00162         
00163         if(JJ) jj_add_int_array("a04cluster", ld->cluster, n);
00164         ld_remove_small_clusters(ld, min_cluster_size);
00165         ld_mark_cluster_as_invalid(ld, -1);
00166         if(JJ) jj_add_int_array("a06cluster", ld->cluster, n);
00167         
00168         double filter[10] = {.5, .4, .3, .2, .2, .2, .2, .2, .2, .2};
00169         double deriv_filter[7] = {0, .6, .3, .2, .2, .2, .1};
00170         double smooth_alpha[n];
00171         double deriv_alpha[n];
00172 
00173         int p;
00174         if(JJ) jj_loop_enter("it");
00175         
00176         for(p=0;p<npeaks;p++) {  if(JJ) jj_loop_iteration();
00177                 
00178                 if(JJ) jj_add_int_array("cluster", ld->cluster, n);
00179 
00180                 ld_compute_orientation(ld, orientation_neighbours, sigma);
00181                 
00182                 int i;
00183                 for(i=0;i<ld->nrays;i++) 
00184                         if(!ld->alpha_valid[i])
00185                         ld->cluster[i] = -1;
00186                 
00187                 if(JJ) jj_add_double_array("alpha", ld->alpha, n);
00188                 cluster_convolve(ld->cluster, ld->alpha, n, smooth_alpha, filter, 10, 0);
00189                 if(JJ) jj_add_int_array("alpha_valid", ld->alpha_valid, n);
00190 
00191                 if(JJ) jj_add_double_array("smooth_alpha", smooth_alpha, n);
00192                 cluster_convolve(ld->cluster, smooth_alpha, n, deriv_alpha, deriv_filter, 7, 1);
00193                 if(JJ) jj_add_double_array("deriv_alpha", deriv_alpha, n);
00194                 array_abs(deriv_alpha, n);
00195                 
00196                 int peak = cluster_find_max(ld->cluster, deriv_alpha, n);
00197                 if(JJ) jj_add_int("peak", peak);
00198                 
00199                 int peak_cluster = ld->cluster[peak];
00200                 int up = peak; double threshold = near_peak_threshold  * deriv_alpha[peak];
00201                 while(up<n-1 && (ld->cluster[up]==peak_cluster) && deriv_alpha[up+1] >  threshold) up++;
00202                 int down = peak;
00203                 while(down>1  && (ld->cluster[up]==peak_cluster) && deriv_alpha[down-1] > threshold) down--;
00204                 int j;
00205                 for(j=down;j<=up;j++) {
00206                         ld->cluster[j] = -1;
00207                         ld->valid[j] = 0;
00208                         ld->readings[j] = NAN;
00209                 }
00210                 
00211                 int next_cluster = ld_max_cluster_id(ld) + 1;
00212                 for(j = up+1; j<ld->nrays; j++) {
00213                         if(ld->cluster[j] == peak_cluster)
00214                                 ld->cluster[j] = next_cluster;
00215                 }
00216         }
00217         if(JJ) jj_loop_exit();
00218 
00219         if(JJ) jj_context_exit();
00220 }
00221 
00222 


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