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
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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;
00052 int j;
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
00160
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