Program Listing for File occupancy_grid.hpp

↰ Return to documentation for file (include/beluga/sensor/data/occupancy_grid.hpp)

// Copyright 2023 Ekumen, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef BELUGA_SENSOR_DATA_OCCUPANCY_GRID_HPP
#define BELUGA_SENSOR_DATA_OCCUPANCY_GRID_HPP

#include <cstdint>
#include <tuple>
#include <vector>

#include <beluga/sensor/data/linear_grid.hpp>

#include <range/v3/view/enumerate.hpp>
#include <range/v3/view/filter.hpp>
#include <range/v3/view/transform.hpp>

#include <Eigen/Core>

namespace beluga {


template <typename Derived>
class BaseOccupancyGrid2 : public BaseLinearGrid2<Derived> {
 public:
  enum class Frame : std::uint8_t { kLocal, kGlobal };


  [[nodiscard]] bool free_at(std::size_t index) const {
    const auto data = this->self().data_at(index);
    if (!data.has_value()) {
      return false;
    }
    return this->self().value_traits().is_free(data.value());
  }


  [[nodiscard]] bool free_at(int xi, int yi) const { return this->self().free_at(this->self().index_at(xi, yi)); }


  [[nodiscard]] bool free_at(const Eigen::Vector2i& pi) const { return this->self().free_at(pi.x(), pi.y()); }


  [[nodiscard]] bool free_near(double x, double y) const {
    return this->self().free_at(this->self().cell_near(Eigen::Vector2d{x, y}));
  }


  [[nodiscard]] bool free_near(const Eigen::Vector2d& p) const { return this->self().free_near(p.x(), p.y()); }

  using BaseLinearGrid2<Derived>::coordinates_at;


  [[nodiscard]] auto coordinates_at(std::size_t index, Frame frame) const {
    auto position = this->self().coordinates_at(index);
    if (frame == Frame::kGlobal) {
      position = this->self().origin() * position;
    }
    return position;
  }


  template <class Range>
  [[nodiscard]] auto coordinates_for(Range&& cells, Frame frame) const {
    return cells | ranges::views::transform(
                       [this, frame](const auto& cell) { return this->self().coordinates_at(cell, frame); });
  }

  [[nodiscard]] auto free_cells() const {
    return ranges::views::enumerate(this->self().data()) |
           ranges::views::filter([value_traits = this->self().value_traits()](const auto& tuple) {
             return value_traits.is_free(std::get<1>(tuple));
           }) |
           ranges::views::transform([](const auto& tuple) { return std::get<0>(tuple); });
  }

  [[nodiscard]] auto obstacle_data() const {
    return this->self().data() |
           ranges::views::transform([value_traits = this->self().value_traits()](const auto& value) {
             return value_traits.is_occupied(value);
           });
  }
};

}  // namespace beluga

#endif