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 #include <math.h>
36 #include <stdlib.h>
37 #ifdef USE_ASSERT_EXCEPTION
38 # include <core/assert_exception.h>
39 #else
40 # include <assert.h>
41 #endif
42 #include <unistd.h>
43 
44 #include "amcl_laser.h"
45 
46 using namespace amcl;
47 
48 /// @cond EXTERNAL
49 
50 ////////////////////////////////////////////////////////////////////////////////
51 // Default constructor
52 AMCLLaser::AMCLLaser(size_t max_beams, map_t* map) : AMCLSensor()
53 {
54  this->time = 0.0;
55 
56  this->max_beams = max_beams;
57  this->map = map;
58 
59  return;
60 }
61 
62 void
63 AMCLLaser::SetModelBeam(double z_hit,
64  double z_short,
65  double z_max,
66  double z_rand,
67  double sigma_hit,
68  double lambda_short,
69  double chi_outlier)
70 {
71  this->model_type = LASER_MODEL_BEAM;
72  this->z_hit = z_hit;
73  this->z_short = z_short;
74  this->z_max = z_max;
75  this->z_rand = z_rand;
76  this->sigma_hit = sigma_hit;
77  this->lambda_short = lambda_short;
78  this->chi_outlier = chi_outlier;
79 }
80 
81 void
82 AMCLLaser::SetModelLikelihoodField(double z_hit,
83  double z_rand,
84  double sigma_hit,
85  double max_occ_dist)
86 {
87  this->model_type = LASER_MODEL_LIKELIHOOD_FIELD;
88  this->z_hit = z_hit;
89  this->z_rand = z_rand;
90  this->sigma_hit = sigma_hit;
91 
92  map_update_cspace(this->map, max_occ_dist);
93 }
94 
95 
96 ////////////////////////////////////////////////////////////////////////////////
97 // Apply the laser sensor model
98 bool AMCLLaser::UpdateSensor(pf_t *pf, AMCLSensorData *data)
99 {
100  //AMCLLaserData *ndata;
101 
102  //ndata = (AMCLLaserData*) data;
103  if (this->max_beams < 2)
104  return false;
105 
106  // Apply the laser sensor model
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);
111  else
112  pf_update_sensor(pf, (pf_sensor_model_fn_t) BeamModel, data);
113 
114  return true;
115 }
116 
117 
118 ////////////////////////////////////////////////////////////////////////////////
119 // Determine the probability for the given pose
120 double AMCLLaser::BeamModel(AMCLLaserData *data, pf_sample_set_t* set)
121 {
122  AMCLLaser *self;
123  int i, j, step;
124  double z, pz;
125  double p;
126  double map_range;
127  double obs_range, obs_bearing;
128  double total_weight;
129  pf_sample_t *sample;
130  pf_vector_t pose;
131 
132  self = (AMCLLaser*) data->sensor;
133 
134  total_weight = 0.0;
135 
136  // Compute the sample weights
137  for (j = 0; j < set->sample_count; j++)
138  {
139  sample = set->samples + j;
140  pose = sample->pose;
141 
142  // Take account of the laser pose relative to the robot
143  pose = pf_vector_coord_add(self->laser_pose, pose);
144 
145  p = 1.0;
146 
147  step = (data->range_count - 1) / (self->max_beams - 1);
148  for (i = 0; i < data->range_count; i += step)
149  {
150  obs_range = data->ranges[i][0];
151  obs_bearing = data->ranges[i][1];
152 
153  // Compute the range according to the map
154  map_range = map_calc_range(self->map, pose.v[0], pose.v[1],
155  pose.v[2] + obs_bearing, data->range_max);
156  pz = 0.0;
157 
158  // Part 1: good, but noisy, hit
159  z = obs_range - map_range;
160  pz += self->z_hit * exp(-(z * z) / (2 * self->sigma_hit * self->sigma_hit));
161 
162  // Part 2: short reading from unexpected obstacle (e.g., a person)
163  if(z < 0)
164  pz += self->z_short * self->lambda_short * exp(-self->lambda_short*obs_range);
165 
166  // Part 3: Failure to detect obstacle, reported as max-range
167  if(obs_range == data->range_max)
168  pz += self->z_max * 1.0;
169 
170  // Part 4: Random measurements
171  if(obs_range < data->range_max)
172  pz += self->z_rand * 1.0/data->range_max;
173 
174  // TODO: outlier rejection for short readings
175 
176  //assert(pz <= 1.0);
177  //assert(pz >= 0.0);
178  if ( (pz < 0.) || (pz > 1.) ) pz = 0.;
179 
180  // p *= pz;
181  // here we have an ad-hoc weighting scheme for combining beam probs
182  // works well, though...
183  p += pz*pz*pz;
184  }
185 
186  sample->weight *= p;
187  total_weight += sample->weight;
188  }
189 
190  return(total_weight);
191 }
192 
193 double AMCLLaser::LikelihoodFieldModel(AMCLLaserData *data, pf_sample_set_t* set)
194 {
195  AMCLLaser *self;
196  int i, j, step;
197  double z, pz;
198  double p;
199  double obs_range, obs_bearing;
200  double total_weight;
201  pf_sample_t *sample;
202  pf_vector_t pose;
203  pf_vector_t hit;
204 
205  self = (AMCLLaser*) data->sensor;
206 
207  total_weight = 0.0;
208 
209  // Compute the sample weights
210  for (j = 0; j < set->sample_count; j++)
211  {
212  sample = set->samples + j;
213  pose = sample->pose;
214 
215  // Take account of the laser pose relative to the robot
216  pose = pf_vector_coord_add(self->laser_pose, pose);
217 
218  p = 1.0;
219 
220  // Pre-compute a couple of things
221  double z_hit_denom = 2 * self->sigma_hit * self->sigma_hit;
222  double z_rand_mult = 1.0/data->range_max;
223 
224  step = (data->range_count - 1) / (self->max_beams - 1);
225  for (i = 0; i < data->range_count; i += step)
226  {
227  obs_range = data->ranges[i][0];
228  obs_bearing = data->ranges[i][1];
229 
230  // This model ignores max range readings
231  if(obs_range >= data->range_max)
232  continue;
233 
234  pz = 0.0;
235 
236  // Compute the endpoint of the beam
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);
239 
240  // Convert to map grid coords.
241  int mi, mj;
242  mi = MAP_GXWX(self->map, hit.v[0]);
243  mj = MAP_GYWY(self->map, hit.v[1]);
244 
245  // Part 1: Get distance from the hit to closest obstacle.
246  // Off-map penalized as max distance
247  if(!MAP_VALID(self->map, mi, mj))
248  z = self->map->max_occ_dist;
249  else
250  z = self->map->cells[MAP_INDEX(self->map,mi,mj)].occ_dist;
251  // Gaussian model
252  // NOTE: this should have a normalization of 1/(sqrt(2pi)*sigma)
253  pz += self->z_hit * exp(-(z * z) / z_hit_denom);
254  // Part 2: random measurements
255  pz += self->z_rand * z_rand_mult;
256 
257  // TODO: outlier rejection for short readings
258 
259  //assert(pz <= 1.0);
260  //assert(pz >= 0.0);
261  if ( (pz < 0.) || (pz > 1.) ) pz = 0.;
262 
263  // p *= pz;
264  // here we have an ad-hoc weighting scheme for combining beam probs
265  // works well, though...
266  p += pz*pz*pz;
267  }
268 
269  sample->weight *= p;
270  total_weight += sample->weight;
271  }
272 
273  return(total_weight);
274 }
275 
276 /// @endcond