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