Program Listing for File ndt_ellipsoid.hpp

↰ Return to documentation for file (include/beluga_ros/ndt_ellipsoid.hpp)

// Copyright 2025 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_ROS_NDT_ELLIPSOID_HPP
#define BELUGA_ROS_NDT_ELLIPSOID_HPP

#include <beluga_ros/messages.hpp>

#include <Eigen/Core>
#include <beluga/eigen_compatibility.hpp>

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

#include <beluga/sensor/data/ndt_cell.hpp>
#include <beluga/sensor/data/sparse_value_grid.hpp>

namespace beluga_ros {

namespace detail {


bool use_mean_covariance(
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 3, 3>>& eigen_solver,
    beluga::NDTCell<3> cell,
    beluga_ros::msg::Marker& marker);


void use_cell_size(const Eigen::Vector<int, 3>& position, double size, beluga_ros::msg::Marker& marker);

}  // namespace detail


template <typename MapType, int NDim>
std::pair<beluga_ros::msg::MarkerArray, bool> assign_obstacle_map(
    const beluga::SparseValueGrid<MapType, NDim>& grid,
    beluga_ros::msg::MarkerArray& message) {
  bool cubes_generated = false;
  // Get data from the grid
  auto& map = grid.data();

  // Clean up the message
  beluga_ros::msg::Marker marker;
  marker.ns = "obstacles";
  marker.action = beluga_ros::msg::Marker::DELETEALL;
  message.markers.push_back(marker);

  // Add the markers
  Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 3, 3>> eigen_solver;
  for (auto [index, entry] : ranges::views::enumerate(map)) {
    const auto& [cell_center, cell] = entry;
    beluga_ros::msg::Marker marker;
    marker.header.frame_id = "map";
    marker.id = index + 1;
    marker.ns = "obstacles";

    // Try to create an ellipsoid with values of the cell
    if (!beluga_ros::detail::use_mean_covariance(eigen_solver, cell, marker)) {
      // Create a cube based on the resolution of the grid
      cubes_generated = true;
      beluga_ros::detail::use_cell_size(cell_center, grid.resolution(), marker);
    }

    // Add the marker
    message.markers.push_back(marker);
  }

  return {message, cubes_generated};
}

}  // namespace beluga_ros

#endif  // BELUGA_ROS_NDT_ELLIPSOID_HPP