Program Listing for File OccGrid.h

↰ Return to documentation for file (include/mola_metric_maps/OccGrid.h)

/* -------------------------------------------------------------------------
 *   A Modular Optimization framework for Localization and mApping  (MOLA)
 *
 * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria
 * Licensed under the GNU GPL v3 for non-commercial applications.
 *
 * This file is part of MOLA.
 * MOLA 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.
 *
 * MOLA 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
 * MOLA. If not, see <https://www.gnu.org/licenses/>.
 * ------------------------------------------------------------------------- */
#pragma once

#include <mrpt/containers/CDynamicGrid.h>
#include <mrpt/maps/COccupancyGridMap2D.h>
#include <mrpt/math/TPoint2D.h>
#include <mrpt/obs/CObservation2DRangeScan.h>
#include <mrpt/serialization/CSerializable.h>

namespace mola
{
class OccGrid : public mrpt::serialization::CSerializable
{
    DEFINE_SERIALIZABLE(OccGrid, mola)

   public:
    OccGrid();
    ~OccGrid() = default;

    void setSize(
        const mrpt::math::TPoint2Df& minCorner,
        const mrpt::math::TPoint2Df& maxCorner, float resolution,
        float occupancyValue = 0.5f);

    void resizeGrid(
        const mrpt::math::TPoint2Df& minCorner,
        const mrpt::math::TPoint2Df& maxCorner,
        float                        newCellsOccupancy = 0.5f) noexcept;

    struct InsertionParameters
    {
        InsertionParameters() = default;

        float maxDistanceInsertion = 50.0;

        float maxOccupancyUpdateCertainty{0.65f};

        float maxFreenessUpdateCertainty{.0f};

        float maxFreenessInvalidRanges{.0f};

        bool considerInvalidRangesAsFreeSpace{true};

        uint16_t decimation{1};

        bool wideningBeamsWithDistance{false};
    };

    InsertionParameters insertionParameters_;

    void insertObservation(
        const mrpt::obs::CObservation2DRangeScan& obs,
        const mrpt::math::TPose2D&                robotPose);

    const mrpt::maps::COccupancyGridMap2D& grid() const { return grid_; }

    struct LikelihoodParameters
    {
        LikelihoodParameters() = default;

        double superResolutionFactor = 4;
    };

    LikelihoodParameters likelihoodParameters_;

    void resetLikelihoodCache();

    static constexpr float CACHE_MISS_VALUE = std::numeric_limits<float>::min();

   private:
    mrpt::maps::COccupancyGridMap2D grid_;

    mrpt::containers::CDynamicGrid<float> likelihoodCacheGrid_;
};

}  // namespace mola