Main MRPT website > C++ reference for MRPT 1.4.0
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-2016, 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
15#include <vector>
16
17namespace 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>
51 {
52 typedef TCELL cell_t; //!< The type of cells
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>
146 {
147 typedef TCELL cell_t; //!< The type of
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. */
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
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
signed int int16_t
Definition: pstdint.h:194
unsigned char uint8_t
Definition: pstdint.h:143
signed char int8_t
Definition: pstdint.h:158
A generic provider of log-odds grid-map maintainance functions.
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...
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_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...
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...
detail::logoddscell_traits< TCELL > traits_t
One static instance of this struct should exist in any class implementing CLogOddsGridMap2D to hold t...
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)...
float l2p(const cell_t l)
Scales an integer representation of the log-odd into a real valued probability in [0,...
std::vector< float > logoddsTable
A lookup table to compute occupancy probabilities in [0,1] from integer log-odds values in the cells,...
std::vector< cell_t > p2lTable
A lookup table for passing from float to log-odds as cell_t.
CLogOddsGridMapLUT()
Constructor: computes all the required stuff.
detail::logoddscell_traits< TCELL > traits_t
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...
std::vector< uint8_t > logoddsTable_255
A lookup table to compute occupancy probabilities in the range [0,255] from integer log-odds values i...



Page generated by Doxygen 1.9.6 for MRPT 1.4.0 SVN: at Wed Mar 22 06:31:24 UTC 2023