30 #include <sys/types.h>
45 max_samples(0), max_obs(0),
105 double beam_skip_distance,
106 double beam_skip_threshold,
107 double beam_skip_error_threshold)
151 double obs_range, obs_bearing;
161 for (j = 0; j <
set->sample_count; j++)
163 sample =
set->samples + j;
171 step = (
data->range_count - 1) / (self->max_beams - 1);
172 for (i = 0; i <
data->range_count; i +=
step)
174 obs_range =
data->ranges[i][0];
175 obs_bearing =
data->ranges[i][1];
179 pose.
v[2] + obs_bearing,
data->range_max);
183 z = obs_range - map_range;
184 pz +=
self->z_hit * exp(-(z * z) / (2 * self->sigma_hit * self->sigma_hit));
188 pz +=
self->z_short *
self->lambda_short * exp(-self->lambda_short*obs_range);
191 if(obs_range ==
data->range_max)
192 pz +=
self->z_max * 1.0;
195 if(obs_range < data->range_max)
196 pz +=
self->z_rand * 1.0/
data->range_max;
209 total_weight += sample->
weight;
212 return(total_weight);
221 double obs_range, obs_bearing;
232 for (j = 0; j <
set->sample_count; j++)
234 sample =
set->samples + j;
243 double z_hit_denom = 2 *
self->sigma_hit *
self->sigma_hit;
244 double z_rand_mult = 1.0/
data->range_max;
246 step = (
data->range_count - 1) / (self->max_beams - 1);
252 for (i = 0; i <
data->range_count; i +=
step)
254 obs_range =
data->ranges[i][0];
255 obs_bearing =
data->ranges[i][1];
258 if(obs_range >=
data->range_max)
262 if(obs_range != obs_range)
268 hit.
v[0] =
pose.
v[0] + obs_range * cos(
pose.
v[2] + obs_bearing);
269 hit.
v[1] =
pose.
v[1] + obs_range * sin(
pose.
v[2] + obs_bearing);
279 z =
self->map->max_occ_dist;
281 z =
self->map->cells[
MAP_INDEX(self->map,mi,mj)].occ_dist;
284 pz +=
self->z_hit * exp(-(z * z) / z_hit_denom);
286 pz +=
self->z_rand * z_rand_mult;
299 total_weight += sample->
weight;
302 return(total_weight);
311 double obs_range, obs_bearing;
321 step = ceil((
data->range_count) /
static_cast<double>(
self->max_beams));
328 double z_hit_denom = 2 *
self->sigma_hit *
self->sigma_hit;
329 double z_rand_mult = 1.0/
data->range_max;
331 double max_dist_prob = exp(-(self->map->max_occ_dist * self->map->max_occ_dist) / z_hit_denom);
347 int *obs_count =
new int[
self->max_beams]();
350 bool *obs_mask =
new bool[
self->max_beams]();
355 bool realloc =
false;
358 if(self->max_obs < self->max_beams){
362 if(self->max_samples <
set->sample_count){
367 self->reallocTempData(
set->sample_count, self->max_beams);
368 fprintf(stderr,
"Reallocing temp weights %d - %d\n", self->max_samples, self->max_obs);
373 for (j = 0; j <
set->sample_count; j++)
375 sample =
set->samples + j;
385 for (i = 0; i <
data->range_count; i +=
step, beam_ind++)
387 obs_range =
data->ranges[i][0];
388 obs_bearing =
data->ranges[i][1];
391 if(obs_range >=
data->range_max){
396 if(obs_range != obs_range){
403 hit.
v[0] =
pose.
v[0] + obs_range * cos(
pose.
v[2] + obs_bearing);
404 hit.
v[1] =
pose.
v[1] + obs_range * sin(
pose.
v[2] + obs_bearing);
415 pz +=
self->z_hit * max_dist_prob;
418 z =
self->map->cells[
MAP_INDEX(self->map,mi,mj)].occ_dist;
420 obs_count[beam_ind] += 1;
422 pz +=
self->z_hit * exp(-(z * z) / z_hit_denom);
429 pz +=
self->z_rand * z_rand_mult;
440 self->temp_obs[j][beam_ind] = pz;
444 sample->
weight *= exp(log_p);
445 total_weight += sample->
weight;
450 int skipped_beam_count = 0;
451 for (beam_ind = 0; beam_ind <
self->max_beams; beam_ind++){
453 obs_mask[beam_ind] =
true;
456 obs_mask[beam_ind] =
false;
457 skipped_beam_count++;
467 if(skipped_beam_count >= (beam_ind * self->beam_skip_error_threshold)){
468 fprintf(stderr,
"Over %f%% of the observations were not in the map - pf may have converged to wrong pose - integrating all observations\n", (100 * self->beam_skip_error_threshold));
472 for (j = 0; j <
set->sample_count; j++)
474 sample =
set->samples + j;
479 for (beam_ind = 0; beam_ind <
self->max_beams; beam_ind++){
480 if(error || obs_mask[beam_ind]){
481 log_p += log(self->temp_obs[j][beam_ind]);
485 sample->
weight *= exp(log_p);
487 total_weight += sample->
weight;
493 return(total_weight);