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