CLogOddsGridMap2D.h

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
   |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
   |                                                                           |
   |                       http://www.mrpt.org/                                |
   |                                                                           |
   |   Copyright (C) 2005-2011  University of Malaga                           |
   |                                                                           |
   |    This software was written by the Machine Perception and Intelligent    |
   |      Robotics Lab, University of Malaga (Spain).                          |
   |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
   |                                                                           |
   |  This file is part of the MRPT project.                                   |
   |                                                                           |
   |     MRPT is free software: you can redistribute it and/or modify          |
   |     it under the terms of the GNU General Public License as published by  |
   |     the Free Software Foundation, either version 3 of the License, or     |
   |     (at your option) any later version.                                   |
   |                                                                           |
   |   MRPT is distributed in the hope that it will be useful,                 |
   |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
   |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
   |     GNU General Public License for more details.                          |
   |                                                                           |
   |     You should have received a copy of the GNU General Public License     |
   |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
   |                                                                           |
   +---------------------------------------------------------------------------+ */

#ifndef CLogOddsGridMap2D_H
#define CLogOddsGridMap2D_H

#include <mrpt/utils/utils_defs.h>

namespace mrpt
{
        namespace slam
        {
                namespace detail
                {
                        template <typename TCELL> struct logoddscell_traits;
                        // Specializations:
                        template <> struct logoddscell_traits<int8_t>
                        {
                                static const int8_t CELLTYPE_MIN  = -127; // In mrpt <0.9.4 was -128 (!) - This is to make it compatible with all architectures.
                                static const int8_t CELLTYPE_MAX  = 127;
                                static const int8_t P2LTABLE_SIZE = CELLTYPE_MAX;
                                static const size_t LOGODDS_LUT_ENTRIES = 1<<8;
                        };
                        template <> struct logoddscell_traits<int16_t>
                        {
                                static const int16_t CELLTYPE_MIN  = -32767; // In mrpt <0.9.4 was -32768 (!).
                                static const int16_t CELLTYPE_MAX  = 32767;
                                static const int16_t P2LTABLE_SIZE = CELLTYPE_MAX;
                                static const size_t  LOGODDS_LUT_ENTRIES = 1<<16;
                        };
                }

                /** A generic provider of log-odds grid-map maintainance functions.
                  *  Map cells must be type TCELL, which can be only:
                  *             - int8_t or
                  *             - int16_t
                  *
                  *  \sa CLogOddsGridMapLUT, See derived classes for usage examples.
                  * \ingroup mrpt_maps_grp
                  */
                template <typename TCELL>
                struct CLogOddsGridMap2D : public detail::logoddscell_traits<TCELL>
                {
                        typedef TCELL cell_t; //!< The type of cells
                        typedef detail::logoddscell_traits<TCELL>  traits_t;

                        /** Performs the Bayesian fusion of a new observation of a cell, without checking for grid limits nor updateInfoChangeOnly.
                          * This method increases the "occupancy-ness" of a cell, managing possible saturation.
                          *  \param x Cell index in X axis.
                          *  \param y Cell index in Y axis.
                          *  \param logodd_obs Observation of the cell, in log-odd form as transformed by p2l.
                          *  \param thres  This must be CELLTYPE_MIN+logodd_obs
                          * \sa updateCell, updateCell_fast_free
                         */
                        inline 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)
                        {
                                cell_t *theCell = mapArray + (x+y*_size_x);
                                if (*theCell > thres )
                                                *theCell -= logodd_obs;
                                else    *theCell = traits_t::CELLTYPE_MIN;
                        }

                        /** Performs the Bayesian fusion of a new observation of a cell, without checking for grid limits nor updateInfoChangeOnly.
                          * This method increases the "occupancy-ness" of a cell, managing possible saturation.
                          *  \param theCell The cell to modify
                          *  \param logodd_obs Observation of the cell, in log-odd form as transformed by p2l.
                          *  \param thres  This must be CELLTYPE_MIN+logodd_obs
                          * \sa updateCell, updateCell_fast_free
                         */
                        inline static void  updateCell_fast_occupied(
                                cell_t          *theCell,
                                const cell_t    logodd_obs,
                                const cell_t  thres )
                        {
                                if (*theCell > thres )
                                                *theCell -= logodd_obs;
                                else    *theCell = traits_t::CELLTYPE_MIN;
                        }

                        /** Performs the Bayesian fusion of a new observation of a cell, without checking for grid limits nor updateInfoChangeOnly.
                          * This method increases the "free-ness" of a cell, managing possible saturation.
                          *  \param x Cell index in X axis.
                          *  \param y Cell index in Y axis.
                          *  \param logodd_obs Observation of the cell, in log-odd form as transformed by p2l.
                          *  \param thres  This must be CELLTYPE_MAX-logodd_obs
                          * \sa updateCell_fast_occupied
                         */
                        inline 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)
                        {
                                cell_t *theCell = mapArray + (x+y*_size_x);
                                if (*theCell < thres )
                                                *theCell += logodd_obs;
                                else    *theCell = traits_t::CELLTYPE_MAX;
                        }

                        /** Performs the Bayesian fusion of a new observation of a cell, without checking for grid limits nor updateInfoChangeOnly.
                          * This method increases the "free-ness" of a cell, managing possible saturation.
                          *  \param x Cell index in X axis.
                          *  \param y Cell index in Y axis.
                          *  \param logodd_obs Observation of the cell, in log-odd form as transformed by p2l.
                          *  \param thres  This must be CELLTYPE_MAX-logodd_obs
                          * \sa updateCell_fast_occupied
                         */
                        inline static void  updateCell_fast_free(
                                cell_t          *theCell,
                                const cell_t    logodd_obs,
                                const cell_t  thres)
                        {
                                if (*theCell < thres )
                                                *theCell += logodd_obs;
                                else    *theCell = traits_t::CELLTYPE_MAX;
                        }

                };  // end of CLogOddsGridMap2D

                /** 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.
                  *  Map cells must be type TCELL, which can be only:
                  *             - int8_t or
                  *             - int16_t
                  *
                  *  \sa CLogOddsGridMap2D, see derived classes for usage examples.
                  * \ingroup mrpt_maps_grp
                  */
                template <typename TCELL>
                struct CLogOddsGridMapLUT : public detail::logoddscell_traits<TCELL>
                {
                        typedef TCELL cell_t; //!< The type of
                        typedef detail::logoddscell_traits<TCELL>  traits_t;

                        /** 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$.
                          */
                        std::vector<float>    logoddsTable;

                        /** 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$.
                          *  This is used to speed-up conversions to grayscale images.
                          */
                        std::vector<uint8_t>   logoddsTable_255;

                        /** A lookup table for passing from float to log-odds as cell_t. */
                        std::vector<cell_t>    p2lTable;

                        /** Constructor: computes all the required stuff. */
                        CLogOddsGridMapLUT()
                        {
                                MRPT_START

                                // The factor for converting log2-odds into integers:
                                static const double LOGODD_K  = 16;
                                static const double LOGODD_K_INV = 1.0/LOGODD_K;

                                logoddsTable.resize( traits_t::LOGODDS_LUT_ENTRIES );
                                logoddsTable_255.resize( traits_t::LOGODDS_LUT_ENTRIES );
                                for (int i=traits_t::CELLTYPE_MIN;i<=traits_t::CELLTYPE_MAX;i++)
                                {
                                        float f = 1.0f / (1.0f + exp( - i * LOGODD_K_INV ) );
                                        unsigned int idx =  -traits_t::CELLTYPE_MIN+i;
                                        logoddsTable[idx] = f;
                                        logoddsTable_255[idx] = (uint8_t)(f*255.0f);
                                }

                                // Build the p2lTable as well:
                                p2lTable.resize( traits_t::P2LTABLE_SIZE+1 );
                                double K = 1.0 / traits_t::P2LTABLE_SIZE;
                                for (int j=0;j<=traits_t::P2LTABLE_SIZE;j++)
                                {
                                        double p = j*K;
                                        if (p==0)
                                                p=1e-14;
                                        else if (p==1)
                                                p=1-1e-14;

                                        double logodd = log(p)-log(1-p);
                                        int   L = round(logodd * LOGODD_K);
                                        if (L>traits_t::CELLTYPE_MAX)
                                                L=traits_t::CELLTYPE_MAX;
                                        else if (L<traits_t::CELLTYPE_MIN)
                                                L=traits_t::CELLTYPE_MIN;
                                        p2lTable[j] = L;
                                }

                                MRPT_END
                        }

                        /** Scales an integer representation of the log-odd into a real valued probability in [0,1], using p=exp(l)/(1+exp(l))
                          */
                        inline float l2p(const cell_t l)
                        {
                                if (l<traits_t::CELLTYPE_MIN)   
                                     return logoddsTable[ 0 ];  // This is needed since min can be -127 and int8_t can be -128.
                                else return logoddsTable[ -traits_t::CELLTYPE_MIN+l ];
                        }

                        /** Scales an integer representation of the log-odd into a linear scale [0,255], using p=exp(l)/(1+exp(l))
                          */
                        inline uint8_t l2p_255(const cell_t l)
                        {
                                if (l<traits_t::CELLTYPE_MIN)   
                                     return logoddsTable_255[ 0 ]; // This is needed since min can be -127 and int8_t can be -128.
                                else return logoddsTable_255[ -traits_t::CELLTYPE_MIN+l ];
                        }

                        /** 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.
                          */
                        inline cell_t p2l(const float p)
                        {
                                return p2lTable[ static_cast<unsigned int>(p * traits_t::P2LTABLE_SIZE) ];
                        }

                }; // end of CLogOddsGridMap2D

        } // End of namespace
} // End of namespace

#endif