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                                                      max_samples(0), max_obs(0), 
00044                                                      temp_obs(NULL)
00045 {
00046   this->time = 0.0;
00047 
00048   this->max_beams = max_beams;
00049   this->map = map;
00050 
00051   return;
00052 }
00053 
00054 AMCLLaser::~AMCLLaser()
00055 {
00056   if(temp_obs){
00057         for(int k=0; k < max_samples; k++){
00058           delete [] temp_obs[k];
00059         }
00060         delete []temp_obs; 
00061   }
00062 }
00063 
00064 void 
00065 AMCLLaser::SetModelBeam(double z_hit,
00066                         double z_short,
00067                         double z_max,
00068                         double z_rand,
00069                         double sigma_hit,
00070                         double lambda_short,
00071                         double chi_outlier)
00072 {
00073   this->model_type = LASER_MODEL_BEAM;
00074   this->z_hit = z_hit;
00075   this->z_short = z_short;
00076   this->z_max = z_max;
00077   this->z_rand = z_rand;
00078   this->sigma_hit = sigma_hit;
00079   this->lambda_short = lambda_short;
00080   this->chi_outlier = chi_outlier;
00081 }
00082 
00083 void 
00084 AMCLLaser::SetModelLikelihoodField(double z_hit,
00085                                    double z_rand,
00086                                    double sigma_hit,
00087                                    double max_occ_dist)
00088 {
00089   this->model_type = LASER_MODEL_LIKELIHOOD_FIELD;
00090   this->z_hit = z_hit;
00091   this->z_rand = z_rand;
00092   this->sigma_hit = sigma_hit;
00093 
00094   map_update_cspace(this->map, max_occ_dist);
00095 }
00096 
00097 void 
00098 AMCLLaser::SetModelLikelihoodFieldProb(double z_hit,
00099                                        double z_rand,
00100                                        double sigma_hit,
00101                                        double max_occ_dist,
00102                                        bool do_beamskip,
00103                                        double beam_skip_distance,
00104                                        double beam_skip_threshold, 
00105                                        double beam_skip_error_threshold)
00106 {
00107   this->model_type = LASER_MODEL_LIKELIHOOD_FIELD_PROB;
00108   this->z_hit = z_hit;
00109   this->z_rand = z_rand;
00110   this->sigma_hit = sigma_hit;
00111   this->do_beamskip = do_beamskip;
00112   this->beam_skip_distance = beam_skip_distance;
00113   this->beam_skip_threshold = beam_skip_threshold;
00114   this->beam_skip_error_threshold = beam_skip_error_threshold;
00115   map_update_cspace(this->map, max_occ_dist);
00116 }
00117 
00118 
00120 // Apply the laser sensor model
00121 bool AMCLLaser::UpdateSensor(pf_t *pf, AMCLSensorData *data)
00122 {
00123   if (this->max_beams < 2)
00124     return false;
00125 
00126   // Apply the laser sensor model
00127   if(this->model_type == LASER_MODEL_BEAM)
00128     pf_update_sensor(pf, (pf_sensor_model_fn_t) BeamModel, data);
00129   else if(this->model_type == LASER_MODEL_LIKELIHOOD_FIELD)
00130     pf_update_sensor(pf, (pf_sensor_model_fn_t) LikelihoodFieldModel, data);  
00131   else if(this->model_type == LASER_MODEL_LIKELIHOOD_FIELD_PROB)
00132     pf_update_sensor(pf, (pf_sensor_model_fn_t) LikelihoodFieldModelProb, data);  
00133   else
00134     pf_update_sensor(pf, (pf_sensor_model_fn_t) BeamModel, data);
00135 
00136   return true;
00137 }
00138 
00139 
00141 // Determine the probability for the given pose
00142 double AMCLLaser::BeamModel(AMCLLaserData *data, pf_sample_set_t* set)
00143 {
00144   AMCLLaser *self;
00145   int i, j, step;
00146   double z, pz;
00147   double p;
00148   double map_range;
00149   double obs_range, obs_bearing;
00150   double total_weight;
00151   pf_sample_t *sample;
00152   pf_vector_t pose;
00153 
00154   self = (AMCLLaser*) data->sensor;
00155 
00156   total_weight = 0.0;
00157 
00158   // Compute the sample weights
00159   for (j = 0; j < set->sample_count; j++)
00160   {
00161     sample = set->samples + j;
00162     pose = sample->pose;
00163 
00164     // Take account of the laser pose relative to the robot
00165     pose = pf_vector_coord_add(self->laser_pose, pose);
00166 
00167     p = 1.0;
00168 
00169     step = (data->range_count - 1) / (self->max_beams - 1);
00170     for (i = 0; i < data->range_count; i += step)
00171     {
00172       obs_range = data->ranges[i][0];
00173       obs_bearing = data->ranges[i][1];
00174 
00175       // Compute the range according to the map
00176       map_range = map_calc_range(self->map, pose.v[0], pose.v[1],
00177                                  pose.v[2] + obs_bearing, data->range_max);
00178       pz = 0.0;
00179 
00180       // Part 1: good, but noisy, hit
00181       z = obs_range - map_range;
00182       pz += self->z_hit * exp(-(z * z) / (2 * self->sigma_hit * self->sigma_hit));
00183 
00184       // Part 2: short reading from unexpected obstacle (e.g., a person)
00185       if(z < 0)
00186         pz += self->z_short * self->lambda_short * exp(-self->lambda_short*obs_range);
00187 
00188       // Part 3: Failure to detect obstacle, reported as max-range
00189       if(obs_range == data->range_max)
00190         pz += self->z_max * 1.0;
00191 
00192       // Part 4: Random measurements
00193       if(obs_range < data->range_max)
00194         pz += self->z_rand * 1.0/data->range_max;
00195 
00196       // TODO: outlier rejection for short readings
00197 
00198       assert(pz <= 1.0);
00199       assert(pz >= 0.0);
00200       //      p *= pz;
00201       // here we have an ad-hoc weighting scheme for combining beam probs
00202       // works well, though...
00203       p += pz*pz*pz;
00204     }
00205 
00206     sample->weight *= p;
00207     total_weight += sample->weight;
00208   }
00209 
00210   return(total_weight);
00211 }
00212 
00213 double AMCLLaser::LikelihoodFieldModel(AMCLLaserData *data, pf_sample_set_t* set)
00214 {
00215   AMCLLaser *self;
00216   int i, j, step;
00217   double z, pz;
00218   double p;
00219   double obs_range, obs_bearing;
00220   double total_weight;
00221   pf_sample_t *sample;
00222   pf_vector_t pose;
00223   pf_vector_t hit;
00224 
00225   self = (AMCLLaser*) data->sensor;
00226 
00227   total_weight = 0.0;
00228 
00229   // Compute the sample weights
00230   for (j = 0; j < set->sample_count; j++)
00231   {
00232     sample = set->samples + j;
00233     pose = sample->pose;
00234 
00235     // Take account of the laser pose relative to the robot
00236     pose = pf_vector_coord_add(self->laser_pose, pose);
00237 
00238     p = 1.0;
00239 
00240     // Pre-compute a couple of things
00241     double z_hit_denom = 2 * self->sigma_hit * self->sigma_hit;
00242     double z_rand_mult = 1.0/data->range_max;
00243 
00244     step = (data->range_count - 1) / (self->max_beams - 1);
00245 
00246     // Step size must be at least 1
00247     if(step < 1)
00248       step = 1;
00249 
00250     for (i = 0; i < data->range_count; i += step)
00251     {
00252       obs_range = data->ranges[i][0];
00253       obs_bearing = data->ranges[i][1];
00254 
00255       // This model ignores max range readings
00256       if(obs_range >= data->range_max)
00257         continue;
00258 
00259       // Check for NaN
00260       if(obs_range != obs_range)
00261         continue;
00262 
00263       pz = 0.0;
00264 
00265       // Compute the endpoint of the beam
00266       hit.v[0] = pose.v[0] + obs_range * cos(pose.v[2] + obs_bearing);
00267       hit.v[1] = pose.v[1] + obs_range * sin(pose.v[2] + obs_bearing);
00268 
00269       // Convert to map grid coords.
00270       int mi, mj;
00271       mi = MAP_GXWX(self->map, hit.v[0]);
00272       mj = MAP_GYWY(self->map, hit.v[1]);
00273       
00274       // Part 1: Get distance from the hit to closest obstacle.
00275       // Off-map penalized as max distance
00276       if(!MAP_VALID(self->map, mi, mj))
00277         z = self->map->max_occ_dist;
00278       else
00279         z = self->map->cells[MAP_INDEX(self->map,mi,mj)].occ_dist;
00280       // Gaussian model
00281       // NOTE: this should have a normalization of 1/(sqrt(2pi)*sigma)
00282       pz += self->z_hit * exp(-(z * z) / z_hit_denom);
00283       // Part 2: random measurements
00284       pz += self->z_rand * z_rand_mult;
00285 
00286       // TODO: outlier rejection for short readings
00287 
00288       assert(pz <= 1.0);
00289       assert(pz >= 0.0);
00290       //      p *= pz;
00291       // here we have an ad-hoc weighting scheme for combining beam probs
00292       // works well, though...
00293       p += pz*pz*pz;
00294     }
00295 
00296     sample->weight *= p;
00297     total_weight += sample->weight;
00298   }
00299 
00300   return(total_weight);
00301 }
00302 
00303 double AMCLLaser::LikelihoodFieldModelProb(AMCLLaserData *data, pf_sample_set_t* set)
00304 {
00305   AMCLLaser *self;
00306   int i, j, step;
00307   double z, pz;
00308   double log_p;
00309   double obs_range, obs_bearing;
00310   double total_weight;
00311   pf_sample_t *sample;
00312   pf_vector_t pose;
00313   pf_vector_t hit;
00314 
00315   self = (AMCLLaser*) data->sensor;
00316 
00317   total_weight = 0.0;
00318 
00319   step = ceil((data->range_count) / static_cast<double>(self->max_beams)); 
00320   
00321   // Step size must be at least 1
00322   if(step < 1)
00323     step = 1;
00324 
00325   // Pre-compute a couple of things
00326   double z_hit_denom = 2 * self->sigma_hit * self->sigma_hit;
00327   double z_rand_mult = 1.0/data->range_max;
00328 
00329   double max_dist_prob = exp(-(self->map->max_occ_dist * self->map->max_occ_dist) / z_hit_denom);
00330 
00331   //Beam skipping - ignores beams for which a majoirty of particles do not agree with the map
00332   //prevents correct particles from getting down weighted because of unexpected obstacles 
00333   //such as humans 
00334 
00335   bool do_beamskip = self->do_beamskip;
00336   double beam_skip_distance = self->beam_skip_distance;
00337   double beam_skip_threshold = self->beam_skip_threshold;
00338   
00339   //we only do beam skipping if the filter has converged 
00340   if(do_beamskip && !set->converged){
00341     do_beamskip = false;
00342   }
00343 
00344   //we need a count the no of particles for which the beam agreed with the map 
00345   int *obs_count = new int[self->max_beams]();
00346 
00347   //we also need a mask of which observations to integrate (to decide which beams to integrate to all particles) 
00348   bool *obs_mask = new bool[self->max_beams]();
00349   
00350   int beam_ind = 0;
00351   
00352   //realloc indicates if we need to reallocate the temp data structure needed to do beamskipping 
00353   bool realloc = false; 
00354 
00355   if(do_beamskip){
00356     if(self->max_obs < self->max_beams){
00357       realloc = true;
00358     }
00359 
00360     if(self->max_samples < set->sample_count){
00361       realloc = true;
00362     }
00363 
00364     if(realloc){
00365       self->reallocTempData(set->sample_count, self->max_beams);     
00366       fprintf(stderr, "Reallocing temp weights %d - %d\n", self->max_samples, self->max_obs);
00367     }
00368   }
00369 
00370   // Compute the sample weights
00371   for (j = 0; j < set->sample_count; j++)
00372   {
00373     sample = set->samples + j;
00374     pose = sample->pose;
00375 
00376     // Take account of the laser pose relative to the robot
00377     pose = pf_vector_coord_add(self->laser_pose, pose);
00378 
00379     log_p = 0;
00380     
00381     beam_ind = 0;
00382     
00383     for (i = 0; i < data->range_count; i += step, beam_ind++)
00384     {
00385       obs_range = data->ranges[i][0];
00386       obs_bearing = data->ranges[i][1];
00387 
00388       // This model ignores max range readings
00389       if(obs_range >= data->range_max){
00390         continue;
00391       }
00392 
00393       // Check for NaN
00394       if(obs_range != obs_range){
00395         continue;
00396       }
00397 
00398       pz = 0.0;
00399 
00400       // Compute the endpoint of the beam
00401       hit.v[0] = pose.v[0] + obs_range * cos(pose.v[2] + obs_bearing);
00402       hit.v[1] = pose.v[1] + obs_range * sin(pose.v[2] + obs_bearing);
00403 
00404       // Convert to map grid coords.
00405       int mi, mj;
00406       mi = MAP_GXWX(self->map, hit.v[0]);
00407       mj = MAP_GYWY(self->map, hit.v[1]);
00408       
00409       // Part 1: Get distance from the hit to closest obstacle.
00410       // Off-map penalized as max distance
00411       
00412       if(!MAP_VALID(self->map, mi, mj)){
00413         pz += self->z_hit * max_dist_prob;
00414       }
00415       else{
00416         z = self->map->cells[MAP_INDEX(self->map,mi,mj)].occ_dist;
00417         if(z < beam_skip_distance){
00418           obs_count[beam_ind] += 1;
00419         }
00420         pz += self->z_hit * exp(-(z * z) / z_hit_denom);
00421       }
00422        
00423       // Gaussian model
00424       // NOTE: this should have a normalization of 1/(sqrt(2pi)*sigma)
00425       
00426       // Part 2: random measurements
00427       pz += self->z_rand * z_rand_mult;
00428 
00429       assert(pz <= 1.0); 
00430       assert(pz >= 0.0);
00431 
00432       // TODO: outlier rejection for short readings
00433             
00434       if(!do_beamskip){
00435         log_p += log(pz);
00436       }
00437       else{
00438         self->temp_obs[j][beam_ind] = pz; 
00439       }
00440     }
00441     if(!do_beamskip){
00442       sample->weight *= exp(log_p);
00443       total_weight += sample->weight;
00444     }
00445   }
00446   
00447   if(do_beamskip){
00448     int skipped_beam_count = 0; 
00449     for (beam_ind = 0; beam_ind < self->max_beams; beam_ind++){
00450       if((obs_count[beam_ind] / static_cast<double>(set->sample_count)) > beam_skip_threshold){
00451         obs_mask[beam_ind] = true;
00452       }
00453       else{
00454         obs_mask[beam_ind] = false;
00455         skipped_beam_count++; 
00456       }
00457     }
00458 
00459     //we check if there is at least a critical number of beams that agreed with the map 
00460     //otherwise it probably indicates that the filter converged to a wrong solution
00461     //if that's the case we integrate all the beams and hope the filter might converge to 
00462     //the right solution
00463     bool error = false; 
00464 
00465     if(skipped_beam_count >= (beam_ind * self->beam_skip_error_threshold)){
00466       fprintf(stderr, "Over %f%% of the observations were not in the map - pf may have converged to wrong pose - integrating all observations\n", (100 * self->beam_skip_error_threshold));
00467       error = true; 
00468     }
00469 
00470     for (j = 0; j < set->sample_count; j++)
00471       {
00472         sample = set->samples + j;
00473         pose = sample->pose;
00474 
00475         log_p = 0;
00476 
00477         for (beam_ind = 0; beam_ind < self->max_beams; beam_ind++){
00478           if(error || obs_mask[beam_ind]){
00479             log_p += log(self->temp_obs[j][beam_ind]);
00480           }
00481         }
00482         
00483         sample->weight *= exp(log_p);
00484         
00485         total_weight += sample->weight;
00486       }      
00487   }
00488 
00489   delete [] obs_count; 
00490   delete [] obs_mask;
00491   return(total_weight);
00492 }
00493 
00494 void AMCLLaser::reallocTempData(int new_max_samples, int new_max_obs){
00495   if(temp_obs){
00496     for(int k=0; k < max_samples; k++){
00497       delete [] temp_obs[k];
00498     }
00499     delete []temp_obs; 
00500   }
00501   max_obs = new_max_obs; 
00502   max_samples = fmax(max_samples, new_max_samples); 
00503 
00504   temp_obs = new double*[max_samples]();
00505   for(int k=0; k < max_samples; k++){
00506     temp_obs[k] = new double[max_obs]();
00507   }
00508 }


amcl
Author(s): Brian P. Gerkey, contradict@gmail.com
autogenerated on Thu Aug 27 2015 14:07:48