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 
47 using namespace amcl;
48 
49 /// @cond EXTERNAL
50 
51 ////////////////////////////////////////////////////////////////////////////////
52 // Default constructor
53 AMCLLaser::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 
72 void
73 AMCLLaser::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 
91 void
92 AMCLLaser::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
107 bool
108 AMCLLaser::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
129 double
130 AMCLLaser::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 
191 double
192 AMCLLaser::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