amcl_laser.cpp
Go to the documentation of this file.
00001 /*
00002  *  Player - One Hell of a Robot Server
00003  *  Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
00004  *                      gerkey@usc.edu    kaspers@robotics.usc.edu
00005  *
00006  *  This library is free software; you can redistribute it and/or
00007  *  modify it under the terms of the GNU Lesser General Public
00008  *  License as published by the Free Software Foundation; either
00009  *  version 2.1 of the License, or (at your option) any later version.
00010  *
00011  *  This library is distributed in the hope that it will be useful,
00012  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00013  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00014  *  Lesser General Public License for more details.
00015  *
00016  *  You should have received a copy of the GNU Lesser General Public
00017  *  License along with this library; if not, write to the Free Software
00018  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00019  *
00020  */
00022 //
00023 // Desc: AMCL laser routines
00024 // Author: Andrew Howard
00025 // Date: 6 Feb 2003
00026 // CVS: $Id: amcl_laser.cc 7057 2008-10-02 00:44:06Z gbiggs $
00027 //
00029 
00030 #include <sys/types.h> // required by Darwin
00031 #include <math.h>
00032 #include <stdlib.h>
00033 #include <assert.h>
00034 #include <unistd.h>
00035 
00036 #include "amcl_laser.h"
00037 
00038 using namespace amcl;
00039 
00041 // Default constructor
00042 AMCLLaser::AMCLLaser(size_t max_beams, map_t* map) : AMCLSensor()
00043 {
00044   this->time = 0.0;
00045 
00046   this->max_beams = max_beams;
00047   this->map = map;
00048 
00049   return;
00050 }
00051 
00052 void 
00053 AMCLLaser::SetModelBeam(double z_hit,
00054                         double z_short,
00055                         double z_max,
00056                         double z_rand,
00057                         double sigma_hit,
00058                         double lambda_short,
00059                         double chi_outlier)
00060 {
00061   this->model_type = LASER_MODEL_BEAM;
00062   this->z_hit = z_hit;
00063   this->z_short = z_short;
00064   this->z_max = z_max;
00065   this->z_rand = z_rand;
00066   this->sigma_hit = sigma_hit;
00067   this->lambda_short = lambda_short;
00068   this->chi_outlier = chi_outlier;
00069 }
00070 
00071 void 
00072 AMCLLaser::SetModelLikelihoodField(double z_hit,
00073                                    double z_rand,
00074                                    double sigma_hit,
00075                                    double max_occ_dist)
00076 {
00077   this->model_type = LASER_MODEL_LIKELIHOOD_FIELD;
00078   this->z_hit = z_hit;
00079   this->z_max = z_max;
00080   this->z_rand = z_rand;
00081   this->sigma_hit = sigma_hit;
00082 
00083   map_update_cspace(this->map, max_occ_dist);
00084 }
00085 
00086 
00088 // Apply the laser sensor model
00089 bool AMCLLaser::UpdateSensor(pf_t *pf, AMCLSensorData *data)
00090 {
00091   if (this->max_beams < 2)
00092     return false;
00093 
00094   // Apply the laser sensor model
00095   if(this->model_type == LASER_MODEL_BEAM)
00096     pf_update_sensor(pf, (pf_sensor_model_fn_t) BeamModel, data);
00097   else if(this->model_type == LASER_MODEL_LIKELIHOOD_FIELD)
00098     pf_update_sensor(pf, (pf_sensor_model_fn_t) LikelihoodFieldModel, data);
00099   else
00100     pf_update_sensor(pf, (pf_sensor_model_fn_t) BeamModel, data);
00101 
00102   return true;
00103 }
00104 
00105 
00107 // Determine the probability for the given pose
00108 double AMCLLaser::BeamModel(AMCLLaserData *data, pf_sample_set_t* set)
00109 {
00110   AMCLLaser *self;
00111   int i, j, step;
00112   double z, pz;
00113   double p;
00114   double map_range;
00115   double obs_range, obs_bearing;
00116   double total_weight;
00117   pf_sample_t *sample;
00118   pf_vector_t pose;
00119 
00120   self = (AMCLLaser*) data->sensor;
00121 
00122   total_weight = 0.0;
00123 
00124   // Compute the sample weights
00125   for (j = 0; j < set->sample_count; j++)
00126   {
00127     sample = set->samples + j;
00128     pose = sample->pose;
00129 
00130     // Take account of the laser pose relative to the robot
00131     pose = pf_vector_coord_add(self->laser_pose, pose);
00132 
00133     p = 1.0;
00134 
00135     step = (data->range_count - 1) / (self->max_beams - 1);
00136     for (i = 0; i < data->range_count; i += step)
00137     {
00138       obs_range = data->ranges[i][0];
00139       obs_bearing = data->ranges[i][1];
00140 
00141       // Compute the range according to the map
00142       map_range = map_calc_range(self->map, pose.v[0], pose.v[1],
00143                                  pose.v[2] + obs_bearing, data->range_max);
00144       pz = 0.0;
00145 
00146       // Part 1: good, but noisy, hit
00147       z = obs_range - map_range;
00148       pz += self->z_hit * exp(-(z * z) / (2 * self->sigma_hit * self->sigma_hit));
00149 
00150       // Part 2: short reading from unexpected obstacle (e.g., a person)
00151       if(z < 0)
00152         pz += self->z_short * self->lambda_short * exp(-self->lambda_short*obs_range);
00153 
00154       // Part 3: Failure to detect obstacle, reported as max-range
00155       if(obs_range == data->range_max)
00156         pz += self->z_max * 1.0;
00157 
00158       // Part 4: Random measurements
00159       if(obs_range < data->range_max)
00160         pz += self->z_rand * 1.0/data->range_max;
00161 
00162       // TODO: outlier rejection for short readings
00163 
00164       assert(pz <= 1.0);
00165       assert(pz >= 0.0);
00166       //      p *= pz;
00167       // here we have an ad-hoc weighting scheme for combining beam probs
00168       // works well, though...
00169       p += pz*pz*pz;
00170     }
00171 
00172     sample->weight *= p;
00173     total_weight += sample->weight;
00174   }
00175 
00176   return(total_weight);
00177 }
00178 
00179 double AMCLLaser::LikelihoodFieldModel(AMCLLaserData *data, pf_sample_set_t* set)
00180 {
00181   AMCLLaser *self;
00182   int i, j, step;
00183   double z, pz;
00184   double p;
00185   double obs_range, obs_bearing;
00186   double total_weight;
00187   pf_sample_t *sample;
00188   pf_vector_t pose;
00189   pf_vector_t hit;
00190 
00191   self = (AMCLLaser*) data->sensor;
00192 
00193   total_weight = 0.0;
00194 
00195   // Compute the sample weights
00196   for (j = 0; j < set->sample_count; j++)
00197   {
00198     sample = set->samples + j;
00199     pose = sample->pose;
00200 
00201     // Take account of the laser pose relative to the robot
00202     pose = pf_vector_coord_add(self->laser_pose, pose);
00203 
00204     p = 1.0;
00205 
00206     // Pre-compute a couple of things
00207     double z_hit_denom = 2 * self->sigma_hit * self->sigma_hit;
00208     double z_rand_mult = 1.0/data->range_max;
00209 
00210     step = (data->range_count - 1) / (self->max_beams - 1);
00211     for (i = 0; i < data->range_count; i += step)
00212     {
00213       obs_range = data->ranges[i][0];
00214       obs_bearing = data->ranges[i][1];
00215 
00216       // This model ignores max range readings
00217       if(obs_range >= data->range_max)
00218         continue;
00219 
00220       pz = 0.0;
00221 
00222       // Compute the endpoint of the beam
00223       hit.v[0] = pose.v[0] + obs_range * cos(pose.v[2] + obs_bearing);
00224       hit.v[1] = pose.v[1] + obs_range * sin(pose.v[2] + obs_bearing);
00225 
00226       // Convert to map grid coords.
00227       int mi, mj;
00228       mi = MAP_GXWX(self->map, hit.v[0]);
00229       mj = MAP_GYWY(self->map, hit.v[1]);
00230       
00231       // Part 1: Get distance from the hit to closest obstacle.
00232       // Off-map penalized as max distance
00233       if(!MAP_VALID(self->map, mi, mj))
00234         z = self->map->max_occ_dist;
00235       else
00236         z = self->map->cells[MAP_INDEX(self->map,mi,mj)].occ_dist;
00237       // Gaussian model
00238       // NOTE: this should have a normalization of 1/(sqrt(2pi)*sigma)
00239       pz += self->z_hit * exp(-(z * z) / z_hit_denom);
00240       // Part 2: random measurements
00241       pz += self->z_rand * z_rand_mult;
00242 
00243       // TODO: outlier rejection for short readings
00244 
00245       assert(pz <= 1.0);
00246       assert(pz >= 0.0);
00247       //      p *= pz;
00248       // here we have an ad-hoc weighting scheme for combining beam probs
00249       // works well, though...
00250       p += pz*pz*pz;
00251     }
00252 
00253     sample->weight *= p;
00254     total_weight += sample->weight;
00255   }
00256 
00257   return(total_weight);
00258 }


amcl
Author(s): Brian P. Gerkey
autogenerated on Mon Oct 6 2014 02:49:04