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