Main MRPT website > C++ reference
MRPT logo
CLogOddsGridMap2D.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 
29 #ifndef CLogOddsGridMap2D_H
30 #define CLogOddsGridMap2D_H
31 
32 #include <mrpt/utils/utils_defs.h>
33 
34 namespace mrpt
35 {
36  namespace slam
37  {
38  namespace detail
39  {
40  template <typename TCELL> struct logoddscell_traits;
41  // Specializations:
42  template <> struct logoddscell_traits<int8_t>
43  {
44  static const int8_t CELLTYPE_MIN = -127; // In mrpt <0.9.4 was -128 (!) - This is to make it compatible with all architectures.
45  static const int8_t CELLTYPE_MAX = 127;
46  static const int8_t P2LTABLE_SIZE = CELLTYPE_MAX;
47  static const size_t LOGODDS_LUT_ENTRIES = 1<<8;
48  };
49  template <> struct logoddscell_traits<int16_t>
50  {
51  static const int16_t CELLTYPE_MIN = -32767; // In mrpt <0.9.4 was -32768 (!).
52  static const int16_t CELLTYPE_MAX = 32767;
53  static const int16_t P2LTABLE_SIZE = CELLTYPE_MAX;
54  static const size_t LOGODDS_LUT_ENTRIES = 1<<16;
55  };
56  }
57 
58  /** A generic provider of log-odds grid-map maintainance functions.
59  * Map cells must be type TCELL, which can be only:
60  * - int8_t or
61  * - int16_t
62  *
63  * \sa CLogOddsGridMapLUT, See derived classes for usage examples.
64  * \ingroup mrpt_maps_grp
65  */
66  template <typename TCELL>
67  struct CLogOddsGridMap2D : public detail::logoddscell_traits<TCELL>
68  {
69  typedef TCELL cell_t; //!< The type of cells
70  typedef detail::logoddscell_traits<TCELL> traits_t;
71 
72  /** Performs the Bayesian fusion of a new observation of a cell, without checking for grid limits nor updateInfoChangeOnly.
73  * This method increases the "occupancy-ness" of a cell, managing possible saturation.
74  * \param x Cell index in X axis.
75  * \param y Cell index in Y axis.
76  * \param logodd_obs Observation of the cell, in log-odd form as transformed by p2l.
77  * \param thres This must be CELLTYPE_MIN+logodd_obs
78  * \sa updateCell, updateCell_fast_free
79  */
80  inline static void updateCell_fast_occupied(
81  const unsigned x,
82  const unsigned y,
83  const cell_t logodd_obs,
84  const cell_t thres,
85  cell_t *mapArray,
86  const unsigned _size_x)
87  {
88  cell_t *theCell = mapArray + (x+y*_size_x);
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 "occupancy-ness" of a cell, managing possible saturation.
96  * \param theCell The cell to modify
97  * \param logodd_obs Observation of the cell, in log-odd form as transformed by p2l.
98  * \param thres This must be CELLTYPE_MIN+logodd_obs
99  * \sa updateCell, updateCell_fast_free
100  */
101  inline static void updateCell_fast_occupied(
102  cell_t *theCell,
103  const cell_t logodd_obs,
104  const cell_t thres )
105  {
106  if (*theCell > thres )
107  *theCell -= logodd_obs;
108  else *theCell = traits_t::CELLTYPE_MIN;
109  }
110 
111  /** Performs the Bayesian fusion of a new observation of a cell, without checking for grid limits nor updateInfoChangeOnly.
112  * This method increases the "free-ness" of a cell, managing possible saturation.
113  * \param x Cell index in X axis.
114  * \param y Cell index in Y axis.
115  * \param logodd_obs Observation of the cell, in log-odd form as transformed by p2l.
116  * \param thres This must be CELLTYPE_MAX-logodd_obs
117  * \sa updateCell_fast_occupied
118  */
119  inline static void updateCell_fast_free(
120  const unsigned x,
121  const unsigned y,
122  const cell_t logodd_obs,
123  const cell_t thres,
124  cell_t *mapArray,
125  const unsigned _size_x)
126  {
127  cell_t *theCell = mapArray + (x+y*_size_x);
128  if (*theCell < thres )
129  *theCell += logodd_obs;
130  else *theCell = traits_t::CELLTYPE_MAX;
131  }
132 
133  /** Performs the Bayesian fusion of a new observation of a cell, without checking for grid limits nor updateInfoChangeOnly.
134  * This method increases the "free-ness" of a cell, managing possible saturation.
135  * \param x Cell index in X axis.
136  * \param y Cell index in Y axis.
137  * \param logodd_obs Observation of the cell, in log-odd form as transformed by p2l.
138  * \param thres This must be CELLTYPE_MAX-logodd_obs
139  * \sa updateCell_fast_occupied
140  */
141  inline static void updateCell_fast_free(
142  cell_t *theCell,
143  const cell_t logodd_obs,
144  const cell_t thres)
145  {
146  if (*theCell < thres )
147  *theCell += logodd_obs;
148  else *theCell = traits_t::CELLTYPE_MAX;
149  }
150 
151  }; // end of CLogOddsGridMap2D
152 
153  /** 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.
154  * Map cells must be type TCELL, which can be only:
155  * - int8_t or
156  * - int16_t
157  *
158  * \sa CLogOddsGridMap2D, see derived classes for usage examples.
159  * \ingroup mrpt_maps_grp
160  */
161  template <typename TCELL>
162  struct CLogOddsGridMapLUT : public detail::logoddscell_traits<TCELL>
163  {
164  typedef TCELL cell_t; //!< The type of
165  typedef detail::logoddscell_traits<TCELL> traits_t;
166 
167  /** 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$.
168  */
169  std::vector<float> logoddsTable;
170 
171  /** 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$.
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. */
180  CLogOddsGridMapLUT()
181  {
182  MRPT_START
183 
184  // The factor for converting log2-odds into integers:
185  static const double LOGODD_K = 16;
186  static const double LOGODD_K_INV = 1.0/LOGODD_K;
187 
188  logoddsTable.resize( traits_t::LOGODDS_LUT_ENTRIES );
189  logoddsTable_255.resize( traits_t::LOGODDS_LUT_ENTRIES );
190  for (int i=traits_t::CELLTYPE_MIN;i<=traits_t::CELLTYPE_MAX;i++)
191  {
192  float f = 1.0f / (1.0f + exp( - i * LOGODD_K_INV ) );
193  unsigned int idx = -traits_t::CELLTYPE_MIN+i;
194  logoddsTable[idx] = f;
195  logoddsTable_255[idx] = (uint8_t)(f*255.0f);
196  }
197 
198  // Build the p2lTable as well:
199  p2lTable.resize( traits_t::P2LTABLE_SIZE+1 );
200  double K = 1.0 / traits_t::P2LTABLE_SIZE;
201  for (int j=0;j<=traits_t::P2LTABLE_SIZE;j++)
202  {
203  double p = j*K;
204  if (p==0)
205  p=1e-14;
206  else if (p==1)
207  p=1-1e-14;
208 
209  double logodd = log(p)-log(1-p);
210  int L = round(logodd * LOGODD_K);
211  if (L>traits_t::CELLTYPE_MAX)
212  L=traits_t::CELLTYPE_MAX;
213  else if (L<traits_t::CELLTYPE_MIN)
214  L=traits_t::CELLTYPE_MIN;
215  p2lTable[j] = L;
216  }
217 
218  MRPT_END
219  }
220 
221  /** Scales an integer representation of the log-odd into a real valued probability in [0,1], using p=exp(l)/(1+exp(l))
222  */
223  inline float l2p(const cell_t l)
224  {
225  if (l<traits_t::CELLTYPE_MIN)
226  return logoddsTable[ 0 ]; // This is needed since min can be -127 and int8_t can be -128.
227  else return logoddsTable[ -traits_t::CELLTYPE_MIN+l ];
228  }
229 
230  /** Scales an integer representation of the log-odd into a linear scale [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 and int8_t can be -128.
236  else return logoddsTable_255[ -traits_t::CELLTYPE_MIN+l ];
237  }
238 
239  /** 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.
240  */
241  inline cell_t p2l(const float p)
242  {
243  return p2lTable[ static_cast<unsigned int>(p * traits_t::P2LTABLE_SIZE) ];
244  }
245 
246  }; // end of CLogOddsGridMap2D
247 
248  } // End of namespace
249 } // End of namespace
250 
251 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013