Fawkes API  Fawkes Development Version
amcl_laser.h
00001 
00002 /***************************************************************************
00003  *  amcl_laser.h: LASER sensor model for AMCL
00004  *
00005  *  Created: Thu May 24 18:49:44 2012
00006  *  Copyright  2000  Brian Gerkey
00007  *             2000  Kasper Stoy
00008  *             2012  Tim Niemueller [www.niemueller.de]
00009  ****************************************************************************/
00010 
00011 /*  This program is free software; you can redistribute it and/or modify
00012  *  it under the terms of the GNU General Public License as published by
00013  *  the Free Software Foundation; either version 2 of the License, or
00014  *  (at your option) any later version.
00015  *
00016  *  This program is distributed in the hope that it will be useful,
00017  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00018  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00019  *  GNU Library General Public License for more details.
00020  *
00021  *  Read the full text in the LICENSE.GPL file in the doc directory.
00022  */
00023 
00024 /*  From:
00025  *  Player - One Hell of a Robot Server (LGPL)
00026  *  Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
00027  *                      gerkey@usc.edu    kaspers@robotics.usc.edu
00028  */
00029 ///////////////////////////////////////////////////////////////////////////
00030 // Desc: LASER sensor model for AMCL
00031 // Author: Andrew Howard
00032 // Date: 17 Aug 2003
00033 ///////////////////////////////////////////////////////////////////////////
00034 
00035 #ifndef AMCL_LASER_H
00036 #define AMCL_LASER_H
00037 
00038 #include "amcl_sensor.h"
00039 #include "../map/map.h"
00040 
00041 /// @cond EXTERNAL
00042 
00043 namespace amcl
00044 {
00045 
00046 typedef enum
00047 {
00048   LASER_MODEL_BEAM,
00049   LASER_MODEL_LIKELIHOOD_FIELD
00050 } laser_model_t;
00051 
00052 // Laser sensor data
00053 class AMCLLaserData : public AMCLSensorData
00054 {
00055   public:
00056     AMCLLaserData () {ranges=NULL;};
00057     virtual ~AMCLLaserData() {delete [] ranges;};
00058   // Laser range data (range, bearing tuples)
00059   public: int range_count;
00060   public: double range_max;
00061   public: double (*ranges)[2];
00062 };
00063 
00064 
00065 // Laseretric sensor model
00066 class AMCLLaser : public AMCLSensor
00067 {
00068   // Default constructor
00069   public: AMCLLaser(size_t max_beams, map_t* map);
00070 
00071   public: void SetModelBeam(double z_hit,
00072                             double z_short,
00073                             double z_max,
00074                             double z_rand,
00075                             double sigma_hit,
00076                             double labda_short,
00077                             double chi_outlier);
00078 
00079   public: void SetModelLikelihoodField(double z_hit,
00080                                        double z_rand,
00081                                        double sigma_hit,
00082                                        double max_occ_dist);
00083   
00084   // Update the filter based on the sensor model.  Returns true if the
00085   // filter has been updated.
00086   public: virtual bool UpdateSensor(pf_t *pf, AMCLSensorData *data);
00087 
00088   // Set the laser's pose after construction
00089   public: void SetLaserPose(pf_vector_t& laser_pose) 
00090           {this->laser_pose = laser_pose;}
00091 
00092   // Determine the probability for the given pose
00093   private: static double BeamModel(AMCLLaserData *data, 
00094                                    pf_sample_set_t* set);
00095   // Determine the probability for the given pose
00096   private: static double LikelihoodFieldModel(AMCLLaserData *data, 
00097                                               pf_sample_set_t* set);
00098 
00099   private: laser_model_t model_type;
00100 
00101   // Current data timestamp
00102   private: double time;
00103 
00104   // The laser map
00105   private: map_t *map;
00106 
00107   // Laser offset relative to robot
00108   private: pf_vector_t laser_pose;
00109   
00110   // Max beams to consider
00111   private: int max_beams;
00112 
00113   // Laser model params
00114   //
00115   // Mixture params for the components of the model; must sum to 1
00116   private: double z_hit;
00117   private: double z_short;
00118   private: double z_max;
00119   private: double z_rand;
00120   //
00121   // Stddev of Gaussian model for laser hits.
00122   private: double sigma_hit;
00123   // Decay rate of exponential model for short readings.
00124   private: double lambda_short;
00125   // Threshold for outlier rejection (unused)
00126   private: double chi_outlier;
00127 };
00128 
00129 
00130 }
00131 
00132 /// @endcond
00133 
00134 #endif