Fawkes API  Fawkes Development Version
amcl_laser.h
1 
2 /***************************************************************************
3  * amcl_laser.h: LASER sensor model for AMCL
4  *
5  * Created: Thu May 24 18:49:44 2012
6  * Copyright 2000 Brian Gerkey
7  * 2000 Kasper Stoy
8  * 2012 Tim Niemueller [www.niemueller.de]
9  ****************************************************************************/
10 
11 /* This program is free software; you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation; either version 2 of the License, or
14  * (at your option) any later version.
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL file in the doc directory.
22  */
23 
24 /* From:
25  * Player - One Hell of a Robot Server (LGPL)
26  * Copyright (C) 2000 Brian Gerkey & Kasper Stoy
27  * gerkey@usc.edu kaspers@robotics.usc.edu
28  */
29 ///////////////////////////////////////////////////////////////////////////
30 // Desc: LASER sensor model for AMCL
31 // Author: Andrew Howard
32 // Date: 17 Aug 2003
33 ///////////////////////////////////////////////////////////////////////////
34 
35 #ifndef AMCL_LASER_H
36 #define AMCL_LASER_H
37 
38 #include "amcl_sensor.h"
39 #include "../map/map.h"
40 
41 /// @cond EXTERNAL
42 
43 namespace amcl
44 {
45 
46 typedef enum
47 {
48  LASER_MODEL_BEAM,
49  LASER_MODEL_LIKELIHOOD_FIELD
50 } laser_model_t;
51 
52 // Laser sensor data
53 class AMCLLaserData : public AMCLSensorData
54 {
55  public:
56  AMCLLaserData () {ranges=NULL;};
57  virtual ~AMCLLaserData() {delete [] ranges;};
58  // Laser range data (range, bearing tuples)
59  public: int range_count;
60  public: double range_max;
61  public: double (*ranges)[2];
62 };
63 
64 
65 // Laseretric sensor model
66 class AMCLLaser : public AMCLSensor
67 {
68  // Default constructor
69  public: AMCLLaser(size_t max_beams, map_t* map);
70 
71  public: void SetModelBeam(double z_hit,
72  double z_short,
73  double z_max,
74  double z_rand,
75  double sigma_hit,
76  double labda_short,
77  double chi_outlier);
78 
79  public: void SetModelLikelihoodField(double z_hit,
80  double z_rand,
81  double sigma_hit,
82  double max_occ_dist);
83 
84  // Update the filter based on the sensor model. Returns true if the
85  // filter has been updated.
86  public: virtual bool UpdateSensor(pf_t *pf, AMCLSensorData *data);
87 
88  // Set the laser's pose after construction
89  public: void SetLaserPose(pf_vector_t& laser_pose)
90  {this->laser_pose = laser_pose;}
91 
92  // Determine the probability for the given pose
93  private: static double BeamModel(AMCLLaserData *data,
94  pf_sample_set_t* set);
95  // Determine the probability for the given pose
96  private: static double LikelihoodFieldModel(AMCLLaserData *data,
97  pf_sample_set_t* set);
98 
99  private: laser_model_t model_type;
100 
101  // Current data timestamp
102  private: double time;
103 
104  // The laser map
105  private: map_t *map;
106 
107  // Laser offset relative to robot
108  private: pf_vector_t laser_pose;
109 
110  // Max beams to consider
111  private: int max_beams;
112 
113  // Laser model params
114  //
115  // Mixture params for the components of the model; must sum to 1
116  private: double z_hit;
117  private: double z_short;
118  private: double z_max;
119  private: double z_rand;
120  //
121  // Stddev of Gaussian model for laser hits.
122  private: double sigma_hit;
123  // Decay rate of exponential model for short readings.
124  private: double lambda_short;
125  // Threshold for outlier rejection (unused)
126  private: double chi_outlier;
127 };
128 
129 
130 }
131 
132 /// @endcond
133 
134 #endif