CLogOddsGridMap2D.h

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */
 
#ifndef CLogOddsGridMap2D_H
#define CLogOddsGridMap2D_H
 
#include <cstdint>
#include <vector>
 
namespace mrpt
{
namespace maps
{
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>
{
        /** The type of cells */
        using cell_t = TCELL;
        using traits_t = detail::logoddscell_traits<TCELL>;
 
        /** 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>
{
        /** The type of */
        using cell_t = TCELL;
        using traits_t = detail::logoddscell_traits<TCELL>;
 
        /** 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()
        {
                // 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;
                }
        }
 
        /** 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