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 "../map/map.h"
39 #include "amcl_sensor.h"
40 
41 /// @cond EXTERNAL
42 
43 namespace amcl {
44 
45 typedef enum { LASER_MODEL_BEAM, LASER_MODEL_LIKELIHOOD_FIELD } laser_model_t;
46 
47 // Laser sensor data
48 class AMCLLaserData : public AMCLSensorData
49 {
50 public:
51  AMCLLaserData()
52  {
53  ranges = NULL;
54  };
55  virtual ~AMCLLaserData()
56  {
57  delete[] ranges;
58  };
59  // Laser range data (range, bearing tuples)
60 public:
61  int range_count;
62 
63 public:
64  double range_max;
65 
66 public:
67  double (*ranges)[2];
68 };
69 
70 // Laseretric sensor model
71 class AMCLLaser : public AMCLSensor
72 {
73  // Default constructor
74 public:
75  AMCLLaser(size_t max_beams, map_t *map);
76 
77 public:
78  void SetModelBeam(double z_hit,
79  double z_short,
80  double z_max,
81  double z_rand,
82  double sigma_hit,
83  double labda_short,
84  double chi_outlier);
85 
86 public:
87  void SetModelLikelihoodField(double z_hit, double z_rand, double sigma_hit, double max_occ_dist);
88 
89  // Update the filter based on the sensor model. Returns true if the
90  // filter has been updated.
91 public:
92  virtual bool UpdateSensor(pf_t *pf, AMCLSensorData *data);
93 
94  // Set the laser's pose after construction
95 public:
96  void
97  SetLaserPose(pf_vector_t &laser_pose)
98  {
99  this->laser_pose = laser_pose;
100  }
101 
102  // Determine the probability for the given pose
103 private:
104  static double BeamModel(AMCLLaserData *data, pf_sample_set_t *set);
105  // Determine the probability for the given pose
106 private:
107  static double LikelihoodFieldModel(AMCLLaserData *data, pf_sample_set_t *set);
108 
109 private:
110  laser_model_t model_type;
111 
112  // Current data timestamp
113 private:
114  double time;
115 
116  // The laser map
117 private:
118  map_t *map;
119 
120  // Laser offset relative to robot
121 private:
122  pf_vector_t laser_pose;
123 
124  // Max beams to consider
125 private:
126  int max_beams;
127 
128  // Laser model params
129  //
130  // Mixture params for the components of the model; must sum to 1
131 private:
132  double z_hit;
133 
134 private:
135  double z_short;
136 
137 private:
138  double z_max;
139 
140 private:
141  double z_rand;
142  //
143  // Stddev of Gaussian model for laser hits.
144 private:
145  double sigma_hit;
146  // Decay rate of exponential model for short readings.
147 private:
148  double lambda_short;
149  // Threshold for outlier rejection (unused)
150 private:
151  double chi_outlier;
152 };
153 
154 } // namespace amcl
155 
156 /// @endcond
157 
158 #endif