icp_corr_tricks.c
Go to the documentation of this file.
00001 #include <gsl/gsl_vector.h>
00002 
00003 #include "icp.h"
00004 #include "../csm_all.h"
00005 
00008 INLINE double mysin(double x) {
00009         const double a = -1.0/6.0;
00010         const double b = +1.0/120.0;
00011         double x2 = x*x;
00012         return x * (.99 + x2 * ( a + b * x2));
00013 }
00014 
00015 
00016 #define DEBUG_SEARCH(a) ;
00017 
00018 void ld_create_jump_tables(struct laser_data* ld) {
00019         int i;
00020         for(i=0;i<ld->nrays;i++) {
00021                 int j;
00022                 
00023                 j = i + 1;
00024                 while(j<ld->nrays && ld->valid[j] && ld->readings[j]<=ld->readings[i]) j++;
00025                 ld->up_bigger[i] = j-i;
00026 
00027                 j = i + 1;
00028                 while(j<ld->nrays && ld->valid[j] && ld->readings[j]>=ld->readings[i]) j++;
00029                 ld->up_smaller[i] = j-i;
00030                 
00031                 j = i - 1;
00032                 while(j>=0 && ld->valid[j] && ld->readings[j]>=ld->readings[i]) j--;
00033                 ld->down_smaller[i] = j-i;
00034 
00035                 j = i - 1;
00036                 while(j>=0 && ld->valid[j] && ld->readings[j]<=ld->readings[i]) j--;
00037                 ld->down_bigger[i] = j-i;
00038         }       
00039 }
00040 
00041 extern int distance_counter;
00042 
00043 INLINE double local_distance_squared_d(const double* a, const double* b)  {
00044         distance_counter++;
00045         double x = a[0] - b[0];
00046         double y = a[1] - b[1];
00047         return x*x + y*y;
00048 }
00049 
00050 #include "fast_math.h"
00051 
00052 
00053 #define SQUARE(a) ((a)*(a))
00054 
00055 /* Assumes that points_w is filled.  */
00056 void find_correspondences_tricks(struct sm_params*params) {
00057         const LDP laser_ref  = params->laser_ref;
00058         const LDP laser_sens = params->laser_sens;
00059         int i;
00060         
00061         /* Handy constant */
00062         double C1 =  (double)laser_ref->nrays / (laser_ref->max_theta-laser_ref->min_theta) ;
00063         double max_correspondence_dist2 = square(params->max_correspondence_dist);
00064         /* Last match */
00065         int last_best = -1;
00066         const point2d * restrict points_w = laser_sens->points_w;
00067         for(i=0;i<laser_sens->nrays;i++) {
00068                 if(!ld_valid_ray(laser_sens,i)) {
00069                         ld_set_null_correspondence(laser_sens,i);
00070                         continue; 
00071                 }
00072                 
00073                 const double *p_i_w = points_w[i].p;
00074                 double p_i_w_nrm2 = points_w[i].rho;
00075                 double p_i_w_phi = points_w[i].phi;
00076                 
00078                 int from = 0; 
00079                 int to = laser_ref->nrays-1; 
00080                 
00081                 int start_cell = (int) ((p_i_w_phi - laser_ref->min_theta) * C1); 
00082 
00084                 int j1 = -1;
00086                 double best_dist = 42;
00087                 
00089                 int we_start_at; 
00090                 if (last_best==-1)
00091                         we_start_at = start_cell;
00092                 else
00093                         we_start_at = last_best + 1;
00094                 
00095                 if(we_start_at > to) we_start_at = to;
00096                 if(we_start_at < from) we_start_at = from;
00097                 
00098                 int up =  we_start_at+1; 
00099                 int down = we_start_at; 
00100                 double last_dist_up = 0; /* first is down */
00101                 double last_dist_down = -1;     
00102 
00103                 int up_stopped = 0; 
00104                 int down_stopped = 0;
00105         
00106                 DEBUG_SEARCH(printf("i=%d p_i_w = %f %f\n",i, p_i_w[0], p_i_w,[1]));
00107                 DEBUG_SEARCH(printf("i=%d [from %d down %d mid %d up %d to %d]\n",
00108                         i,from,down,start_cell,up,to));
00109                 
00110                 while ( (!up_stopped) || (!down_stopped) ) {
00111                         int now_up = up_stopped ? 0 : 
00112                                    down_stopped ? 1 : last_dist_up < last_dist_down;
00113                         DEBUG_SEARCH(printf("|"));
00114 
00115                         /* Now two symmetric chunks of code, the now_up and the !now_up */
00116                         if(now_up) {
00117                                 DEBUG_SEARCH(printf("up %d ",up));
00118                                 /* If we have crossed the "to" boundary we stop searching
00119                                         on the "up" direction. */
00120                                 if(up > to) { 
00121                                         up_stopped = 1; continue; }
00122                                 /* Just ignore invalid rays */
00123                                 if(!laser_ref->valid[up]) { 
00124                                         ++up; continue; }
00125                                 
00126                                 /* This is the distance from p_i_w to the "up" point*/
00127                                 last_dist_up = local_distance_squared_d(p_i_w, laser_ref->points[up].p);
00128                                 
00129                                 /* If it is less than the best point, it is our new j1 */
00130                                 if((last_dist_up < best_dist) || (j1==-1) )
00131                                                 j1 = up, best_dist = last_dist_up;
00132                                 
00133                                 /* If we are moving away from start_cell */
00134                                 if (up > start_cell) {
00135                                         double delta_theta = (laser_ref->theta[up] - p_i_w_phi);
00136                                         /* We can compute a bound for early stopping. Currently
00137                                            our best point has distance best_dist; we can compute
00138                                            min_dist_up, which is the minimum distance that can have
00139                                            points for j>up (see figure)*/
00140                                         double min_dist_up = p_i_w_nrm2 * 
00141                                                 ((delta_theta > M_PI*0.5) ? 1 : mysin(delta_theta));
00142                                         /* If going up we can't make better than best_dist, then
00143                                             we stop searching in the "up" direction */
00144                                         if(SQUARE(min_dist_up) > best_dist) { 
00145                                                 up_stopped = 1; continue;
00146                                         }
00147                                         /* If we are moving away, then we can implement the jump tables
00148                                            optimizations. */
00149                                         up += 
00150                                                 /* If p_i_w is shorter than "up" */
00151                                                 (laser_ref->readings[up] < p_i_w_nrm2) 
00152                                                 ?
00153                                                 /* We can jump to a bigger point */
00154                                                 laser_ref->up_bigger[up] 
00155                                                 /* Or else we jump to a smaller point */ 
00156                                                 : laser_ref->up_smaller[up];
00157                                                 
00158                                 } else 
00159                                         /* If we are moving towards "start_cell", we can't do any
00160                                            ot the previous optimizations and we just move to the next point */
00161                                         ++up;
00162                         }
00163                         
00164                         /* This is the specular part of the previous chunk of code. */
00165                         if(!now_up) {
00166                                 DEBUG_SEARCH(printf("down %d ",down));
00167                                 if(down < from) { 
00168                                         down_stopped = 1; continue; }
00169                                 if(!laser_ref->valid[down]) { 
00170                                         --down; continue; }
00171                 
00172                                 last_dist_down = local_distance_squared_d(p_i_w, laser_ref->points[down].p);
00173                                 if( (last_dist_down < best_dist) || (j1==-1) )
00174                                                 j1 = down, best_dist = last_dist_down;
00175 
00176                                 if (down < start_cell) {
00177                                         double delta_theta = (p_i_w_phi - laser_ref->theta[down]);
00178                                         double min_dist_down = p_i_w_nrm2 * 
00179                                                 ((delta_theta > M_PI*0.5) ? 1 : mysin(delta_theta));
00180                                         if( SQUARE(min_dist_down) > best_dist) { 
00181                                                 down_stopped = 1; continue;
00182                                         }
00183                                         down += (laser_ref->readings[down] < p_i_w_nrm2) ?
00184                                                 laser_ref->down_bigger[down] : laser_ref->down_smaller[down];
00185                                 } else --down;
00186                         }
00187                         
00188                 }
00189                 
00190                 DEBUG_SEARCH(printf("i=%d j1=%d dist=%f\n",i,j1,best_dist));
00191                 
00192                 /* If no point matched. */
00193                 if( (-1==j1) || (best_dist > max_correspondence_dist2) ) {
00194                         ld_set_null_correspondence(laser_sens, i);
00195                         continue;
00196                 }
00197                 /* We ignore matching the first or the last point in the scan */
00198                 if( 0==j1 || j1 == (laser_ref->nrays-1)) {/* no match */
00199                         ld_set_null_correspondence(laser_sens, i);
00200                         continue;
00201                 }
00202 
00203                 /* Now we want to find j2, the second best match. */
00204                 int j2;
00205                 /* We find the next valid point, up and down */
00206                 int j2up   = ld_next_valid_up   (laser_ref, j1);
00207                 int j2down = ld_next_valid_down (laser_ref, j1);
00208                 /* And then (very boring) we use the nearest */
00209                 if((j2up==-1)&&(j2down==-1)) {
00210                         ld_set_null_correspondence(laser_sens, i);
00211                         continue;
00212                 }
00213                 if(j2up  ==-1) { j2 = j2down; } else
00214                 if(j2down==-1) { j2 = j2up; } else {
00215                         double dist_up   = local_distance_squared_d(p_i_w, laser_ref->points[j2up  ].p);
00216                         double dist_down = local_distance_squared_d(p_i_w, laser_ref->points[j2down].p);
00217                         j2 = dist_up < dist_down ? j2up : j2down;
00218                 }
00219 
00220                 last_best = j1;
00221                 
00222                 laser_sens->corr[i].valid = 1;
00223                 laser_sens->corr[i].j1 = j1;
00224                 laser_sens->corr[i].j2 = j2;
00225                 laser_sens->corr[i].dist2_j1 = best_dist;
00226                 laser_sens->corr[i].type = 
00227                         params->use_point_to_line_distance ? corr_pl : corr_pp;
00228                 
00229         }
00230 }
00231 
00232 
00233 
00234 
00235 
00236 


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