icp_corr_tricks.c
Go to the documentation of this file.
1 #include <gsl/gsl_vector.h>
2 
3 #include "icp.h"
4 #include "../csm_all.h"
5 
8 INLINE double mysin(double x) {
9  const double a = -1.0/6.0;
10  const double b = +1.0/120.0;
11  double x2 = x*x;
12  return x * (.99 + x2 * ( a + b * x2));
13 }
14 
15 
16 #define DEBUG_SEARCH(a) ;
17 
18 void ld_create_jump_tables(struct laser_data* ld) {
19  int i;
20  for(i=0;i<ld->nrays;i++) {
21  int j;
22 
23  j = i + 1;
24  while(j<ld->nrays && ld->valid[j] && ld->readings[j]<=ld->readings[i]) j++;
25  ld->up_bigger[i] = j-i;
26 
27  j = i + 1;
28  while(j<ld->nrays && ld->valid[j] && ld->readings[j]>=ld->readings[i]) j++;
29  ld->up_smaller[i] = j-i;
30 
31  j = i - 1;
32  while(j>=0 && ld->valid[j] && ld->readings[j]>=ld->readings[i]) j--;
33  ld->down_smaller[i] = j-i;
34 
35  j = i - 1;
36  while(j>=0 && ld->valid[j] && ld->readings[j]<=ld->readings[i]) j--;
37  ld->down_bigger[i] = j-i;
38  }
39 }
40 
41 extern int distance_counter;
42 
43 INLINE double local_distance_squared_d(const double* a, const double* b) {
44  distance_counter++;
45  double x = a[0] - b[0];
46  double y = a[1] - b[1];
47  return x*x + y*y;
48 }
49 
50 #include "fast_math.h"
51 
52 
53 #define SQUARE(a) ((a)*(a))
54 
55 /* Assumes that points_w is filled. */
56 void find_correspondences_tricks(struct sm_params*params) {
57  const LDP laser_ref = params->laser_ref;
58  const LDP laser_sens = params->laser_sens;
59  int i;
60 
61  /* Handy constant */
62  double C1 = (double)laser_ref->nrays / (laser_ref->max_theta-laser_ref->min_theta) ;
63  double max_correspondence_dist2 = square(params->max_correspondence_dist);
64  /* Last match */
65  int last_best = -1;
66  const point2d * restrict points_w = laser_sens->points_w;
67  for(i=0;i<laser_sens->nrays;i++) {
68  if(!ld_valid_ray(laser_sens,i)) {
69  ld_set_null_correspondence(laser_sens,i);
70  continue;
71  }
72 
73  const double *p_i_w = points_w[i].p;
74  double p_i_w_nrm2 = points_w[i].rho;
75  double p_i_w_phi = points_w[i].phi;
76 
78  int from = 0;
79  int to = laser_ref->nrays-1;
80 
81  int start_cell = (int) ((p_i_w_phi - laser_ref->min_theta) * C1);
82 
84  int j1 = -1;
86  double best_dist = 42;
87 
89  int we_start_at;
90  if (last_best==-1)
91  we_start_at = start_cell;
92  else
93  we_start_at = last_best + 1;
94 
95  if(we_start_at > to) we_start_at = to;
96  if(we_start_at < from) we_start_at = from;
97 
98  int up = we_start_at+1;
99  int down = we_start_at;
100  double last_dist_up = 0; /* first is down */
101  double last_dist_down = -1;
102 
103  int up_stopped = 0;
104  int down_stopped = 0;
105 
106  DEBUG_SEARCH(printf("i=%d p_i_w = %f %f\n",i, p_i_w[0], p_i_w,[1]));
107  DEBUG_SEARCH(printf("i=%d [from %d down %d mid %d up %d to %d]\n",
108  i,from,down,start_cell,up,to));
109 
110  while ( (!up_stopped) || (!down_stopped) ) {
111  int now_up = up_stopped ? 0 :
112  down_stopped ? 1 : last_dist_up < last_dist_down;
113  DEBUG_SEARCH(printf("|"));
114 
115  /* Now two symmetric chunks of code, the now_up and the !now_up */
116  if(now_up) {
117  DEBUG_SEARCH(printf("up %d ",up));
118  /* If we have crossed the "to" boundary we stop searching
119  on the "up" direction. */
120  if(up > to) {
121  up_stopped = 1; continue; }
122  /* Just ignore invalid rays */
123  if(!laser_ref->valid[up]) {
124  ++up; continue; }
125 
126  /* This is the distance from p_i_w to the "up" point*/
127  last_dist_up = local_distance_squared_d(p_i_w, laser_ref->points[up].p);
128 
129  /* If it is less than the best point, it is our new j1 */
130  if((last_dist_up < best_dist) || (j1==-1) )
131  j1 = up, best_dist = last_dist_up;
132 
133  /* If we are moving away from start_cell */
134  if (up > start_cell) {
135  double delta_theta = (laser_ref->theta[up] - p_i_w_phi);
136  /* We can compute a bound for early stopping. Currently
137  our best point has distance best_dist; we can compute
138  min_dist_up, which is the minimum distance that can have
139  points for j>up (see figure)*/
140  double min_dist_up = p_i_w_nrm2 *
141  ((delta_theta > M_PI*0.5) ? 1 : mysin(delta_theta));
142  /* If going up we can't make better than best_dist, then
143  we stop searching in the "up" direction */
144  if(SQUARE(min_dist_up) > best_dist) {
145  up_stopped = 1; continue;
146  }
147  /* If we are moving away, then we can implement the jump tables
148  optimizations. */
149  up +=
150  /* If p_i_w is shorter than "up" */
151  (laser_ref->readings[up] < p_i_w_nrm2)
152  ?
153  /* We can jump to a bigger point */
154  laser_ref->up_bigger[up]
155  /* Or else we jump to a smaller point */
156  : laser_ref->up_smaller[up];
157 
158  } else
159  /* If we are moving towards "start_cell", we can't do any
160  ot the previous optimizations and we just move to the next point */
161  ++up;
162  }
163 
164  /* This is the specular part of the previous chunk of code. */
165  if(!now_up) {
166  DEBUG_SEARCH(printf("down %d ",down));
167  if(down < from) {
168  down_stopped = 1; continue; }
169  if(!laser_ref->valid[down]) {
170  --down; continue; }
171 
172  last_dist_down = local_distance_squared_d(p_i_w, laser_ref->points[down].p);
173  if( (last_dist_down < best_dist) || (j1==-1) )
174  j1 = down, best_dist = last_dist_down;
175 
176  if (down < start_cell) {
177  double delta_theta = (p_i_w_phi - laser_ref->theta[down]);
178  double min_dist_down = p_i_w_nrm2 *
179  ((delta_theta > M_PI*0.5) ? 1 : mysin(delta_theta));
180  if( SQUARE(min_dist_down) > best_dist) {
181  down_stopped = 1; continue;
182  }
183  down += (laser_ref->readings[down] < p_i_w_nrm2) ?
184  laser_ref->down_bigger[down] : laser_ref->down_smaller[down];
185  } else --down;
186  }
187 
188  }
189 
190  DEBUG_SEARCH(printf("i=%d j1=%d dist=%f\n",i,j1,best_dist));
191 
192  /* If no point matched. */
193  if( (-1==j1) || (best_dist > max_correspondence_dist2) ) {
194  ld_set_null_correspondence(laser_sens, i);
195  continue;
196  }
197  /* We ignore matching the first or the last point in the scan */
198  if( 0==j1 || j1 == (laser_ref->nrays-1)) {/* no match */
199  ld_set_null_correspondence(laser_sens, i);
200  continue;
201  }
202 
203  /* Now we want to find j2, the second best match. */
204  int j2;
205  /* We find the next valid point, up and down */
206  int j2up = ld_next_valid_up (laser_ref, j1);
207  int j2down = ld_next_valid_down (laser_ref, j1);
208  /* And then (very boring) we use the nearest */
209  if((j2up==-1)&&(j2down==-1)) {
210  ld_set_null_correspondence(laser_sens, i);
211  continue;
212  }
213  if(j2up ==-1) { j2 = j2down; } else
214  if(j2down==-1) { j2 = j2up; } else {
215  double dist_up = local_distance_squared_d(p_i_w, laser_ref->points[j2up ].p);
216  double dist_down = local_distance_squared_d(p_i_w, laser_ref->points[j2down].p);
217  j2 = dist_up < dist_down ? j2up : j2down;
218  }
219 
220  last_best = j1;
221 
222  laser_sens->corr[i].valid = 1;
223  laser_sens->corr[i].j1 = j1;
224  laser_sens->corr[i].j2 = j2;
225  laser_sens->corr[i].dist2_j1 = best_dist;
226  laser_sens->corr[i].type =
227  params->use_point_to_line_distance ? corr_pl : corr_pp;
228 
229  }
230 }
231 
232 
233 
234 
235 
236 
correspondence::dist2_j1
double dist2_j1
Definition: laser_data.h:71
laser_data::valid
int *restrict valid
Definition: laser_data.h:23
ld_next_valid_up
INLINE int ld_next_valid_up(LDP ld, int i)
Definition: laser_data_inline.h:42
laser_data::points
point2d *restrict points
Definition: laser_data.h:44
laser_data::max_theta
double max_theta
Definition: laser_data.h:19
laser_data::min_theta
double min_theta
Definition: laser_data.h:18
distance_counter
int distance_counter
Definition: math_utils.c:46
SQUARE
#define SQUARE(a)
Definition: icp_corr_tricks.c:53
laser_data::theta
double *restrict theta
Definition: laser_data.h:21
ld_create_jump_tables
void ld_create_jump_tables(struct laser_data *ld)
Definition: icp_corr_tricks.c:18
ld_valid_ray
INLINE int ld_valid_ray(LDP ld, int i)
Definition: laser_data_inline.h:13
laser_data::up_bigger
int *restrict up_bigger
Definition: laser_data.h:55
correspondence::type
enum correspondence::@5 type
M_PI
#define M_PI
Definition: math_utils.h:7
laser_data::up_smaller
int *restrict *restrict up_smaller
Definition: laser_data.h:55
laser_data::readings
double *restrict readings
Definition: laser_data.h:24
sm_params::max_correspondence_dist
double max_correspondence_dist
Definition: algos.h:34
laser_data::nrays
int nrays
Definition: laser_data.h:17
INLINE
#define INLINE
Definition: restrict.h:5
restrict
#define restrict
Definition: restrict.h:17
sm_params::laser_ref
LDP laser_ref
Definition: algos.h:14
laser_data::points_w
point2d *restrict points_w
Definition: laser_data.h:47
laser_data::down_smaller
int *restrict *restrict *restrict *restrict down_smaller
Definition: laser_data.h:55
sm_params::laser_sens
LDP laser_sens
Definition: algos.h:16
mysin
INLINE double mysin(double x)
Definition: icp_corr_tricks.c:8
find_correspondences_tricks
void find_correspondences_tricks(struct sm_params *params)
Definition: icp_corr_tricks.c:56
DEBUG_SEARCH
#define DEBUG_SEARCH(a)
Definition: icp_corr_tricks.c:16
laser_data::corr
struct correspondence *restrict corr
Definition: laser_data.h:36
ld_set_null_correspondence
INLINE void ld_set_null_correspondence(LDP ld, int i)
Definition: laser_data_inline.h:21
ld_next_valid_down
INLINE int ld_next_valid_down(LDP ld, int i)
Definition: laser_data_inline.h:46
local_distance_squared_d
INLINE double local_distance_squared_d(const double *a, const double *b)
Definition: icp_corr_tricks.c:43
fast_math.h
laser_data
Definition: laser_data.h:16
sm_params
Definition: algos.h:12
laser_data::down_bigger
int *restrict *restrict *restrict down_bigger
Definition: laser_data.h:55
square
double square(double x)
Definition: math_utils.c:124
sm_params::use_point_to_line_distance
int use_point_to_line_distance
Definition: algos.h:99
point2d::p
double p[2]
Definition: laser_data.h:12

csm
Author(s): Andrea Censi
autogenerated on Tue May 11 2021 02:18:23