ndt_ellipsoid.hpp

Go to the documentation of this file.

// 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