$search
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 AMCLLaserData *ndata; 00092 00093 ndata = (AMCLLaserData*) data; 00094 if (this->max_beams < 2) 00095 return false; 00096 00097 // Apply the laser sensor model 00098 if(this->model_type == LASER_MODEL_BEAM) 00099 pf_update_sensor(pf, (pf_sensor_model_fn_t) BeamModel, data); 00100 else if(this->model_type == LASER_MODEL_LIKELIHOOD_FIELD) 00101 pf_update_sensor(pf, (pf_sensor_model_fn_t) LikelihoodFieldModel, data); 00102 else 00103 pf_update_sensor(pf, (pf_sensor_model_fn_t) BeamModel, data); 00104 00105 return true; 00106 } 00107 00108 00110 // Determine the probability for the given pose 00111 double AMCLLaser::BeamModel(AMCLLaserData *data, pf_sample_set_t* set) 00112 { 00113 AMCLLaser *self; 00114 int i, j, step; 00115 double z, pz; 00116 double p; 00117 double map_range; 00118 double obs_range, obs_bearing; 00119 double total_weight; 00120 pf_sample_t *sample; 00121 pf_vector_t pose; 00122 00123 self = (AMCLLaser*) data->sensor; 00124 00125 total_weight = 0.0; 00126 00127 // Compute the sample weights 00128 for (j = 0; j < set->sample_count; j++) 00129 { 00130 sample = set->samples + j; 00131 pose = sample->pose; 00132 00133 // Take account of the laser pose relative to the robot 00134 pose = pf_vector_coord_add(self->laser_pose, pose); 00135 00136 p = 1.0; 00137 00138 step = (data->range_count - 1) / (self->max_beams - 1); 00139 for (i = 0; i < data->range_count; i += step) 00140 { 00141 obs_range = data->ranges[i][0]; 00142 obs_bearing = data->ranges[i][1]; 00143 00144 // Compute the range according to the map 00145 map_range = map_calc_range(self->map, pose.v[0], pose.v[1], 00146 pose.v[2] + obs_bearing, data->range_max); 00147 pz = 0.0; 00148 00149 // Part 1: good, but noisy, hit 00150 z = obs_range - map_range; 00151 pz += self->z_hit * exp(-(z * z) / (2 * self->sigma_hit * self->sigma_hit)); 00152 00153 // Part 2: short reading from unexpected obstacle (e.g., a person) 00154 if(z < 0) 00155 pz += self->z_short * self->lambda_short * exp(-self->lambda_short*obs_range); 00156 00157 // Part 3: Failure to detect obstacle, reported as max-range 00158 if(obs_range == data->range_max) 00159 pz += self->z_max * 1.0; 00160 00161 // Part 4: Random measurements 00162 if(obs_range < data->range_max) 00163 pz += self->z_rand * 1.0/data->range_max; 00164 00165 // TODO: outlier rejection for short readings 00166 00167 assert(pz <= 1.0); 00168 assert(pz >= 0.0); 00169 // p *= pz; 00170 // here we have an ad-hoc weighting scheme for combining beam probs 00171 // works well, though... 00172 p += pz*pz*pz; 00173 } 00174 00175 sample->weight *= p; 00176 total_weight += sample->weight; 00177 } 00178 00179 return(total_weight); 00180 } 00181 00182 double AMCLLaser::LikelihoodFieldModel(AMCLLaserData *data, pf_sample_set_t* set) 00183 { 00184 AMCLLaser *self; 00185 int i, j, step; 00186 double z, pz; 00187 double p; 00188 double obs_range, obs_bearing; 00189 double total_weight; 00190 pf_sample_t *sample; 00191 pf_vector_t pose; 00192 pf_vector_t hit; 00193 00194 self = (AMCLLaser*) data->sensor; 00195 00196 total_weight = 0.0; 00197 00198 // Compute the sample weights 00199 for (j = 0; j < set->sample_count; j++) 00200 { 00201 sample = set->samples + j; 00202 pose = sample->pose; 00203 00204 // Take account of the laser pose relative to the robot 00205 pose = pf_vector_coord_add(self->laser_pose, pose); 00206 00207 p = 1.0; 00208 00209 // Pre-compute a couple of things 00210 double z_hit_denom = 2 * self->sigma_hit * self->sigma_hit; 00211 double z_rand_mult = 1.0/data->range_max; 00212 00213 step = (data->range_count - 1) / (self->max_beams - 1); 00214 for (i = 0; i < data->range_count; i += step) 00215 { 00216 obs_range = data->ranges[i][0]; 00217 obs_bearing = data->ranges[i][1]; 00218 00219 // This model ignores max range readings 00220 if(obs_range >= data->range_max) 00221 continue; 00222 00223 pz = 0.0; 00224 00225 // Compute the endpoint of the beam 00226 hit.v[0] = pose.v[0] + obs_range * cos(pose.v[2] + obs_bearing); 00227 hit.v[1] = pose.v[1] + obs_range * sin(pose.v[2] + obs_bearing); 00228 00229 // Convert to map grid coords. 00230 int mi, mj; 00231 mi = MAP_GXWX(self->map, hit.v[0]); 00232 mj = MAP_GYWY(self->map, hit.v[1]); 00233 00234 // Part 1: Get distance from the hit to closest obstacle. 00235 // Off-map penalized as max distance 00236 if(!MAP_VALID(self->map, mi, mj)) 00237 z = self->map->max_occ_dist; 00238 else 00239 z = self->map->cells[MAP_INDEX(self->map,mi,mj)].occ_dist; 00240 // Gaussian model 00241 // NOTE: this should have a normalization of 1/(sqrt(2pi)*sigma) 00242 pz += self->z_hit * exp(-(z * z) / z_hit_denom); 00243 // Part 2: random measurements 00244 pz += self->z_rand * z_rand_mult; 00245 00246 // TODO: outlier rejection for short readings 00247 00248 assert(pz <= 1.0); 00249 assert(pz >= 0.0); 00250 // p *= pz; 00251 // here we have an ad-hoc weighting scheme for combining beam probs 00252 // works well, though... 00253 p += pz*pz*pz; 00254 } 00255 00256 sample->weight *= p; 00257 total_weight += sample->weight; 00258 } 00259 00260 return(total_weight); 00261 }