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