38#ifdef USE_ASSERT_EXCEPTION
39# include <core/assert_exception.h>
43#include "amcl_laser.h"
53AMCLLaser::AMCLLaser(
size_t max_beams, map_t *map) : AMCLSensor()
57 this->max_beams = max_beams;
60 this->model_type = LASER_MODEL_BEAM;
66 this->lambda_short = .1;
67 this->chi_outlier = 0.0;
73AMCLLaser::SetModelBeam(
double z_hit,
81 this->model_type = LASER_MODEL_BEAM;
83 this->z_short = z_short;
85 this->z_rand = z_rand;
86 this->sigma_hit = sigma_hit;
87 this->lambda_short = lambda_short;
88 this->chi_outlier = chi_outlier;
92AMCLLaser::SetModelLikelihoodField(
double z_hit,
97 this->model_type = LASER_MODEL_LIKELIHOOD_FIELD;
99 this->z_rand = z_rand;
100 this->sigma_hit = sigma_hit;
102 map_update_cspace(this->map, max_occ_dist);
108AMCLLaser::UpdateSensor(pf_t *pf, AMCLSensorData *data)
113 if (this->max_beams < 2)
117 if (this->model_type == LASER_MODEL_BEAM)
118 pf_update_sensor(pf, (pf_sensor_model_fn_t)BeamModel, data);
119 else if (this->model_type == LASER_MODEL_LIKELIHOOD_FIELD)
120 pf_update_sensor(pf, (pf_sensor_model_fn_t)LikelihoodFieldModel, data);
122 pf_update_sensor(pf, (pf_sensor_model_fn_t)BeamModel, data);
130AMCLLaser::BeamModel(AMCLLaserData *data, pf_sample_set_t *set)
132 AMCLLaser *self =
static_cast<AMCLLaser *
>(data->sensor);
133 double total_weight = 0.0;
136 for (
int j = 0; j < set->sample_count; j++) {
137 pf_sample_t *sample = set->samples + j;
138 pf_vector_t pose{sample->pose};
141 pose = pf_vector_coord_add(self->laser_pose, pose);
145 int step = (data->range_count - 1) / (self->max_beams - 1);
146 for (
int i = 0; i < data->range_count; i += step) {
147 double obs_range = data->ranges[i][0];
148 double obs_bearing = data->ranges[i][1];
152 map_calc_range(self->map, pose.v[0], pose.v[1], pose.v[2] + obs_bearing, data->range_max);
156 double z = obs_range - map_range;
157 pz += self->z_hit * exp(-(z * z) / (2 * self->sigma_hit * self->sigma_hit));
161 pz += self->z_short * self->lambda_short * exp(-self->lambda_short * obs_range);
164 if (obs_range == data->range_max)
165 pz += self->z_max * 1.0;
168 if (obs_range < data->range_max)
169 pz += self->z_rand * 1.0 / data->range_max;
175 if ((pz < 0.) || (pz > 1.))
185 total_weight += sample->weight;
188 return (total_weight);
192AMCLLaser::LikelihoodFieldModel(AMCLLaserData *data, pf_sample_set_t *set)
197 double obs_range, obs_bearing;
203 self = (AMCLLaser *)data->sensor;
208 for (j = 0; j < set->sample_count; j++) {
209 sample = set->samples + j;
213 pose = pf_vector_coord_add(self->laser_pose, pose);
218 double z_hit_denom = 2 * self->sigma_hit * self->sigma_hit;
219 double z_rand_mult = 1.0 / data->range_max;
221 int step = (data->range_count - 1) / (self->max_beams - 1);
222 for (i = 0; i < data->range_count; i += step) {
223 obs_range = data->ranges[i][0];
224 obs_bearing = data->ranges[i][1];
227 if (obs_range >= data->range_max)
233 hit.v[0] = pose.v[0] + obs_range * cos(pose.v[2] + obs_bearing);
234 hit.v[1] = pose.v[1] + obs_range * sin(pose.v[2] + obs_bearing);
238 mi = MAP_GXWX(self->map, hit.v[0]);
239 mj = MAP_GYWY(self->map, hit.v[1]);
243 if (!MAP_VALID(self->map, mi, mj))
244 z = self->map->max_occ_dist;
246 z = self->map->cells[MAP_INDEX(self->map, mi, mj)].occ_dist;
249 pz += self->z_hit * exp(-(z * z) / z_hit_denom);
251 pz += self->z_rand * z_rand_mult;
257 if ((pz < 0.) || (pz > 1.))
267 total_weight += sample->weight;
270 return (total_weight);