COccupancyGridMap2D_insert.cpp

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 |
   +------------------------------------------------------------------------+ */

#include "maps-precomp.h"  // Precomp header

#include <mrpt/maps/COccupancyGridMap2D.h>
#include <mrpt/obs/CObservation2DRangeScan.h>
#include <mrpt/obs/CObservationRange.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/core/round.h>  // round()
#include <mrpt/system/memory.h>  // alloca()

#if HAVE_ALLOCA_H
#include <alloca.h>
#endif

using namespace mrpt;
using namespace mrpt::maps;
using namespace mrpt::obs;
using namespace mrpt::math;
using namespace mrpt::poses;
using namespace std;

/** Local stucture used in the next method (must be here for usage within STL
 * stuff) */
struct TLocalPoint
{
    float x, y;
    int cx, cy;
};

/*---------------------------------------------------------------
                    insertObservation

Insert the observation information into this map.
 ---------------------------------------------------------------*/
bool COccupancyGridMap2D::internal_insertObservation(
    const CObservation* obs, const CPose3D* robotPose)
{
    //  MRPT_START   // Avoid "try" since we use "alloca"

#define FRBITS 9

    CPose2D robotPose2D;
    CPose3D robotPose3D;

    // This is required to indicate the grid map has changed!
    // resetFeaturesCache();
    // For the precomputed likelihood trick:
    precomputedLikelihoodToBeRecomputed = true;

    if (robotPose)
    {
        robotPose2D = CPose2D(*robotPose);
        robotPose3D = (*robotPose);
    }
    else
    {
        // Default values are (0,0,0)
    }

    // the occupied and free probabilities:
    const float maxCertainty = insertionOptions.maxOccupancyUpdateCertainty;
    float maxFreeCertainty = insertionOptions.maxFreenessUpdateCertainty;
    if (maxFreeCertainty == .0f) maxFreeCertainty = maxCertainty;
    float maxFreeCertaintyNoEcho = insertionOptions.maxFreenessInvalidRanges;
    if (maxFreeCertaintyNoEcho == .0f) maxFreeCertaintyNoEcho = maxCertainty;

    cellType logodd_observation_free =
        std::max<cellType>(1, p2l(maxFreeCertainty));
    cellType logodd_observation_occupied =
        3 * std::max<cellType>(1, p2l(maxCertainty));
    cellType logodd_noecho_free =
        std::max<cellType>(1, p2l(maxFreeCertaintyNoEcho));

    // saturation limits:
    cellType logodd_thres_occupied =
        OCCGRID_CELLTYPE_MIN + logodd_observation_occupied;
    cellType logodd_thres_free =
        OCCGRID_CELLTYPE_MAX -
        std::max(logodd_noecho_free, logodd_observation_free);

    if (CLASS_ID(CObservation2DRangeScan) == obs->GetRuntimeClass())
    {
        /********************************************************************

                    OBSERVATION TYPE: CObservation2DRangeScan

            ********************************************************************/
        const CObservation2DRangeScan* o =
            static_cast<const CObservation2DRangeScan*>(obs);
        CPose3D sensorPose3D = robotPose3D + o->sensorPose;
        CPose2D laserPose(sensorPose3D);

        // Insert only HORIZONTAL scans, since the grid is supposed to
        //  be a horizontal representation of space.
        bool reallyInsert =
            o->isPlanarScan(insertionOptions.horizontalTolerance);
        unsigned int decimation = insertionOptions.decimation;

        // Check the altitude of the map (if feature enabled!)
        if (insertionOptions.useMapAltitude &&
            fabs(insertionOptions.mapAltitude - sensorPose3D.z()) > 0.001)
        {
            reallyInsert = false;
        }

        // Manage horizontal scans, but with the sensor bottom-up:
        //  Use the z-axis direction of the transformed Z axis of the sensor
        //  coordinates:
        bool sensorIsBottomwards =
            sensorPose3D.getHomogeneousMatrixVal<CMatrixDouble44>().get_unsafe(
                2, 2) < 0;

        if (reallyInsert)
        {
            // ---------------------------------------------
            //      Insert the scan as simple rays:
            // ---------------------------------------------
            int cx, cy, N = o->scan.size();
            float px, py;
            double A, dAK;

            // Parameters values:
            const float maxDistanceInsertion =
                insertionOptions.maxDistanceInsertion;
            const bool invalidAsFree =
                insertionOptions.considerInvalidRangesAsFreeSpace;
            float new_x_max, new_x_min;
            float new_y_max, new_y_min;
            float last_valid_range = maxDistanceInsertion;

            int K = updateInfoChangeOnly.enabled
                        ? updateInfoChangeOnly.laserRaysSkip
                        : decimation;
            size_t idx, nRanges = o->scan.size();
            float curRange = 0;

            // Start position:
            px = laserPose.x();
            py = laserPose.y();

#if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
            MRPT_CHECK_NORMAL_NUMBER(px);
            MRPT_CHECK_NORMAL_NUMBER(py);
#endif

            // Here we go! Now really insert changes in the grid:
            if (!insertionOptions.wideningBeamsWithDistance)
            {
                // Method: Simple rays:
                // -------------------------------------

                // Reserve a temporary block of memory on the stack with
                // "alloca": this memory has NOT to be deallocated,
                //  so it's ideal for an efficient, small buffer:
                float* scanPoints_x =
                    (float*)mrpt_alloca(sizeof(float) * nRanges);
                float* scanPoints_y =
                    (float*)mrpt_alloca(sizeof(float) * nRanges);

                float *scanPoint_x, *scanPoint_y;

                if (o->rightToLeft ^ sensorIsBottomwards)
                {
                    A = laserPose.phi() - 0.5 * o->aperture;
                    dAK = K * o->aperture / N;
                }
                else
                {
                    A = laserPose.phi() + 0.5 * o->aperture;
                    dAK = -K * o->aperture / N;
                }

                new_x_max = -(numeric_limits<float>::max)();
                new_x_min = (numeric_limits<float>::max)();
                new_y_max = -(numeric_limits<float>::max)();
                new_y_min = (numeric_limits<float>::max)();

                for (idx = 0, scanPoint_x = scanPoints_x,
                    scanPoint_y = scanPoints_y;
                     idx < nRanges; idx += K, scanPoint_x++, scanPoint_y++)
                {
                    if (o->validRange[idx])
                    {
                        curRange = o->scan[idx];
                        float R = min(maxDistanceInsertion, curRange);

                        *scanPoint_x = px + cos(A) * R;
                        *scanPoint_y = py + sin(A) * R;
                        last_valid_range = curRange;
                    }
                    else
                    {
                        if (invalidAsFree)
                        {
                            // Invalid range:
                            float R = min(
                                maxDistanceInsertion, 0.5f * last_valid_range);
                            *scanPoint_x = px + cos(A) * R;
                            *scanPoint_y = py + sin(A) * R;
                        }
                        else
                        {
                            *scanPoint_x = px;
                            *scanPoint_y = py;
                        }
                    }
                    A += dAK;

                    // Asjust size (will not change if not required):
                    new_x_max = max(new_x_max, *scanPoint_x);
                    new_x_min = min(new_x_min, *scanPoint_x);
                    new_y_max = max(new_y_max, *scanPoint_y);
                    new_y_min = min(new_y_min, *scanPoint_y);
                }

                // Add an extra margin:
                float securMargen = 15 * resolution;

                if (new_x_max > x_max - securMargen)
                    new_x_max += 2 * securMargen;
                else
                    new_x_max = x_max;
                if (new_x_min < x_min + securMargen)
                    new_x_min -= 2;
                else
                    new_x_min = x_min;

                if (new_y_max > y_max - securMargen)
                    new_y_max += 2 * securMargen;
                else
                    new_y_max = y_max;
                if (new_y_min < y_min + securMargen)
                    new_y_min -= 2;
                else
                    new_y_min = y_min;

                // -----------------------
                //   Resize to make room:
                // -----------------------
                resizeGrid(new_x_min, new_x_max, new_y_min, new_y_max, 0.5);

                // For updateCell_fast methods:
                cellType* theMapArray = &map[0];
                unsigned theMapSize_x = size_x;

                int cx0 =
                    x2idx(px);  // Remember: This must be after the resizeGrid!!
                int cy0 = y2idx(py);

                // Insert rays:
                for (idx = 0; idx < nRanges; idx += K)
                {
                    if (!o->validRange[idx] && !invalidAsFree) continue;

                    // Starting position: Laser position
                    cx = cx0;
                    cy = cy0;

                    // Target, in cell indexes:
                    int trg_cx = x2idx(scanPoints_x[idx]);
                    int trg_cy = y2idx(scanPoints_y[idx]);

                    // The x> comparison implicitly holds if x<0
                    ASSERT_(
                        static_cast<unsigned int>(trg_cx) < size_x &&
                        static_cast<unsigned int>(trg_cy) < size_y);

                    // Use "fractional integers" to approximate float operations
                    //  during the ray tracing:
                    int Acx = trg_cx - cx;
                    int Acy = trg_cy - cy;

                    int Acx_ = abs(Acx);
                    int Acy_ = abs(Acy);

                    int nStepsRay = max(Acx_, Acy_);
                    if (!nStepsRay) continue;  // May be...

                    // Integers store "float values * 128"
                    float N_1 = 1.0f / nStepsRay;  // Avoid division twice.

                    // Increments at each raytracing step:
                    int frAcx =
                        (Acx < 0 ? -1 : +1) * round((Acx_ << FRBITS) * N_1);
                    int frAcy =
                        (Acy < 0 ? -1 : +1) * round((Acy_ << FRBITS) * N_1);

                    int frCX = cx << FRBITS;
                    int frCY = cy << FRBITS;
                    const auto logodd_free = o->validRange[idx]
                                                 ? logodd_observation_free
                                                 : logodd_noecho_free;

                    for (int nStep = 0; nStep < nStepsRay; nStep++)
                    {
                        updateCell_fast_free(
                            cx, cy, logodd_free, logodd_thres_free, theMapArray,
                            theMapSize_x);

                        frCX += frAcx;
                        frCY += frAcy;

                        cx = frCX >> FRBITS;
                        cy = frCY >> FRBITS;
                    }

                    // And finally, the occupied cell at the end:
                    // Only if:
                    //  - It was a valid ray, and
                    //  - The ray was not truncated
                    if (o->validRange[idx] &&
                        o->scan[idx] < maxDistanceInsertion)
                        updateCell_fast_occupied(
                            trg_cx, trg_cy, logodd_observation_occupied,
                            logodd_thres_occupied, theMapArray, theMapSize_x);

                }  // End of each range

                mrpt_alloca_free(scanPoints_x);
                mrpt_alloca_free(scanPoints_y);

            }  // end insert with simple rays
            else
            {
                // ---------------------------------
                //          Widen rays
                // Algorithm in: http://www.mrpt.org/Occupancy_Grids
                // ---------------------------------
                if (o->rightToLeft ^ sensorIsBottomwards)
                {
                    A = laserPose.phi() - 0.5 * o->aperture;
                    dAK = K * o->aperture / N;
                }
                else
                {
                    A = laserPose.phi() + 0.5 * o->aperture;
                    dAK = -K * o->aperture / N;
                }

                new_x_max = -(numeric_limits<float>::max)();
                new_x_min = (numeric_limits<float>::max)();
                new_y_max = -(numeric_limits<float>::max)();
                new_y_min = (numeric_limits<float>::max)();

                last_valid_range = maxDistanceInsertion;
                for (idx = 0; idx < nRanges; idx += K)
                {
                    float scanPoint_x, scanPoint_y;
                    if (o->validRange[idx])
                    {
                        curRange = o->scan[idx];
                        float R = min(maxDistanceInsertion, curRange);

                        scanPoint_x = px + cos(A) * R;
                        scanPoint_y = py + sin(A) * R;
                        last_valid_range = curRange;
                    }
                    else
                    {
                        if (invalidAsFree)
                        {
                            // Invalid range:
                            float R = min(
                                maxDistanceInsertion, 0.5f * last_valid_range);
                            scanPoint_x = px + cos(A) * R;
                            scanPoint_y = py + sin(A) * R;
                        }
                        else
                        {
                            scanPoint_x = px;
                            scanPoint_y = py;
                        }
                    }
                    A += dAK;

                    // Asjust size (will not change if not required):
                    new_x_max = max(new_x_max, scanPoint_x);
                    new_x_min = min(new_x_min, scanPoint_x);
                    new_y_max = max(new_y_max, scanPoint_y);
                    new_y_min = min(new_y_min, scanPoint_y);
                }

                // Add an extra margin:
                float securMargen = 15 * resolution;

                if (new_x_max > x_max - securMargen)
                    new_x_max += 2 * securMargen;
                else
                    new_x_max = x_max;
                if (new_x_min < x_min + securMargen)
                    new_x_min -= 2;
                else
                    new_x_min = x_min;

                if (new_y_max > y_max - securMargen)
                    new_y_max += 2 * securMargen;
                else
                    new_y_max = y_max;
                if (new_y_min < y_min + securMargen)
                    new_y_min -= 2;
                else
                    new_y_min = y_min;

                // -----------------------
                //   Resize to make room:
                // -----------------------
                resizeGrid(new_x_min, new_x_max, new_y_min, new_y_max, 0.5);

                // For updateCell_fast methods:
                cellType* theMapArray = &map[0];
                unsigned theMapSize_x = size_x;

                // int  cx0 = x2idx(px);        // Remember: This must be after
                // the
                // resizeGrid!!
                // int  cy0 = y2idx(py);

                // Now go and insert the triangles of each beam:
                // -----------------------------------------------
                if (o->rightToLeft ^ sensorIsBottomwards)
                {
                    A = laserPose.phi() - 0.5 * o->aperture;
                    dAK = K * o->aperture / N;
                }
                else
                {
                    A = laserPose.phi() + 0.5 * o->aperture;
                    dAK = -K * o->aperture / N;
                }

                // Insert the rays:
                // ------------------------------------------
                // Vertices of the triangle: In meters
                TLocalPoint P0, P1, P2, P1b;

                last_valid_range = maxDistanceInsertion;

                const double dA_2 = 0.5 * o->aperture / N;
                for (idx = 0; idx < nRanges; idx += K, A += dAK)
                {
                    float theR;  // The range of this beam
                    if (o->validRange[idx])
                    {
                        curRange = o->scan[idx];
                        last_valid_range = curRange;
                        theR = min(maxDistanceInsertion, curRange);
                    }
                    else
                    {
                        // Invalid range:
                        if (invalidAsFree)
                        {
                            theR = min(
                                maxDistanceInsertion, 0.5f * last_valid_range);
                        }
                        else
                            continue;  // Nothing to do
                    }
                    if (theR < resolution)
                        continue;  // Range must be larger than a cell...
                    theR -= resolution;  // Remove one cell of length, which
                    // will be filled with "occupied"
                    // later.

                    /* ---------------------------------------------------------
                          Fill one triangle with vertices: P0,P1,P2
                       ---------------------------------------------------------
                       */
                    P0.x = px;
                    P0.y = py;

                    P1.x = px + cos(A - dA_2) * theR;
                    P1.y = py + sin(A - dA_2) * theR;

                    P2.x = px + cos(A + dA_2) * theR;
                    P2.y = py + sin(A + dA_2) * theR;

                    // Order the vertices by the "y": P0->bottom, P2: top
                    if (P2.y < P1.y) std::swap(P2, P1);
                    if (P2.y < P0.y) std::swap(P2, P0);
                    if (P1.y < P0.y) std::swap(P1, P0);

                    // In cell indexes:
                    P0.cx = x2idx(P0.x);
                    P0.cy = y2idx(P0.y);
                    P1.cx = x2idx(P1.x);
                    P1.cy = y2idx(P1.y);
                    P2.cx = x2idx(P2.x);
                    P2.cy = y2idx(P2.y);

#if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
                    // The x> comparison implicitly holds if x<0
                    ASSERT_(
                        static_cast<unsigned int>(P0.cx) < size_x &&
                        static_cast<unsigned int>(P0.cy) < size_y);
                    ASSERT_(
                        static_cast<unsigned int>(P1.cx) < size_x &&
                        static_cast<unsigned int>(P1.cy) < size_y);
                    ASSERT_(
                        static_cast<unsigned int>(P2.cx) < size_x &&
                        static_cast<unsigned int>(P2.cy) < size_y);
#endif

                    struct
                    {
                        int frX, frY;
                        int cx, cy;
                    } R1, R2;  // Fractional coords of the two rays:

                    // Special case: one single row
                    if (P0.cy == P2.cy && P0.cy == P1.cy)
                    {
                        // Optimized case:
                        int min_cx = min3(P0.cx, P1.cx, P2.cx);
                        int max_cx = max3(P0.cx, P1.cx, P2.cx);

                        for (int ccx = min_cx; ccx <= max_cx; ccx++)
                            updateCell_fast_free(
                                ccx, P0.cy, logodd_observation_free,
                                logodd_thres_free, theMapArray, theMapSize_x);
                    }
                    else
                    {
                        // The intersection point P1b in the segment P0-P2 at
                        // the "y" of P1:
                        P1b.y = P1.y;
                        P1b.x = P0.x +
                                (P1.y - P0.y) * (P2.x - P0.x) / (P2.y - P0.y);

                        P1b.cx = x2idx(P1b.x);
                        P1b.cy = y2idx(P1b.y);

                        // Use "fractional integers" to approximate float
                        // operations during the ray tracing:
                        // Integers store "float values * 128"
                        const int Acx01 = P1.cx - P0.cx;
                        const int Acy01 = P1.cy - P0.cy;
                        const int Acx01b = P1b.cx - P0.cx;
                        // const int Acy01b = P1b.cy - P0.cy;  // = Acy01

                        // Increments at each raytracing step:
                        const float inv_N_01 =
                            1.0f / (max3(abs(Acx01), abs(Acy01), abs(Acx01b)) +
                                    1);  // Number of steps ^ -1
                        const int frAcx01 = round(
                            (Acx01 << FRBITS) * inv_N_01);  //  Acx*128 / N
                        const int frAcy01 = round(
                            (Acy01 << FRBITS) * inv_N_01);  //  Acy*128 / N
                        const int frAcx01b = round(
                            (Acx01b << FRBITS) * inv_N_01);  //  Acx*128 / N

                        // ------------------------------------
                        // First sub-triangle: P0-P1-P1b
                        // ------------------------------------
                        R1.cx = P0.cx;
                        R1.cy = P0.cy;
                        R1.frX = P0.cx << FRBITS;
                        R1.frY = P0.cy << FRBITS;

                        int frAx_R1 = 0, frAx_R2 = 0;  //, frAy_R2;
                        int frAy_R1 = frAcy01;

                        // Start R1=R2 = P0... unlesss P0.cy == P1.cy, i.e.
                        // there is only one row:
                        if (P0.cy != P1.cy)
                        {
                            R2 = R1;
                            //  R1 & R2 follow the edges: P0->P1  & P0->P1b
                            //  R1 is forced to be at the left hand:
                            if (P1.x < P1b.x)
                            {
                                // R1: P0->P1
                                frAx_R1 = frAcx01;
                                frAx_R2 = frAcx01b;
                            }
                            else
                            {
                                // R1: P0->P1b
                                frAx_R1 = frAcx01b;
                                frAx_R2 = frAcx01;
                            }
                        }
                        else
                        {
                            R2.cx = P1.cx;
                            R2.cy = P1.cy;
                            R2.frX = P1.cx << FRBITS;
                            // R2.frY = P1.cy << FRBITS;
                        }

                        int last_insert_cy = -1;
                        // int last_insert_cx = -1;
                        do
                        {
                            if (last_insert_cy !=
                                R1.cy)  // || last_insert_cx!=R1.cx)
                            {
                                last_insert_cy = R1.cy;
                                //  last_insert_cx = R1.cx;

                                for (int ccx = R1.cx; ccx <= R2.cx; ccx++)
                                    updateCell_fast_free(
                                        ccx, R1.cy, logodd_observation_free,
                                        logodd_thres_free, theMapArray,
                                        theMapSize_x);
                            }

                            R1.frX += frAx_R1;
                            R1.frY += frAy_R1;
                            R2.frX += frAx_R2;  // R1.frY += frAcy01;

                            R1.cx = R1.frX >> FRBITS;
                            R1.cy = R1.frY >> FRBITS;
                            R2.cx = R2.frX >> FRBITS;
                        } while (R1.cy < P1.cy);

                        // ------------------------------------
                        // Second sub-triangle: P1-P1b-P2
                        // ------------------------------------

                        // Use "fractional integers" to approximate float
                        // operations during the ray tracing:
                        // Integers store "float values * 128"
                        const int Acx12 = P2.cx - P1.cx;
                        const int Acy12 = P2.cy - P1.cy;
                        const int Acx1b2 = P2.cx - P1b.cx;
                        // const int Acy1b2 = Acy12

                        // Increments at each raytracing step:
                        const float inv_N_12 =
                            1.0f / (max3(abs(Acx12), abs(Acy12), abs(Acx1b2)) +
                                    1);  // Number of steps ^ -1
                        const int frAcx12 = round(
                            (Acx12 << FRBITS) * inv_N_12);  //  Acx*128 / N
                        const int frAcy12 = round(
                            (Acy12 << FRBITS) * inv_N_12);  //  Acy*128 / N
                        const int frAcx1b2 = round(
                            (Acx1b2 << FRBITS) * inv_N_12);  //  Acx*128 / N

                        // struct { int frX,frY; int cx,cy; } R1,R2;    //
                        // Fractional coords of the two rays:
                        // R1, R2 follow edges P1->P2 & P1b->P2
                        // R1 forced to be at the left hand
                        frAy_R1 = frAcy12;
                        if (!frAy_R1)
                            frAy_R1 = 2 << FRBITS;  // If Ay=0, force it to be
                        // >0 so the "do...while"
                        // loop below ends in ONE
                        // iteration.

                        if (P1.x < P1b.x)
                        {
                            // R1: P1->P2,  R2: P1b->P2
                            R1.cx = P1.cx;
                            R1.cy = P1.cy;
                            R2.cx = P1b.cx;
                            R2.cy = P1b.cy;
                            frAx_R1 = frAcx12;
                            frAx_R2 = frAcx1b2;
                        }
                        else
                        {
                            // R1: P1b->P2,  R2: P1->P2
                            R1.cx = P1b.cx;
                            R1.cy = P1b.cy;
                            R2.cx = P1.cx;
                            R2.cy = P1.cy;
                            frAx_R1 = frAcx1b2;
                            frAx_R2 = frAcx12;
                        }

                        R1.frX = R1.cx << FRBITS;
                        R1.frY = R1.cy << FRBITS;
                        R2.frX = R2.cx << FRBITS;
                        R2.frY = R2.cy << FRBITS;

                        last_insert_cy = -100;
                        // last_insert_cx=-100;

                        do
                        {
                            if (last_insert_cy !=
                                R1.cy)  // || last_insert_cx!=R1.cx)
                            {
                                //  last_insert_cx = R1.cx;
                                last_insert_cy = R1.cy;
                                for (int ccx = R1.cx; ccx <= R2.cx; ccx++)
                                    updateCell_fast_free(
                                        ccx, R1.cy, logodd_observation_free,
                                        logodd_thres_free, theMapArray,
                                        theMapSize_x);
                            }

                            R1.frX += frAx_R1;
                            R1.frY += frAy_R1;
                            R2.frX += frAx_R2;  // R1.frY += frAcy01;

                            R1.cx = R1.frX >> FRBITS;
                            R1.cy = R1.frY >> FRBITS;
                            R2.cx = R2.frX >> FRBITS;
                        } while (R1.cy <= P2.cy);

                    }  // end of free-area normal case (not a single row)

                    // ----------------------------------------------------
                    // The final occupied cells along the edge P1<->P2
                    // Only if:
                    //  - It was a valid ray, and
                    //  - The ray was not truncated
                    // ----------------------------------------------------
                    if (o->validRange[idx] &&
                        o->scan[idx] < maxDistanceInsertion)
                    {
                        theR += resolution;

                        P1.x = px + cos(A - dA_2) * theR;
                        P1.y = py + sin(A - dA_2) * theR;

                        P2.x = px + cos(A + dA_2) * theR;
                        P2.y = py + sin(A + dA_2) * theR;

                        P1.cx = x2idx(P1.x);
                        P1.cy = y2idx(P1.y);
                        P2.cx = x2idx(P2.x);
                        P2.cy = y2idx(P2.y);

#if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
                        // The x> comparison implicitly holds if x<0
                        ASSERT_(
                            static_cast<unsigned int>(P1.cx) < size_x &&
                            static_cast<unsigned int>(P1.cy) < size_y);
                        ASSERT_(
                            static_cast<unsigned int>(P2.cx) < size_x &&
                            static_cast<unsigned int>(P2.cy) < size_y);
#endif

                        // Special case: Only one cell:
                        if (P2.cx == P1.cx && P2.cy == P1.cy)
                        {
                            updateCell_fast_occupied(
                                P1.cx, P1.cy, logodd_observation_occupied,
                                logodd_thres_occupied, theMapArray,
                                theMapSize_x);
                        }
                        else
                        {
                            // Use "fractional integers" to approximate float
                            // operations during the ray tracing:
                            // Integers store "float values * 128"
                            const int AcxE = P2.cx - P1.cx;
                            const int AcyE = P2.cy - P1.cy;

                            // Increments at each raytracing step:
                            const int nSteps = (max(abs(AcxE), abs(AcyE)) + 1);
                            const float inv_N_12 =
                                1.0f / nSteps;  // Number of steps ^ -1
                            const int frAcxE = round(
                                (AcxE << FRBITS) * inv_N_12);  //  Acx*128 / N
                            const int frAcyE = round(
                                (AcyE << FRBITS) * inv_N_12);  //  Acy*128 / N

                            R1.cx = P1.cx;
                            R1.cy = P1.cy;
                            R1.frX = R1.cx << FRBITS;
                            R1.frY = R1.cy << FRBITS;

                            for (int nStep = 0; nStep <= nSteps; nStep++)
                            {
                                updateCell_fast_occupied(
                                    R1.cx, R1.cy, logodd_observation_occupied,
                                    logodd_thres_occupied, theMapArray,
                                    theMapSize_x);

                                R1.frX += frAcxE;
                                R1.frY += frAcyE;
                                R1.cx = R1.frX >> FRBITS;
                                R1.cy = R1.frY >> FRBITS;
                            }

                        }  // end do a line

                    }  // end if we must set occupied cells

                }  // End of each range

            }  // end insert with beam widening

            // Finished:
            return true;
        }
        else
        {
            // A non-horizontal scan:
            return false;
        }
    }
    else if (CLASS_ID(CObservationRange) == obs->GetRuntimeClass())
    {
        const CObservationRange* o = static_cast<const CObservationRange*>(obs);
        CPose3D spose;
        o->getSensorPose(spose);
        CPose3D sensorPose3D = robotPose3D + spose;
        CPose2D laserPose(sensorPose3D);

        // Insert only HORIZONTAL scans, since the grid is supposed to
        //  be a horizontal representation of space.
        bool reallyInsert = true;
        unsigned int decimation = insertionOptions.decimation;

        // Check the altitude of the map (if feature enabled!)
        if (insertionOptions.useMapAltitude &&
            fabs(insertionOptions.mapAltitude - sensorPose3D.z()) > 0.001)
        {
            reallyInsert = false;
        }
        if (reallyInsert)
        {
            // ---------------------------------------------
            //      Insert the scan as simple rays:
            // ---------------------------------------------

            // int      /*cx,cy,*/ N =  o->sensedData.size();
            float px, py;
            double A, dAK;

            // Parameters values:
            const float maxDistanceInsertion =
                insertionOptions.maxDistanceInsertion;
            const bool invalidAsFree =
                insertionOptions.considerInvalidRangesAsFreeSpace;
            float new_x_max, new_x_min;
            float new_y_max, new_y_min;
            float last_valid_range = maxDistanceInsertion;

            int K = updateInfoChangeOnly.enabled
                        ? updateInfoChangeOnly.laserRaysSkip
                        : decimation;
            size_t idx, nRanges = o->sensedData.size();
            float curRange = 0;

            // Start position:
            px = laserPose.x();
            py = laserPose.y();

#if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
            MRPT_CHECK_NORMAL_NUMBER(px);
            MRPT_CHECK_NORMAL_NUMBER(py);
#endif
            // ---------------------------------
            //          Widen rays
            // Algorithm in: http://www.mrpt.org/Occupancy_Grids
            // FIXME: doesn't support many different poses in one measurement
            // ---------------------------------
            A = laserPose.phi();
            dAK = 0;

            new_x_max = -(numeric_limits<float>::max)();
            new_x_min = (numeric_limits<float>::max)();
            new_y_max = -(numeric_limits<float>::max)();
            new_y_min = (numeric_limits<float>::max)();

            last_valid_range = maxDistanceInsertion;

            for (idx = 0; idx < nRanges; idx += K)
            {
                float scanPoint_x, scanPoint_y;
                if (o->sensedData[idx].sensedDistance < maxDistanceInsertion)
                {
                    curRange = o->sensedData[idx].sensedDistance;
                    float R = min(maxDistanceInsertion, curRange);

                    scanPoint_x = px + cos(A) * R;
                    scanPoint_y = py + sin(A) * R;
                    last_valid_range = curRange;
                }
                else
                {
                    if (invalidAsFree)
                    {
                        // Invalid range:
                        float R =
                            min(maxDistanceInsertion, 0.5f * last_valid_range);
                        scanPoint_x = px + cos(A) * R;
                        scanPoint_y = py + sin(A) * R;
                    }
                    else
                    {
                        scanPoint_x = px;
                        scanPoint_y = py;
                    }
                }
                A += dAK;

                // Asjust size (will not change if not required):
                new_x_max = max(new_x_max, scanPoint_x);
                new_x_min = min(new_x_min, scanPoint_x);
                new_y_max = max(new_y_max, scanPoint_y);
                new_y_min = min(new_y_min, scanPoint_y);
            }

            // Add an extra margin:
            float securMargen = 15 * resolution;

            if (new_x_max > x_max - securMargen)
                new_x_max += 2 * securMargen;
            else
                new_x_max = x_max;
            if (new_x_min < x_min + securMargen)
                new_x_min -= 2;
            else
                new_x_min = x_min;

            if (new_y_max > y_max - securMargen)
                new_y_max += 2 * securMargen;
            else
                new_y_max = y_max;
            if (new_y_min < y_min + securMargen)
                new_y_min -= 2;
            else
                new_y_min = y_min;

            // -----------------------
            //   Resize to make room:
            // -----------------------
            resizeGrid(new_x_min, new_x_max, new_y_min, new_y_max, 0.5);

            // For updateCell_fast methods:
            cellType* theMapArray = &map[0];
            unsigned theMapSize_x = size_x;

            // int  cx0 = x2idx(px);        // Remember: This must be after the
            // resizeGrid!!
            // int  cy0 = y2idx(py);

            // Now go and insert the triangles of each beam:
            // -----------------------------------------------
            A = laserPose.phi() - 0.5 * o->sensorConeApperture;
            dAK = 0;

            // Insert the rays:
            // ------------------------------------------
            // Vertices of the triangle: In meters
            TLocalPoint P0, P1, P2, P1b;

            last_valid_range = maxDistanceInsertion;

            const double dA_2 = 0.5 * o->sensorConeApperture;
            for (idx = 0; idx < nRanges; idx += K, A += dAK)
            {
                float theR;  // The range of this beam
                if (o->sensedData[idx].sensedDistance < maxDistanceInsertion)
                {
                    curRange = o->sensedData[idx].sensedDistance;
                    last_valid_range = curRange;
                    theR = min(maxDistanceInsertion, curRange);
                }
                else
                {
                    // Invalid range:
                    if (invalidAsFree)
                    {
                        theR =
                            min(maxDistanceInsertion, 0.5f * last_valid_range);
                    }
                    else
                        continue;  // Nothing to do
                }
                if (theR < resolution)
                    continue;  // Range must be larger than a cell...
                theR -= resolution;  // Remove one cell of length, which will be
                // filled with "occupied" later.

                /* ---------------------------------------------------------
                      Fill one triangle with vertices: P0,P1,P2
                   --------------------------------------------------------- */
                P0.x = px;
                P0.y = py;

                P1.x = px + cos(A - dA_2) * theR;
                P1.y = py + sin(A - dA_2) * theR;

                P2.x = px + cos(A + dA_2) * theR;
                P2.y = py + sin(A + dA_2) * theR;

                // Order the vertices by the "y": P0->bottom, P2: top
                if (P2.y < P1.y) std::swap(P2, P1);
                if (P2.y < P0.y) std::swap(P2, P0);
                if (P1.y < P0.y) std::swap(P1, P0);

                // In cell indexes:
                P0.cx = x2idx(P0.x);
                P0.cy = y2idx(P0.y);
                P1.cx = x2idx(P1.x);
                P1.cy = y2idx(P1.y);
                P2.cx = x2idx(P2.x);
                P2.cy = y2idx(P2.y);

#if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
                // The x> comparison implicitly holds if x<0
                ASSERT_(
                    static_cast<unsigned int>(P0.cx) < size_x &&
                    static_cast<unsigned int>(P0.cy) < size_y);
                ASSERT_(
                    static_cast<unsigned int>(P1.cx) < size_x &&
                    static_cast<unsigned int>(P1.cy) < size_y);
                ASSERT_(
                    static_cast<unsigned int>(P2.cx) < size_x &&
                    static_cast<unsigned int>(P2.cy) < size_y);
#endif

                struct
                {
                    int frX, frY;
                    int cx, cy;
                } R1, R2;  // Fractional coords of the two rays:

                // Special case: one single row
                if (P0.cy == P2.cy && P0.cy == P1.cy)
                {
                    // Optimized case:
                    int min_cx = min3(P0.cx, P1.cx, P2.cx);
                    int max_cx = max3(P0.cx, P1.cx, P2.cx);

                    for (int ccx = min_cx; ccx <= max_cx; ccx++)
                        updateCell_fast_free(
                            ccx, P0.cy, logodd_observation_free,
                            logodd_thres_free, theMapArray, theMapSize_x);
                }
                else
                {
                    // The intersection point P1b in the segment P0-P2 at the
                    // "y" of P1:
                    P1b.y = P1.y;
                    P1b.x =
                        P0.x + (P1.y - P0.y) * (P2.x - P0.x) / (P2.y - P0.y);

                    P1b.cx = x2idx(P1b.x);
                    P1b.cy = y2idx(P1b.y);

                    // Use "fractional integers" to approximate float operations
                    // during the ray tracing:
                    // Integers store "float values * 128"
                    const int Acx01 = P1.cx - P0.cx;
                    const int Acy01 = P1.cy - P0.cy;
                    const int Acx01b = P1b.cx - P0.cx;
                    // const int Acy01b = P1b.cy - P0.cy;  // = Acy01

                    // Increments at each raytracing step:
                    const float inv_N_01 =
                        1.0f / (max3(abs(Acx01), abs(Acy01), abs(Acx01b)) +
                                1);  // Number of steps ^ -1
                    const int frAcx01 =
                        round((Acx01 << FRBITS) * inv_N_01);  //  Acx*128 / N
                    const int frAcy01 =
                        round((Acy01 << FRBITS) * inv_N_01);  //  Acy*128 / N
                    const int frAcx01b =
                        round((Acx01b << FRBITS) * inv_N_01);  //  Acx*128 / N

                    // ------------------------------------
                    // First sub-triangle: P0-P1-P1b
                    // ------------------------------------
                    R1.cx = P0.cx;
                    R1.cy = P0.cy;
                    R1.frX = P0.cx << FRBITS;
                    R1.frY = P0.cy << FRBITS;

                    int frAx_R1 = 0, frAx_R2 = 0;  //, frAy_R2;
                    int frAy_R1 = frAcy01;

                    // Start R1=R2 = P0... unlesss P0.cy == P1.cy, i.e. there is
                    // only one row:
                    if (P0.cy != P1.cy)
                    {
                        R2 = R1;
                        //  R1 & R2 follow the edges: P0->P1  & P0->P1b
                        //  R1 is forced to be at the left hand:
                        if (P1.x < P1b.x)
                        {
                            // R1: P0->P1
                            frAx_R1 = frAcx01;
                            frAx_R2 = frAcx01b;
                        }
                        else
                        {
                            // R1: P0->P1b
                            frAx_R1 = frAcx01b;
                            frAx_R2 = frAcx01;
                        }
                    }
                    else
                    {
                        R2.cx = P1.cx;
                        R2.cy = P1.cy;
                        R2.frX = P1.cx << FRBITS;
                        // R2.frY = P1.cy <<FRBITS;
                    }
                    int last_insert_cy = -1;
                    // int last_insert_cx = -1;
                    do
                    {
                        if (last_insert_cy !=
                            R1.cy)  // || last_insert_cx!=R1.cx)
                        {
                            last_insert_cy = R1.cy;
                            //  last_insert_cx = R1.cx;

                            for (int ccx = R1.cx; ccx <= R2.cx; ccx++)
                                updateCell_fast_free(
                                    ccx, R1.cy, logodd_observation_free,
                                    logodd_thres_free, theMapArray,
                                    theMapSize_x);
                        }

                        R1.frX += frAx_R1;
                        R1.frY += frAy_R1;
                        R2.frX += frAx_R2;  // R1.frY += frAcy01;

                        R1.cx = R1.frX >> FRBITS;
                        R1.cy = R1.frY >> FRBITS;
                        R2.cx = R2.frX >> FRBITS;
                    } while (R1.cy < P1.cy);
                    // ------------------------------------
                    // Second sub-triangle: P1-P1b-P2
                    // ------------------------------------

                    // Use "fractional integers" to approximate float operations
                    // during the ray tracing:
                    // Integers store "float values * 128"
                    const int Acx12 = P2.cx - P1.cx;
                    const int Acy12 = P2.cy - P1.cy;
                    const int Acx1b2 = P2.cx - P1b.cx;
                    // const int Acy1b2 = Acy12

                    // Increments at each raytracing step:
                    const float inv_N_12 =
                        1.0f / (max3(abs(Acx12), abs(Acy12), abs(Acx1b2)) +
                                1);  // Number of steps ^ -1
                    const int frAcx12 =
                        round((Acx12 << FRBITS) * inv_N_12);  //  Acx*128 / N
                    const int frAcy12 =
                        round((Acy12 << FRBITS) * inv_N_12);  //  Acy*128 / N
                    const int frAcx1b2 =
                        round((Acx1b2 << FRBITS) * inv_N_12);  //  Acx*128 / N

                    // struct { int frX,frY; int cx,cy; } R1,R2;    //
                    // Fractional
                    // coords of the two rays:
                    // R1, R2 follow edges P1->P2 & P1b->P2
                    // R1 forced to be at the left hand
                    frAy_R1 = frAcy12;
                    if (!frAy_R1)
                        frAy_R1 = 2 << FRBITS;  // If Ay=0, force it to be >0 so
                    // the "do...while" loop below
                    // ends in ONE iteration.

                    if (P1.x < P1b.x)
                    {
                        // R1: P1->P2,  R2: P1b->P2
                        R1.cx = P1.cx;
                        R1.cy = P1.cy;
                        R2.cx = P1b.cx;
                        R2.cy = P1b.cy;
                        frAx_R1 = frAcx12;
                        frAx_R2 = frAcx1b2;
                    }
                    else
                    {
                        // R1: P1b->P2,  R2: P1->P2
                        R1.cx = P1b.cx;
                        R1.cy = P1b.cy;
                        R2.cx = P1.cx;
                        R2.cy = P1.cy;
                        frAx_R1 = frAcx1b2;
                        frAx_R2 = frAcx12;
                    }

                    R1.frX = R1.cx << FRBITS;
                    R1.frY = R1.cy << FRBITS;
                    R2.frX = R2.cx << FRBITS;
                    R2.frY = R2.cy << FRBITS;

                    last_insert_cy = -100;
                    // last_insert_cx=-100;

                    do
                    {
                        if (last_insert_cy !=
                            R1.cy)  // || last_insert_cx!=R1.cx)
                        {
                            //  last_insert_cx = R1.cx;
                            last_insert_cy = R1.cy;
                            for (int ccx = R1.cx; ccx <= R2.cx; ccx++)
                                updateCell_fast_free(
                                    ccx, R1.cy, logodd_observation_free,
                                    logodd_thres_free, theMapArray,
                                    theMapSize_x);
                        }

                        R1.frX += frAx_R1;
                        R1.frY += frAy_R1;
                        R2.frX += frAx_R2;  // R1.frY += frAcy01;

                        R1.cx = R1.frX >> FRBITS;
                        R1.cy = R1.frY >> FRBITS;
                        R2.cx = R2.frX >> FRBITS;
                    } while (R1.cy <= P2.cy);

                }  // end of free-area normal case (not a single row)

                // ----------------------------------------------------
                // The final occupied cells along the edge P1<->P2
                // Only if:
                //  - It was a valid ray, and
                //  - The ray was not truncated
                // ----------------------------------------------------
                if (o->sensedData[idx].sensedDistance < maxDistanceInsertion)
                {
                    theR += resolution;

                    P1.x = px + cos(A - dA_2) * theR;
                    P1.y = py + sin(A - dA_2) * theR;

                    P2.x = px + cos(A + dA_2) * theR;
                    P2.y = py + sin(A + dA_2) * theR;

                    P1.cx = x2idx(P1.x);
                    P1.cy = y2idx(P1.y);
                    P2.cx = x2idx(P2.x);
                    P2.cy = y2idx(P2.y);

#if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
                    // The x> comparison implicitly holds if x<0
                    ASSERT_(
                        static_cast<unsigned int>(P1.cx) < size_x &&
                        static_cast<unsigned int>(P1.cy) < size_y);
                    ASSERT_(
                        static_cast<unsigned int>(P2.cx) < size_x &&
                        static_cast<unsigned int>(P2.cy) < size_y);
#endif

                    // Special case: Only one cell:
                    if (P2.cx == P1.cx && P2.cy == P1.cy)
                    {
                        updateCell_fast_occupied(
                            P1.cx, P1.cy, logodd_observation_occupied,
                            logodd_thres_occupied, theMapArray, theMapSize_x);
                    }
                    else
                    {
                        // Use "fractional integers" to approximate float
                        // operations during the ray tracing:
                        // Integers store "float values * 128"
                        const int AcxE = P2.cx - P1.cx;
                        const int AcyE = P2.cy - P1.cy;

                        // Increments at each raytracing step:
                        const int nSteps = (max(abs(AcxE), abs(AcyE)) + 1);
                        const float inv_N_12 =
                            1.0f / nSteps;  // Number of steps ^ -1
                        const int frAcxE =
                            round((AcxE << FRBITS) * inv_N_12);  //  Acx*128 / N
                        const int frAcyE =
                            round((AcyE << FRBITS) * inv_N_12);  //  Acy*128 / N

                        R1.cx = P1.cx;
                        R1.cy = P1.cy;
                        R1.frX = R1.cx << FRBITS;
                        R1.frY = R1.cy << FRBITS;

                        for (int nStep = 0; nStep <= nSteps; nStep++)
                        {
                            updateCell_fast_occupied(
                                R1.cx, R1.cy, logodd_observation_occupied,
                                logodd_thres_occupied, theMapArray,
                                theMapSize_x);

                            R1.frX += frAcxE;
                            R1.frY += frAcyE;
                            R1.cx = R1.frX >> FRBITS;
                            R1.cy = R1.frY >> FRBITS;
                        }

                    }  // end do a line

                }  // end if we must set occupied cells

            }  // End of each range

            return true;
        }  // end reallyInsert
        else
            return false;
    }
    else
    {
        /********************************************************************
                OBSERVATION TYPE: Unknown
        ********************************************************************/
        return false;
    }

    //  MRPT_END
}

/*---------------------------------------------------------------
    Initilization of values, don't needed to be called directly.
  ---------------------------------------------------------------*/
COccupancyGridMap2D::TInsertionOptions::TInsertionOptions()
    : mapAltitude(0),
      useMapAltitude(false),
      maxDistanceInsertion(15.0f),
      maxOccupancyUpdateCertainty(0.65f),
      maxFreenessUpdateCertainty(.0f),
      maxFreenessInvalidRanges(.0f),
      considerInvalidRangesAsFreeSpace(true),
      decimation(1),
      horizontalTolerance(DEG2RAD(0.05)),

      CFD_features_gaussian_size(1),
      CFD_features_median_size(3),

      wideningBeamsWithDistance(false)
{
}

/*---------------------------------------------------------------
                    loadFromConfigFile
  ---------------------------------------------------------------*/
void COccupancyGridMap2D::TInsertionOptions::loadFromConfigFile(
    const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
{
    MRPT_LOAD_CONFIG_VAR(mapAltitude, float, iniFile, section);
    MRPT_LOAD_CONFIG_VAR(maxDistanceInsertion, float, iniFile, section);
    MRPT_LOAD_CONFIG_VAR(maxOccupancyUpdateCertainty, float, iniFile, section);
    MRPT_LOAD_CONFIG_VAR(maxFreenessUpdateCertainty, float, iniFile, section);
    MRPT_LOAD_CONFIG_VAR(maxFreenessInvalidRanges, float, iniFile, section);
    MRPT_LOAD_CONFIG_VAR(useMapAltitude, bool, iniFile, section);
    MRPT_LOAD_CONFIG_VAR(
        considerInvalidRangesAsFreeSpace, bool, iniFile, section);
    MRPT_LOAD_CONFIG_VAR(decimation, int, iniFile, section);
    MRPT_LOAD_CONFIG_VAR_DEGREES(horizontalTolerance, iniFile, section);

    MRPT_LOAD_CONFIG_VAR(CFD_features_gaussian_size, float, iniFile, section);
    MRPT_LOAD_CONFIG_VAR(CFD_features_median_size, float, iniFile, section);
    MRPT_LOAD_CONFIG_VAR(wideningBeamsWithDistance, bool, iniFile, section);
}

/*---------------------------------------------------------------
                    dumpToTextStream
  ---------------------------------------------------------------*/
void COccupancyGridMap2D::TInsertionOptions::dumpToTextStream(
    std::ostream& out) const
{
    out << mrpt::format(
        "\n----------- [COccupancyGridMap2D::TInsertionOptions] ------------ "
        "\n\n");

    LOADABLEOPTS_DUMP_VAR(mapAltitude, float)
    LOADABLEOPTS_DUMP_VAR(maxDistanceInsertion, float)
    LOADABLEOPTS_DUMP_VAR(maxOccupancyUpdateCertainty, float)
    LOADABLEOPTS_DUMP_VAR(maxFreenessUpdateCertainty, float)
    LOADABLEOPTS_DUMP_VAR(maxFreenessInvalidRanges, float)
    LOADABLEOPTS_DUMP_VAR(useMapAltitude, bool)
    LOADABLEOPTS_DUMP_VAR(considerInvalidRangesAsFreeSpace, bool)
    LOADABLEOPTS_DUMP_VAR(decimation, int)
    LOADABLEOPTS_DUMP_VAR(horizontalTolerance, float)
    LOADABLEOPTS_DUMP_VAR(CFD_features_gaussian_size, float)
    LOADABLEOPTS_DUMP_VAR(CFD_features_median_size, float)
    LOADABLEOPTS_DUMP_VAR(wideningBeamsWithDistance, bool)

    out << mrpt::format("\n");
}

void COccupancyGridMap2D::OnPostSuccesfulInsertObs(
    const mrpt::obs::CObservation*)
{
    m_is_empty = false;
}