icp_outliers.c
Go to the documentation of this file.
00001 #include <math.h>
00002 
00003 #include "icp.h"
00004 
00005 void quicksort(double *array, int begin, int end);
00006 double hoare_selection(double *data, int start, int end, int k);
00007 
00009 void visibilityTest(LDP laser_ref, const gsl_vector*u) {
00010 
00011         double theta_from_u[laser_ref->nrays];
00012         
00013         int j;
00014         for(j=0;j<laser_ref->nrays;j++) {
00015                 if(!ld_valid_ray(laser_ref,j)) continue;
00016                 theta_from_u[j] = 
00017                         atan2(gvg(u,1)-laser_ref->points[j].p[1],
00018                               gvg(u,0)-laser_ref->points[j].p[0]);
00019         }
00020         
00021         sm_debug("\tvisibility: Found outliers: ");
00022         int invalid = 0;
00023         for(j=1;j<laser_ref->nrays;j++) {
00024                 if(!ld_valid_ray(laser_ref,j)||!ld_valid_ray(laser_ref,j-1)) continue;
00025                 if(theta_from_u[j]<theta_from_u[j-1]) {
00026                         laser_ref->valid[j] = 0;
00027                         invalid ++;
00028                         sm_debug("%d ",j);
00029                 }
00030         }
00031         sm_debug("\n");
00032 }
00033 
00034 
00042 void kill_outliers_double(struct sm_params*params) {
00043         double threshold = 3; /* TODO: add as configurable */
00044 
00045         LDP laser_ref  = params->laser_ref;
00046         LDP laser_sens = params->laser_sens;
00047 
00048         double dist2_i[laser_sens->nrays];
00049         double dist2_j[laser_ref->nrays];
00050         int j; for(j=0;j<laser_ref->nrays;j++) 
00051                 dist2_j[j]= 1000000;
00052         
00053         int i;
00054         for(i=0;i<laser_sens->nrays;i++) {
00055                 if(!ld_valid_corr(laser_sens, i)) continue;
00056                 int j1 = laser_sens->corr[i].j1;
00057                 dist2_i[i] = laser_sens->corr[i].dist2_j1;
00058                 dist2_j[j1] = GSL_MIN(dist2_j[j1], dist2_i[i]);
00059         }
00060         
00061         int nkilled = 0;
00062         for(i=0;i<laser_sens->nrays;i++) {
00063                 if(!ld_valid_corr(laser_sens, i)) continue;
00064                 int j1 = laser_sens->corr[i].j1;
00065                 if(dist2_i[i] > (threshold*threshold)*dist2_j[j1]) {
00066                         laser_sens->corr[i].valid=0;
00067                         nkilled ++;
00068                 }
00069         }
00070         sm_debug("\tkill_outliers_double: killed %d correspondences\n",nkilled);
00071 }
00072         
00083 void kill_outliers_trim(struct sm_params*params,  double*total_error) {
00084                 
00085         if(JJ) jj_context_enter("kill_outliers_trim");
00086                 
00087         LDP laser_ref  = params->laser_ref;
00088         LDP laser_sens = params->laser_sens;
00089         
00090         /* dist2, indexed by k, contains the error for the k-th correspondence */
00091         int k = 0; 
00092         double dist2[laser_sens->nrays];
00093                 
00094         int i;
00095         double dist[laser_sens->nrays];
00096         /* for each point in laser_sens */
00097         for(i=0;i<laser_sens->nrays;i++) {
00098                 /* which has a valid correspondence */
00099                 if(!ld_valid_corr(laser_sens, i)) { dist[i]=NAN; continue; }
00100                 double *p_i_w = laser_sens->points_w[i].p;
00101                 
00102                 int j1 = laser_sens->corr[i].j1;
00103                 int j2 = laser_sens->corr[i].j2;
00104                 /* Compute the distance to the corresponding segment */
00105                 dist[i]=  dist_to_segment_d(
00106                         laser_ref->points[j1].p, laser_ref->points[j2].p, p_i_w);
00107                 dist2[k] = dist[i];
00108                 k++;    
00109         }
00110         
00111         
00112         if(JJ) jj_add_int("num_valid_before", k);
00113         if(JJ) jj_add_double_array("dist_points", dist2, laser_sens->nrays);
00114         if(JJ) jj_add_double_array("dist_corr_unsorted", dist2, k);
00115 
00116 #if 0   
00117         double dist2_copy[k]; for(i=0;i<k;i++) dist2_copy[i] = dist2[i];
00118 #endif 
00119 
00120         /* two errors limits are defined: */
00121                 /* In any case, we don't want more than outliers_maxPerc% */
00122                 int order = (int)floor(k*(params->outliers_maxPerc));
00123                         order = GSL_MAX(0, GSL_MIN(order, k-1));
00124 
00125         /* The dists for the correspondence are sorted
00126            in ascending order */
00127                 quicksort(dist2, 0, k-1);
00128                 double error_limit1 = dist2[order];
00129                 if(JJ) jj_add_double_array("dist_corr_sorted", dist2, k);
00130         
00131                 /* Then we take a order statics (o*K) */
00132                 /* And we say that the error must be less than alpha*dist(o*K) */
00133                 int order2 = (int)floor(k*params->outliers_adaptive_order);
00134                         order2 = GSL_MAX(0, GSL_MIN(order2, k-1));
00135                 double error_limit2 = params->outliers_adaptive_mult*dist2[order2];
00136         
00137         double error_limit = GSL_MIN(error_limit1, error_limit2);
00138         
00139 #if 0
00140         double error_limit1_ho = hoare_selection(dist2_copy, 0, k-1, order);
00141         double error_limit2_ho = error_limit2;
00142         if((error_limit1_ho != error_limit1) || (error_limit2_ho != error_limit2)) {
00143                 printf("%f == %f    %f  == %f\n",
00144                         error_limit1_ho, error_limit1, error_limit2_ho, error_limit2);
00145         }
00146 #endif
00147 
00148         if(JJ) jj_add_double_array("dist_corr_sorted", dist2, k);
00149         if(JJ) jj_add_double("error_limit_max_perc", error_limit1);
00150         if(JJ) jj_add_double("error_limit_adaptive", error_limit2);
00151         if(JJ) jj_add_double("error_limit", error_limit);
00152         
00153         sm_debug("\ticp_outliers: maxPerc %f error_limit: fix %f adaptive %f \n",
00154                 params->outliers_maxPerc,error_limit1,error_limit2);
00155 
00156         *total_error = 0;
00157         int nvalid = 0;
00158         for(i=0;i<laser_sens->nrays;i++) {
00159                 if(!ld_valid_corr(laser_sens, i)) continue;
00160                 if(dist[i] > error_limit) {
00161                         laser_sens->corr[i].valid = 0;
00162                         laser_sens->corr[i].j1 = -1;
00163                         laser_sens->corr[i].j2 = -1;
00164                 } else {
00165                         nvalid++;
00166                         *total_error += dist[i];
00167                 }
00168         }
00169         
00170         sm_debug("\ticp_outliers: valid %d/%d (limit: %f) mean error = %f \n",nvalid,k,error_limit,
00171                 *total_error/nvalid);   
00172 
00173         if(JJ) jj_add_int("num_valid_after", nvalid);
00174         if(JJ) jj_add_double("total_error", *total_error);
00175         if(JJ) jj_add_double("mean_error", *total_error / nvalid);
00176                 
00177         if(JJ) jj_context_exit();
00178 }
00179 
00180 
00181 void swap_double(double*a,double*b) {
00182         double t = *a; *a = *b; *b=t;
00183 }
00184 
00186 void quicksort(double *array, int begin, int end) {
00187         if (end > begin) {
00188                 double pivot = array[begin];
00189                 int l = begin + 1;
00190                 int r = end+1;
00191                 while(l < r) {
00192                         if (array[l] < pivot) {
00193                                 l++;
00194                         } else {
00195                                 r--;
00196                                 swap_double(array+l, array+r); 
00197                         }
00198                 }
00199                 l--;
00200                 swap_double(array+begin, array+l);
00201                 if(l>begin)
00202                 quicksort(array, begin, l);
00203                 if(end>r)
00204                 quicksort(array, r, end);
00205         }
00206 }
00207 
00208 #if 0
00209 double hoare_selection(double *data, int start, int end, int k)
00210 {
00211   int pivotIndex, i, j;
00212   float pivotValue, tmp;
00213 
00214   while(start < end) {
00215           //select a random pivot
00216         pivotIndex = start + (end-start)/2;
00217          pivotValue = data[pivotIndex];
00218                                  //sort the array into two sub arrays around the pivot value
00219          i = start;
00220          j = end;
00221          while(i <= j) {
00222                 while(data[i] < pivotValue)
00223                   i++;
00224                 while(data[j] > pivotValue)
00225                   j--;
00226                 if(i<=j) {
00227                   tmp = data[i];
00228                   data[i] = data[j];
00229                   data[j] = tmp;
00230                   i++;
00231                   j--;
00232                 }
00233          }
00234                          //continue search in left sub array
00235          if(j < k)
00236                 start = i;
00237          if(k < i) //continue search in right sub array
00238                 end = j;
00239   }
00240   return(data[k]);
00241 }
00242 
00243 #endif


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