Fawkes API Fawkes Development Version
amcl_laser.cpp
1/***************************************************************************
2 * amcl_laser.cpp: AMCL laser routines
3 *
4 * Created: Thu May 24 18:50:35 2012
5 * Copyright 2000 Brian Gerkey
6 * 2000 Kasper Stoy
7 * 2012 Tim Niemueller [www.niemueller.de]
8 ****************************************************************************/
9
10/* This program is free software; you can redistribute it and/or modify
11 * it under the terms of the GNU General Public License as published by
12 * the Free Software Foundation; either version 2 of the License, or
13 * (at your option) any later version.
14 *
15 * This program is distributed in the hope that it will be useful,
16 * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 * GNU Library General Public License for more details.
19 *
20 * Read the full text in the LICENSE.GPL file in the doc directory.
21 */
22
23/* From:
24 * Player - One Hell of a Robot Server (LGPL)
25 * Copyright (C) 2000 Brian Gerkey & Kasper Stoy
26 * gerkey@usc.edu kaspers@robotics.usc.edu
27 */
28///////////////////////////////////////////////////////////////////////////
29// Desc: AMCL laser routines
30// Author: Andrew Howard
31// Date: 6 Feb 2003
32///////////////////////////////////////////////////////////////////////////
33
34#include <sys/types.h> // required by Darwin
35
36#include <math.h>
37#include <stdlib.h>
38#ifdef USE_ASSERT_EXCEPTION
39# include <core/assert_exception.h>
40#else
41# include <assert.h>
42#endif
43#include "amcl_laser.h"
44
45#include <unistd.h>
46
47using namespace amcl;
48
49/// @cond EXTERNAL
50
51////////////////////////////////////////////////////////////////////////////////
52// Default constructor
53AMCLLaser::AMCLLaser(size_t max_beams, map_t *map) : AMCLSensor()
54{
55 this->time = 0.0;
56
57 this->max_beams = max_beams;
58 this->map = map;
59
60 this->model_type = LASER_MODEL_BEAM;
61 this->z_hit = .95;
62 this->z_short = .05;
63 this->z_max = .05;
64 this->z_rand = .05;
65 this->sigma_hit = .2;
66 this->lambda_short = .1;
67 this->chi_outlier = 0.0;
68
69 return;
70}
71
72void
73AMCLLaser::SetModelBeam(double z_hit,
74 double z_short,
75 double z_max,
76 double z_rand,
77 double sigma_hit,
78 double lambda_short,
79 double chi_outlier)
80{
81 this->model_type = LASER_MODEL_BEAM;
82 this->z_hit = z_hit;
83 this->z_short = z_short;
84 this->z_max = z_max;
85 this->z_rand = z_rand;
86 this->sigma_hit = sigma_hit;
87 this->lambda_short = lambda_short;
88 this->chi_outlier = chi_outlier;
89}
90
91void
92AMCLLaser::SetModelLikelihoodField(double z_hit,
93 double z_rand,
94 double sigma_hit,
95 double max_occ_dist)
96{
97 this->model_type = LASER_MODEL_LIKELIHOOD_FIELD;
98 this->z_hit = z_hit;
99 this->z_rand = z_rand;
100 this->sigma_hit = sigma_hit;
101
102 map_update_cspace(this->map, max_occ_dist);
103}
104
105////////////////////////////////////////////////////////////////////////////////
106// Apply the laser sensor model
107bool
108AMCLLaser::UpdateSensor(pf_t *pf, AMCLSensorData *data)
109{
110 //AMCLLaserData *ndata;
111
112 //ndata = (AMCLLaserData*) data;
113 if (this->max_beams < 2)
114 return false;
115
116 // Apply the laser sensor model
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);
121 else
122 pf_update_sensor(pf, (pf_sensor_model_fn_t)BeamModel, data);
123
124 return true;
125}
126
127////////////////////////////////////////////////////////////////////////////////
128// Determine the probability for the given pose
129double
130AMCLLaser::BeamModel(AMCLLaserData *data, pf_sample_set_t *set)
131{
132 AMCLLaser *self = static_cast<AMCLLaser *>(data->sensor);
133 double total_weight = 0.0;
134
135 // Compute the sample weights
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};
139
140 // Take account of the laser pose relative to the robot
141 pose = pf_vector_coord_add(self->laser_pose, pose);
142
143 double p = 1.0;
144
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];
149
150 // Compute the range according to the map
151 double map_range =
152 map_calc_range(self->map, pose.v[0], pose.v[1], pose.v[2] + obs_bearing, data->range_max);
153 double pz = 0.0;
154
155 // Part 1: good, but noisy, hit
156 double z = obs_range - map_range;
157 pz += self->z_hit * exp(-(z * z) / (2 * self->sigma_hit * self->sigma_hit));
158
159 // Part 2: short reading from unexpected obstacle (e.g., a person)
160 if (z < 0)
161 pz += self->z_short * self->lambda_short * exp(-self->lambda_short * obs_range);
162
163 // Part 3: Failure to detect obstacle, reported as max-range
164 if (obs_range == data->range_max)
165 pz += self->z_max * 1.0;
166
167 // Part 4: Random measurements
168 if (obs_range < data->range_max)
169 pz += self->z_rand * 1.0 / data->range_max;
170
171 // TODO: outlier rejection for short readings
172
173 //assert(pz <= 1.0);
174 //assert(pz >= 0.0);
175 if ((pz < 0.) || (pz > 1.))
176 pz = 0.;
177
178 // p *= pz;
179 // here we have an ad-hoc weighting scheme for combining beam probs
180 // works well, though...
181 p += pz * pz * pz;
182 }
183
184 sample->weight *= p;
185 total_weight += sample->weight;
186 }
187
188 return (total_weight);
189}
190
191double
192AMCLLaser::LikelihoodFieldModel(AMCLLaserData *data, pf_sample_set_t *set)
193{
194 AMCLLaser * self;
195 int i, j;
196 double z, pz;
197 double obs_range, obs_bearing;
198 double total_weight;
199 pf_sample_t *sample;
200 pf_vector_t pose;
201 pf_vector_t hit;
202
203 self = (AMCLLaser *)data->sensor;
204
205 total_weight = 0.0;
206
207 // Compute the sample weights
208 for (j = 0; j < set->sample_count; j++) {
209 sample = set->samples + j;
210 pose = sample->pose;
211
212 // Take account of the laser pose relative to the robot
213 pose = pf_vector_coord_add(self->laser_pose, pose);
214
215 double p = 1.0;
216
217 // Pre-compute a couple of things
218 double z_hit_denom = 2 * self->sigma_hit * self->sigma_hit;
219 double z_rand_mult = 1.0 / data->range_max;
220
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];
225
226 // This model ignores max range readings
227 if (obs_range >= data->range_max)
228 continue;
229
230 pz = 0.0;
231
232 // Compute the endpoint of the beam
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);
235
236 // Convert to map grid coords.
237 int mi, mj;
238 mi = MAP_GXWX(self->map, hit.v[0]);
239 mj = MAP_GYWY(self->map, hit.v[1]);
240
241 // Part 1: Get distance from the hit to closest obstacle.
242 // Off-map penalized as max distance
243 if (!MAP_VALID(self->map, mi, mj))
244 z = self->map->max_occ_dist;
245 else
246 z = self->map->cells[MAP_INDEX(self->map, mi, mj)].occ_dist;
247 // Gaussian model
248 // NOTE: this should have a normalization of 1/(sqrt(2pi)*sigma)
249 pz += self->z_hit * exp(-(z * z) / z_hit_denom);
250 // Part 2: random measurements
251 pz += self->z_rand * z_rand_mult;
252
253 // TODO: outlier rejection for short readings
254
255 //assert(pz <= 1.0);
256 //assert(pz >= 0.0);
257 if ((pz < 0.) || (pz > 1.))
258 pz = 0.;
259
260 // p *= pz;
261 // here we have an ad-hoc weighting scheme for combining beam probs
262 // works well, though...
263 p += pz * pz * pz;
264 }
265
266 sample->weight *= p;
267 total_weight += sample->weight;
268 }
269
270 return (total_weight);
271}
272
273/// @endcond