Main MRPT website > C++ reference for MRPT 1.9.9
CLogOddsGridMap2D.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #ifndef CLogOddsGridMap2D_H
11 #define CLogOddsGridMap2D_H
12 
13 #include <cstdint>
14 #include <vector>
15 
16 namespace mrpt
17 {
18 namespace maps
19 {
20 namespace detail
21 {
22 template <typename TCELL>
24 // Specializations:
25 template <>
27 {
28  static const int8_t CELLTYPE_MIN = -127; // In mrpt <0.9.4 was -128 (!) -
29  // This is to make it compatible
30  // with all architectures.
31  static const int8_t CELLTYPE_MAX = 127;
32  static const int8_t P2LTABLE_SIZE = CELLTYPE_MAX;
33  static const size_t LOGODDS_LUT_ENTRIES = 1 << 8;
34 };
35 template <>
37 {
38  static const int16_t CELLTYPE_MIN =
39  -32767; // In mrpt <0.9.4 was -32768 (!).
40  static const int16_t CELLTYPE_MAX = 32767;
41  static const int16_t P2LTABLE_SIZE = CELLTYPE_MAX;
42  static const size_t LOGODDS_LUT_ENTRIES = 1 << 16;
43 };
44 }
45 
46 /** A generic provider of log-odds grid-map maintainance functions.
47  * Map cells must be type TCELL, which can be only:
48  * - int8_t or
49  * - int16_t
50  *
51  * \sa CLogOddsGridMapLUT, See derived classes for usage examples.
52  * \ingroup mrpt_maps_grp
53  */
54 template <typename TCELL>
56 {
57  /** The type of cells */
58  using cell_t = TCELL;
60 
61  /** Performs the Bayesian fusion of a new observation of a cell, without
62  * checking for grid limits nor updateInfoChangeOnly.
63  * This method increases the "occupancy-ness" of a cell, managing possible
64  * saturation.
65  * \param x Cell index in X axis.
66  * \param y Cell index in Y axis.
67  * \param logodd_obs Observation of the cell, in log-odd form as
68  * transformed by p2l.
69  * \param thres This must be CELLTYPE_MIN+logodd_obs
70  * \sa updateCell, updateCell_fast_free
71  */
72  inline static void updateCell_fast_occupied(
73  const unsigned x, const unsigned y, const cell_t logodd_obs,
74  const cell_t thres, cell_t* mapArray, const unsigned _size_x)
75  {
76  cell_t* theCell = mapArray + (x + y * _size_x);
77  if (*theCell > thres)
78  *theCell -= logodd_obs;
79  else
80  *theCell = traits_t::CELLTYPE_MIN;
81  }
82 
83  /** Performs the Bayesian fusion of a new observation of a cell, without
84  * checking for grid limits nor updateInfoChangeOnly.
85  * This method increases the "occupancy-ness" of a cell, managing possible
86  * saturation.
87  * \param theCell The cell to modify
88  * \param logodd_obs Observation of the cell, in log-odd form as
89  * transformed by p2l.
90  * \param thres This must be CELLTYPE_MIN+logodd_obs
91  * \sa updateCell, updateCell_fast_free
92  */
93  inline static void updateCell_fast_occupied(
94  cell_t* theCell, const cell_t logodd_obs, const cell_t thres)
95  {
96  if (*theCell > thres)
97  *theCell -= logodd_obs;
98  else
99  *theCell = traits_t::CELLTYPE_MIN;
100  }
101 
102  /** Performs the Bayesian fusion of a new observation of a cell, without
103  * checking for grid limits nor updateInfoChangeOnly.
104  * This method increases the "free-ness" of a cell, managing possible
105  * saturation.
106  * \param x Cell index in X axis.
107  * \param y Cell index in Y axis.
108  * \param logodd_obs Observation of the cell, in log-odd form as
109  * transformed by p2l.
110  * \param thres This must be CELLTYPE_MAX-logodd_obs
111  * \sa updateCell_fast_occupied
112  */
113  inline static void updateCell_fast_free(
114  const unsigned x, const unsigned y, const cell_t logodd_obs,
115  const cell_t thres, cell_t* mapArray, const unsigned _size_x)
116  {
117  cell_t* theCell = mapArray + (x + y * _size_x);
118  if (*theCell < thres)
119  *theCell += logodd_obs;
120  else
121  *theCell = traits_t::CELLTYPE_MAX;
122  }
123 
124  /** Performs the Bayesian fusion of a new observation of a cell, without
125  * checking for grid limits nor updateInfoChangeOnly.
126  * This method increases the "free-ness" of a cell, managing possible
127  * saturation.
128  * \param x Cell index in X axis.
129  * \param y Cell index in Y axis.
130  * \param logodd_obs Observation of the cell, in log-odd form as
131  * transformed by p2l.
132  * \param thres This must be CELLTYPE_MAX-logodd_obs
133  * \sa updateCell_fast_occupied
134  */
135  inline static void updateCell_fast_free(
136  cell_t* theCell, const cell_t logodd_obs, const cell_t thres)
137  {
138  if (*theCell < thres)
139  *theCell += logodd_obs;
140  else
141  *theCell = traits_t::CELLTYPE_MAX;
142  }
143 
144 }; // end of CLogOddsGridMap2D
145 
146 /** One static instance of this struct should exist in any class implementing
147  *CLogOddsGridMap2D to hold the Look-up-tables (LUTs) for log-odss Bayesian
148  *update.
149  * Map cells must be type TCELL, which can be only:
150  * - int8_t or
151  * - int16_t
152  *
153  * \sa CLogOddsGridMap2D, see derived classes for usage examples.
154  * \ingroup mrpt_maps_grp
155  */
156 template <typename TCELL>
158 {
159  /** The type of */
160  using cell_t = TCELL;
162 
163  /** A lookup table to compute occupancy probabilities in [0,1] from integer
164  * log-odds values in the cells, using \f$ p(m_{xy}) =
165  * \frac{1}{1+exp(-log_odd)} \f$.
166  */
167  std::vector<float> logoddsTable;
168 
169  /** A lookup table to compute occupancy probabilities in the range [0,255]
170  * from integer log-odds values in the cells, using \f$ p(m_{xy}) =
171  * \frac{1}{1+exp(-log_odd)} \f$.
172  * This is used to speed-up conversions to grayscale images.
173  */
174  std::vector<uint8_t> logoddsTable_255;
175 
176  /** A lookup table for passing from float to log-odds as cell_t. */
177  std::vector<cell_t> p2lTable;
178 
179  /** Constructor: computes all the required stuff. */
181  {
182  // The factor for converting log2-odds into integers:
183  static const double LOGODD_K = 16;
184  static const double LOGODD_K_INV = 1.0 / LOGODD_K;
185 
186  logoddsTable.resize(traits_t::LOGODDS_LUT_ENTRIES);
187  logoddsTable_255.resize(traits_t::LOGODDS_LUT_ENTRIES);
188  for (int i = traits_t::CELLTYPE_MIN; i <= traits_t::CELLTYPE_MAX; i++)
189  {
190  float f = 1.0f / (1.0f + exp(-i * LOGODD_K_INV));
191  unsigned int idx = -traits_t::CELLTYPE_MIN + i;
192  logoddsTable[idx] = f;
193  logoddsTable_255[idx] = (uint8_t)(f * 255.0f);
194  }
195 
196  // Build the p2lTable as well:
197  p2lTable.resize(traits_t::P2LTABLE_SIZE + 1);
198  double K = 1.0 / traits_t::P2LTABLE_SIZE;
199  for (int j = 0; j <= traits_t::P2LTABLE_SIZE; j++)
200  {
201  double p = j * K;
202  if (p == 0)
203  p = 1e-14;
204  else if (p == 1)
205  p = 1 - 1e-14;
206 
207  double logodd = log(p) - log(1 - p);
208  int L = round(logodd * LOGODD_K);
209  if (L > traits_t::CELLTYPE_MAX)
210  L = traits_t::CELLTYPE_MAX;
211  else if (L < traits_t::CELLTYPE_MIN)
212  L = traits_t::CELLTYPE_MIN;
213  p2lTable[j] = L;
214  }
215  }
216 
217  /** Scales an integer representation of the log-odd into a real valued
218  * probability in [0,1], using p=exp(l)/(1+exp(l))
219  */
220  inline float l2p(const cell_t l)
221  {
222  if (l < traits_t::CELLTYPE_MIN)
223  return logoddsTable[0]; // This is needed since min can be -127 and
224  // int8_t can be -128.
225  else
226  return logoddsTable[-traits_t::CELLTYPE_MIN + l];
227  }
228 
229  /** Scales an integer representation of the log-odd into a linear scale
230  * [0,255], using p=exp(l)/(1+exp(l))
231  */
232  inline uint8_t l2p_255(const cell_t l)
233  {
234  if (l < traits_t::CELLTYPE_MIN)
235  return logoddsTable_255[0]; // This is needed since min can be -127
236  // and int8_t can be -128.
237  else
238  return logoddsTable_255[-traits_t::CELLTYPE_MIN + l];
239  }
240 
241  /** Scales a real valued probability in [0,1] to an integer representation
242  * of: log(p)-log(1-p) in the valid range of cell_t.
243  */
244  inline cell_t p2l(const float p)
245  {
246  return p2lTable[static_cast<unsigned int>(p * traits_t::P2LTABLE_SIZE)];
247  }
248 
249 }; // end of CLogOddsGridMap2D
250 
251 } // End of namespace
252 } // End of namespace
253 
254 #endif
signed char int8_t
Definition: rptypes.h:40
std::vector< uint8_t > logoddsTable_255
A lookup table to compute occupancy probabilities in the range [0,255] from integer log-odds values i...
unsigned char uint8_t
Definition: rptypes.h:41
cell_t p2l(const float p)
Scales a real valued probability in [0,1] to an integer representation of: log(p)-log(1-p) in the val...
__int16 int16_t
Definition: rptypes.h:43
static void updateCell_fast_free(const unsigned x, const unsigned y, const cell_t logodd_obs, const cell_t thres, cell_t *mapArray, const unsigned _size_x)
Performs the Bayesian fusion of a new observation of a cell, without checking for grid limits nor upd...
CLogOddsGridMapLUT()
Constructor: computes all the required stuff.
std::vector< cell_t > p2lTable
A lookup table for passing from float to log-odds as cell_t.
std::vector< float > logoddsTable
A lookup table to compute occupancy probabilities in [0,1] from integer log-odds values in the cells...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
uint8_t l2p_255(const cell_t l)
Scales an integer representation of the log-odd into a linear scale [0,255], using p=exp(l)/(1+exp(l)...
GLenum GLint GLint y
Definition: glext.h:3538
A generic provider of log-odds grid-map maintainance functions.
GLenum GLint x
Definition: glext.h:3538
float l2p(const cell_t l)
Scales an integer representation of the log-odd into a real valued probability in [0...
static void updateCell_fast_free(cell_t *theCell, const cell_t logodd_obs, const cell_t thres)
Performs the Bayesian fusion of a new observation of a cell, without checking for grid limits nor upd...
GLfloat GLfloat p
Definition: glext.h:6305
One static instance of this struct should exist in any class implementing CLogOddsGridMap2D to hold t...
static void updateCell_fast_occupied(cell_t *theCell, const cell_t logodd_obs, const cell_t thres)
Performs the Bayesian fusion of a new observation of a cell, without checking for grid limits nor upd...
static void updateCell_fast_occupied(const unsigned x, const unsigned y, const cell_t logodd_obs, const cell_t thres, cell_t *mapArray, const unsigned _size_x)
Performs the Bayesian fusion of a new observation of a cell, without checking for grid limits nor upd...
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:23



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019