Program Listing for File sparse_value_grid.hpp

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

// Copyright 2024 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_SPARSE_VALUE_GRID_HPP
#define BELUGA_SENSOR_DATA_SPARSE_VALUE_GRID_HPP

#include <Eigen/Core>
#include <optional>
#include <type_traits>

#include <beluga/eigen_compatibility.hpp>
#include <beluga/sensor/data/regular_grid.hpp>
namespace beluga {


template <typename MapType, int NDim>
class SparseValueGrid : public BaseRegularGrid<SparseValueGrid<MapType, NDim>, NDim> {
 public:
  SparseValueGrid() = default;
  using map_type = MapType;
  using mapped_type = typename map_type::mapped_type;
  using key_type = typename map_type::key_type;

  static_assert(std::is_same_v<key_type, Eigen::Vector<int, NDim>>);

  explicit SparseValueGrid(map_type data, double resolution = 1.) : data_(std::move(data)), resolution_(resolution) {
    assert(resolution_ > 0);
  }

  [[nodiscard]] double resolution() const { return resolution_; }

  [[nodiscard]] std::size_t size() const { return data_.size(); }

  [[nodiscard]] const map_type& data() const { return data_; }

  [[nodiscard]] std::optional<mapped_type> data_at(const Eigen::Vector<int, NDim>& cell_index) const {
    const auto itr = data_.find(cell_index);
    if (itr == data_.end()) {
      return std::nullopt;
    }
    return itr->second;
  }

  [[nodiscard]] std::optional<mapped_type> data_near(const Eigen::Vector<double, NDim>& coordinates) const {
    return data_at(this->self().cell_near(coordinates));
  }

 private:
  map_type data_;
  double resolution_ = 1.0;
};

template <typename MapType>
using SparseValueGrid2 = SparseValueGrid<MapType, 2>;

template <typename MapType>
using SparseValueGrid3 = SparseValueGrid<MapType, 3>;
}  // namespace beluga

#endif