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;
90 this->z_rand = z_rand;
91 this->sigma_hit = sigma_hit;
93 map_update_cspace(this->map, max_occ_dist);
99 bool AMCLLaser::UpdateSensor(pf_t *pf, AMCLSensorData *data)
104 if (this->max_beams < 2)
108 if(this->model_type == LASER_MODEL_BEAM)
109 pf_update_sensor(pf, (pf_sensor_model_fn_t) BeamModel, data);
110 else if(this->model_type == LASER_MODEL_LIKELIHOOD_FIELD)
111 pf_update_sensor(pf, (pf_sensor_model_fn_t) LikelihoodFieldModel, data);
113 pf_update_sensor(pf, (pf_sensor_model_fn_t) BeamModel, data);
121 double AMCLLaser::BeamModel(AMCLLaserData *data, pf_sample_set_t*
set)
128 double obs_range, obs_bearing;
133 self = (AMCLLaser*) data->sensor;
138 for (j = 0; j <
set->sample_count; j++)
140 sample =
set->samples + j;
144 pose = pf_vector_coord_add(self->laser_pose, pose);
148 step = (data->range_count - 1) / (self->max_beams - 1);
149 for (i = 0; i < data->range_count; i += step)
151 obs_range = data->ranges[i][0];
152 obs_bearing = data->ranges[i][1];
155 map_range = map_calc_range(self->map, pose.v[0], pose.v[1],
156 pose.v[2] + obs_bearing, data->range_max);
160 z = obs_range - map_range;
161 pz +=
self->z_hit * exp(-(z * z) / (2 * self->sigma_hit * self->sigma_hit));
165 pz +=
self->z_short *
self->lambda_short * exp(-self->lambda_short*obs_range);
168 if(obs_range == data->range_max)
169 pz +=
self->z_max * 1.0;
172 if(obs_range < data->range_max)
173 pz +=
self->z_rand * 1.0/data->range_max;
179 if ( (pz < 0.) || (pz > 1.) ) pz = 0.;
188 total_weight += sample->weight;
191 return(total_weight);
194 double AMCLLaser::LikelihoodFieldModel(AMCLLaserData *data, pf_sample_set_t*
set)
200 double obs_range, obs_bearing;
206 self = (AMCLLaser*) data->sensor;
211 for (j = 0; j <
set->sample_count; j++)
213 sample =
set->samples + j;
217 pose = pf_vector_coord_add(self->laser_pose, pose);
222 double z_hit_denom = 2 *
self->sigma_hit *
self->sigma_hit;
223 double z_rand_mult = 1.0/data->range_max;
225 step = (data->range_count - 1) / (self->max_beams - 1);
226 for (i = 0; i < data->range_count; i += step)
228 obs_range = data->ranges[i][0];
229 obs_bearing = data->ranges[i][1];
232 if(obs_range >= data->range_max)
238 hit.v[0] = pose.v[0] + obs_range * cos(pose.v[2] + obs_bearing);
239 hit.v[1] = pose.v[1] + obs_range * sin(pose.v[2] + obs_bearing);
243 mi = MAP_GXWX(self->map, hit.v[0]);
244 mj = MAP_GYWY(self->map, hit.v[1]);
248 if(!MAP_VALID(self->map, mi, mj))
249 z =
self->map->max_occ_dist;
251 z =
self->map->cells[MAP_INDEX(self->map,mi,mj)].occ_dist;
254 pz +=
self->z_hit * exp(-(z * z) / z_hit_denom);
256 pz +=
self->z_rand * z_rand_mult;
262 if ( (pz < 0.) || (pz > 1.) ) pz = 0.;
271 total_weight += sample->weight;
274 return(total_weight);