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_max = z_max;
90  this->z_rand = z_rand;
91  this->sigma_hit = sigma_hit;
92 
93  map_update_cspace(this->map, max_occ_dist);
94 }
95 
96 
97 ////////////////////////////////////////////////////////////////////////////////
98 // Apply the laser sensor model
99 bool AMCLLaser::UpdateSensor(pf_t *pf, AMCLSensorData *data)
100 {
101  //AMCLLaserData *ndata;
102 
103  //ndata = (AMCLLaserData*) data;
104  if (this->max_beams < 2)
105  return false;
106 
107  // Apply the laser sensor model
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);
112  else
113  pf_update_sensor(pf, (pf_sensor_model_fn_t) BeamModel, data);
114 
115  return true;
116 }
117 
118 
119 ////////////////////////////////////////////////////////////////////////////////
120 // Determine the probability for the given pose
121 double AMCLLaser::BeamModel(AMCLLaserData *data, pf_sample_set_t* set)
122 {
123  AMCLLaser *self;
124  int i, j, step;
125  double z, pz;
126  double p;
127  double map_range;
128  double obs_range, obs_bearing;
129  double total_weight;
130  pf_sample_t *sample;
131  pf_vector_t pose;
132 
133  self = (AMCLLaser*) data->sensor;
134 
135  total_weight = 0.0;
136 
137  // Compute the sample weights
138  for (j = 0; j < set->sample_count; j++)
139  {
140  sample = set->samples + j;
141  pose = sample->pose;
142 
143  // Take account of the laser pose relative to the robot
144  pose = pf_vector_coord_add(self->laser_pose, pose);
145 
146  p = 1.0;
147 
148  step = (data->range_count - 1) / (self->max_beams - 1);
149  for (i = 0; i < data->range_count; i += step)
150  {
151  obs_range = data->ranges[i][0];
152  obs_bearing = data->ranges[i][1];
153 
154  // Compute the range according to the map
155  map_range = map_calc_range(self->map, pose.v[0], pose.v[1],
156  pose.v[2] + obs_bearing, data->range_max);
157  pz = 0.0;
158 
159  // Part 1: good, but noisy, hit
160  z = obs_range - map_range;
161  pz += self->z_hit * exp(-(z * z) / (2 * self->sigma_hit * self->sigma_hit));
162 
163  // Part 2: short reading from unexpected obstacle (e.g., a person)
164  if(z < 0)
165  pz += self->z_short * self->lambda_short * exp(-self->lambda_short*obs_range);
166 
167  // Part 3: Failure to detect obstacle, reported as max-range
168  if(obs_range == data->range_max)
169  pz += self->z_max * 1.0;
170 
171  // Part 4: Random measurements
172  if(obs_range < data->range_max)
173  pz += self->z_rand * 1.0/data->range_max;
174 
175  // TODO: outlier rejection for short readings
176 
177  //assert(pz <= 1.0);
178  //assert(pz >= 0.0);
179  if ( (pz < 0.) || (pz > 1.) ) pz = 0.;
180 
181  // p *= pz;
182  // here we have an ad-hoc weighting scheme for combining beam probs
183  // works well, though...
184  p += pz*pz*pz;
185  }
186 
187  sample->weight *= p;
188  total_weight += sample->weight;
189  }
190 
191  return(total_weight);
192 }
193 
194 double AMCLLaser::LikelihoodFieldModel(AMCLLaserData *data, pf_sample_set_t* set)
195 {
196  AMCLLaser *self;
197  int i, j, step;
198  double z, pz;
199  double p;
200  double obs_range, obs_bearing;
201  double total_weight;
202  pf_sample_t *sample;
203  pf_vector_t pose;
204  pf_vector_t hit;
205 
206  self = (AMCLLaser*) data->sensor;
207 
208  total_weight = 0.0;
209 
210  // Compute the sample weights
211  for (j = 0; j < set->sample_count; j++)
212  {
213  sample = set->samples + j;
214  pose = sample->pose;
215 
216  // Take account of the laser pose relative to the robot
217  pose = pf_vector_coord_add(self->laser_pose, pose);
218 
219  p = 1.0;
220 
221  // Pre-compute a couple of things
222  double z_hit_denom = 2 * self->sigma_hit * self->sigma_hit;
223  double z_rand_mult = 1.0/data->range_max;
224 
225  step = (data->range_count - 1) / (self->max_beams - 1);
226  for (i = 0; i < data->range_count; i += step)
227  {
228  obs_range = data->ranges[i][0];
229  obs_bearing = data->ranges[i][1];
230 
231  // This model ignores max range readings
232  if(obs_range >= data->range_max)
233  continue;
234 
235  pz = 0.0;
236 
237  // Compute the endpoint of the beam
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);
240 
241  // Convert to map grid coords.
242  int mi, mj;
243  mi = MAP_GXWX(self->map, hit.v[0]);
244  mj = MAP_GYWY(self->map, hit.v[1]);
245 
246  // Part 1: Get distance from the hit to closest obstacle.
247  // Off-map penalized as max distance
248  if(!MAP_VALID(self->map, mi, mj))
249  z = self->map->max_occ_dist;
250  else
251  z = self->map->cells[MAP_INDEX(self->map,mi,mj)].occ_dist;
252  // Gaussian model
253  // NOTE: this should have a normalization of 1/(sqrt(2pi)*sigma)
254  pz += self->z_hit * exp(-(z * z) / z_hit_denom);
255  // Part 2: random measurements
256  pz += self->z_rand * z_rand_mult;
257 
258  // TODO: outlier rejection for short readings
259 
260  //assert(pz <= 1.0);
261  //assert(pz >= 0.0);
262  if ( (pz < 0.) || (pz > 1.) ) pz = 0.;
263 
264  // p *= pz;
265  // here we have an ad-hoc weighting scheme for combining beam probs
266  // works well, though...
267  p += pz*pz*pz;
268  }
269 
270  sample->weight *= p;
271  total_weight += sample->weight;
272  }
273 
274  return(total_weight);
275 }
276 
277 /// @endcond