34 #include <sys/types.h> 37 #ifdef USE_ASSERT_EXCEPTION 38 # include <core/assert_exception.h> 44 #include "amcl_laser.h" 52 AMCLLaser::AMCLLaser(
size_t max_beams, map_t* map) : AMCLSensor()
56 this->max_beams = max_beams;
63 AMCLLaser::SetModelBeam(
double z_hit,
71 this->model_type = LASER_MODEL_BEAM;
73 this->z_short = z_short;
75 this->z_rand = z_rand;
76 this->sigma_hit = sigma_hit;
77 this->lambda_short = lambda_short;
78 this->chi_outlier = chi_outlier;
82 AMCLLaser::SetModelLikelihoodField(
double z_hit,
87 this->model_type = LASER_MODEL_LIKELIHOOD_FIELD;
89 this->z_rand = z_rand;
90 this->sigma_hit = sigma_hit;
92 map_update_cspace(this->map, max_occ_dist);
98 bool AMCLLaser::UpdateSensor(pf_t *pf, AMCLSensorData *data)
103 if (this->max_beams < 2)
107 if(this->model_type == LASER_MODEL_BEAM)
108 pf_update_sensor(pf, (pf_sensor_model_fn_t) BeamModel, data);
109 else if(this->model_type == LASER_MODEL_LIKELIHOOD_FIELD)
110 pf_update_sensor(pf, (pf_sensor_model_fn_t) LikelihoodFieldModel, data);
112 pf_update_sensor(pf, (pf_sensor_model_fn_t) BeamModel, data);
120 double AMCLLaser::BeamModel(AMCLLaserData *data, pf_sample_set_t*
set)
127 double obs_range, obs_bearing;
132 self = (AMCLLaser*) data->sensor;
137 for (j = 0; j <
set->sample_count; j++)
139 sample =
set->samples + j;
143 pose = pf_vector_coord_add(self->laser_pose, pose);
147 step = (data->range_count - 1) / (self->max_beams - 1);
148 for (i = 0; i < data->range_count; i += step)
150 obs_range = data->ranges[i][0];
151 obs_bearing = data->ranges[i][1];
154 map_range = map_calc_range(self->map, pose.v[0], pose.v[1],
155 pose.v[2] + obs_bearing, data->range_max);
159 z = obs_range - map_range;
160 pz +=
self->z_hit * exp(-(z * z) / (2 * self->sigma_hit * self->sigma_hit));
164 pz +=
self->z_short *
self->lambda_short * exp(-self->lambda_short*obs_range);
167 if(obs_range == data->range_max)
168 pz +=
self->z_max * 1.0;
171 if(obs_range < data->range_max)
172 pz +=
self->z_rand * 1.0/data->range_max;
178 if ( (pz < 0.) || (pz > 1.) ) pz = 0.;
187 total_weight += sample->weight;
190 return(total_weight);
193 double AMCLLaser::LikelihoodFieldModel(AMCLLaserData *data, pf_sample_set_t*
set)
199 double obs_range, obs_bearing;
205 self = (AMCLLaser*) data->sensor;
210 for (j = 0; j <
set->sample_count; j++)
212 sample =
set->samples + j;
216 pose = pf_vector_coord_add(self->laser_pose, pose);
221 double z_hit_denom = 2 *
self->sigma_hit *
self->sigma_hit;
222 double z_rand_mult = 1.0/data->range_max;
224 step = (data->range_count - 1) / (self->max_beams - 1);
225 for (i = 0; i < data->range_count; i += step)
227 obs_range = data->ranges[i][0];
228 obs_bearing = data->ranges[i][1];
231 if(obs_range >= data->range_max)
237 hit.v[0] = pose.v[0] + obs_range * cos(pose.v[2] + obs_bearing);
238 hit.v[1] = pose.v[1] + obs_range * sin(pose.v[2] + obs_bearing);
242 mi = MAP_GXWX(self->map, hit.v[0]);
243 mj = MAP_GYWY(self->map, hit.v[1]);
247 if(!MAP_VALID(self->map, mi, mj))
248 z =
self->map->max_occ_dist;
250 z =
self->map->cells[MAP_INDEX(self->map,mi,mj)].occ_dist;
253 pz +=
self->z_hit * exp(-(z * z) / z_hit_denom);
255 pz +=
self->z_rand * z_rand_mult;
261 if ( (pz < 0.) || (pz > 1.) ) pz = 0.;
270 total_weight += sample->weight;
273 return(total_weight);