Fawkes API
Fawkes Development Version
|
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