dense_grid.hpp

Go to the documentation of this file.

// 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_DENSE_GRID_HPP
#define BELUGA_SENSOR_DATA_DENSE_GRID_HPP
 
#include <optional>
#include <vector>
 
#include <beluga/sensor/data/regular_grid.hpp>
 
#include <Eigen/Core>
#include <beluga/eigen_compatibility.hpp>
 
namespace beluga {
 
 
template <typename Derived>
class BaseDenseGrid2 : public BaseRegularGrid2<Derived> {
 public:
 
  [[nodiscard]] bool contains(int xi, int yi) const {
    const auto width = static_cast<int>(this->self().width());
    const auto height = static_cast<int>(this->self().height());
    return xi >= 0 && yi >= 0 && xi < width && yi < height;
  }
 
 
  [[nodiscard]] bool contains(const Eigen::Vector2i& pi) const { return this->self().contains(pi.x(), pi.y()); }
 
 
  [[nodiscard]] auto data_at(int xi, int yi) const {
    return this->self().contains(xi, yi) ? this->self().data_at(this->self().index_at(Eigen::Vector2i{xi, yi}))
                                         : std::nullopt;
  }
 
 
  [[nodiscard]] auto data_at(const Eigen::Vector2i& pi) const { return this->self().data_at(pi.x(), pi.y()); }
 
 
  [[nodiscard]] auto data_near(double x, double y) const {
    return this->self().data_at(this->self().cell_near(Eigen::Vector2d{x, y}));
  }
 
 
  [[nodiscard]] auto data_near(const Eigen::Vector2d& p) const { return this->self().data_near(p.x(), p.y()); }
 
 
  [[nodiscard]] auto neighborhood4(int xi, int yi) const {
    auto result = std::vector<Eigen::Vector2i>{};
    if (xi < static_cast<int>(this->self().width() - 1)) {
      result.emplace_back(xi + 1, yi);
    }
    if (yi < static_cast<int>(this->self().height() - 1)) {
      result.emplace_back(xi, yi + 1);
    }
    if (xi > 0) {
      result.emplace_back(xi - 1, yi);
    }
    if (yi > 0) {
      result.emplace_back(xi, yi - 1);
    }
    return result;
  }
 
 
  [[nodiscard]] auto neighborhood4(const Eigen::Vector2i& pi) const {
    return this->self().neighborhood4(pi.x(), pi.y());
  }
};
 
}  // namespace beluga
 
#endif