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


Page generated by Doxygen 1.8.14 for MRPT 1.5.9 Git: 690a4699f Wed Apr 15 19:29:53 2020 +0200 at mié abr 15 19:30:12 CEST 2020