amcl_laser.h
Go to the documentation of this file.
00001 /*
00002  *  Player - One Hell of a Robot Server
00003  *  Copyright (C) 2000  Brian Gerkey et al.
00004  *
00005  *  This library is free software; you can redistribute it and/or
00006  *  modify it under the terms of the GNU Lesser General Public
00007  *  License as published by the Free Software Foundation; either
00008  *  version 2.1 of the License, or (at your option) any later version.
00009  *
00010  *  This library is distributed in the hope that it will be useful,
00011  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00012  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00013  *  Lesser General Public License for more details.
00014  *
00015  *  You should have received a copy of the GNU Lesser General Public
00016  *  License along with this library; if not, write to the Free Software
00017  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00018  *
00019  */
00021 //
00022 // Desc: LASER sensor model for AMCL
00023 // Author: Andrew Howard
00024 // Date: 17 Aug 2003
00025 // CVS: $Id: amcl_laser.h 6443 2008-05-15 19:46:11Z gerkey $
00026 //
00028 
00029 #ifndef AMCL_LASER_H
00030 #define AMCL_LASER_H
00031 
00032 #include "amcl_sensor.h"
00033 #include "../map/map.h"
00034 
00035 namespace amcl
00036 {
00037 
00038 typedef enum
00039 {
00040   LASER_MODEL_BEAM,
00041   LASER_MODEL_LIKELIHOOD_FIELD,
00042   LASER_MODEL_LIKELIHOOD_FIELD_PROB
00043 } laser_model_t;
00044 
00045 // Laser sensor data
00046 class AMCLLaserData : public AMCLSensorData
00047 {
00048   public:
00049     AMCLLaserData () {ranges=NULL;};
00050     virtual ~AMCLLaserData() {delete [] ranges;};
00051   // Laser range data (range, bearing tuples)
00052   public: int range_count;
00053   public: double range_max;
00054   public: double (*ranges)[2];
00055 };
00056 
00057 
00058 // Laseretric sensor model
00059 class AMCLLaser : public AMCLSensor
00060 {
00061   // Default constructor
00062   public: AMCLLaser(size_t max_beams, map_t* map);
00063 
00064   public: virtual ~AMCLLaser(); 
00065 
00066   public: void SetModelBeam(double z_hit,
00067                             double z_short,
00068                             double z_max,
00069                             double z_rand,
00070                             double sigma_hit,
00071                             double labda_short,
00072                             double chi_outlier);
00073 
00074   public: void SetModelLikelihoodField(double z_hit,
00075                                        double z_rand,
00076                                        double sigma_hit,
00077                                        double max_occ_dist);
00078 
00079   //a more probabilistically correct model - also with the option to do beam skipping
00080   public: void SetModelLikelihoodFieldProb(double z_hit,
00081                                            double z_rand,
00082                                            double sigma_hit,
00083                                            double max_occ_dist, 
00084                                            bool do_beamskip, 
00085                                            double beam_skip_distance, 
00086                                            double beam_skip_threshold, 
00087                                            double beam_skip_error_threshold);
00088 
00089   // Update the filter based on the sensor model.  Returns true if the
00090   // filter has been updated.
00091   public: virtual bool UpdateSensor(pf_t *pf, AMCLSensorData *data);
00092 
00093   // Set the laser's pose after construction
00094   public: void SetLaserPose(pf_vector_t& laser_pose) 
00095           {this->laser_pose = laser_pose;}
00096 
00097   // Determine the probability for the given pose
00098   private: static double BeamModel(AMCLLaserData *data, 
00099                                    pf_sample_set_t* set);
00100   // Determine the probability for the given pose
00101   private: static double LikelihoodFieldModel(AMCLLaserData *data, 
00102                                               pf_sample_set_t* set);
00103 
00104   // Determine the probability for the given pose - more probablistic model 
00105   private: static double LikelihoodFieldModelProb(AMCLLaserData *data, 
00106                                              pf_sample_set_t* set);
00107 
00108   private: void reallocTempData(int max_samples, int max_obs);
00109 
00110   private: laser_model_t model_type;
00111 
00112   // Current data timestamp
00113   private: double time;
00114 
00115   // The laser map
00116   private: map_t *map;
00117 
00118   // Laser offset relative to robot
00119   private: pf_vector_t laser_pose;
00120   
00121   // Max beams to consider
00122   private: int max_beams;
00123 
00124   // Beam skipping parameters (used by LikelihoodFieldModelProb model)
00125   private: bool do_beamskip; 
00126   private: double beam_skip_distance; 
00127   private: double beam_skip_threshold; 
00128   //threshold for the ratio of invalid beams - at which all beams are integrated to the likelihoods 
00129   //this would be an error condition 
00130   private: double beam_skip_error_threshold;
00131 
00132   //temp data that is kept before observations are integrated to each particle (requried for beam skipping)
00133   private: int max_samples;
00134   private: int max_obs;
00135   private: double **temp_obs;
00136 
00137   // Laser model params
00138   //
00139   // Mixture params for the components of the model; must sum to 1
00140   private: double z_hit;
00141   private: double z_short;
00142   private: double z_max;
00143   private: double z_rand;
00144   //
00145   // Stddev of Gaussian model for laser hits.
00146   private: double sigma_hit;
00147   // Decay rate of exponential model for short readings.
00148   private: double lambda_short;
00149   // Threshold for outlier rejection (unused)
00150   private: double chi_outlier;
00151 };
00152 
00153 
00154 }
00155 
00156 #endif


amcl
Author(s): Brian P. Gerkey, contradict@gmail.com
autogenerated on Sun Mar 3 2019 03:46:02