Ndt map visualization

beluga_amcl/include/beluga_amcl/ndt_amcl_node_3d.hpp

@@ -90,6 +90,9 @@ class NdtAmclNode3D : public BaseAMCLNode {
   /// Callback for lifecycle transitions from the INACTIVE state to the ACTIVE state.
   void do_activate(const rclcpp_lifecycle::State&) override;
 
+  /// Callback for lifecycle transitions from the UNCONFIGURED state to the INACTIVE state.
+  void do_configure(const rclcpp_lifecycle::State&) override;
+
   /// Callback for lifecycle transitions from the ACTIVE state to the INACTIVE state.
   void do_deactivate(const rclcpp_lifecycle::State&) override;
 
@@ -137,6 +140,9 @@ class NdtAmclNode3D : public BaseAMCLNode {
   /// Connection for laser scan updates filter and callback.
   message_filters::Connection laser_scan_connection_;
 
+  /// NDT map representation
+  beluga_amcl::NDTMapRepresentation map_;
+
   /// Particle filter instance.
   std::unique_ptr<NdtAmclVariant> particle_filter_;
   /// Last known pose estimate, if any.
@@ -145,6 +151,9 @@ class NdtAmclNode3D : public BaseAMCLNode {
   std::optional<Sophus::SE3d> last_known_odom_transform_in_map_;
   /// Whether to broadcast transforms or not.
   bool enable_tf_broadcast_{false};
+
+  /// Map visualization publisher.
+  rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray>::SharedPtr map_visualization_pub_;
 };
 
 }  // namespace beluga_amcl

beluga_amcl/src/ndt_amcl_node_3d.cpp

@@ -78,6 +78,8 @@
 #include "beluga_amcl/ndt_amcl_node_3d.hpp"
 #include "beluga_amcl/ros2_common.hpp"
 
+#include "beluga_ros/ndt_ellipsoid.hpp"
+
 namespace beluga_amcl {
 
 NdtAmclNode3D::NdtAmclNode3D(const rclcpp::NodeOptions& options) : BaseAMCLNode{"ndt_amcl", "", options} {
@@ -152,6 +154,21 @@ NdtAmclNode3D::NdtAmclNode3D(const rclcpp::NodeOptions& options) : BaseAMCLNode{
 
 void NdtAmclNode3D::do_activate(const rclcpp_lifecycle::State&) {
   RCLCPP_INFO(get_logger(), "Making particle filter");
+
+  map_visualization_pub_->on_activate();
+
+  // Publish markers for map visualization
+  beluga_ros::msg::MarkerArray obstacle_markers{};
+  bool visualization_error;
+  std::tie(obstacle_markers, visualization_error) = beluga_ros::assign_obstacle_map(map_, obstacle_markers);
+  if (visualization_error) {
+    RCLCPP_WARN(
+        get_logger(),
+        "Some cell covariances appear to be non-diagonalizable. A cube will be generated instead of an ellipsoid for "
+        "those.");
+  }
+  map_visualization_pub_->publish(obstacle_markers);
+
   particle_filter_ = make_particle_filter();
   {
     using LaserScanSubscriber =
@@ -179,12 +196,29 @@ void NdtAmclNode3D::do_activate(const rclcpp_lifecycle::State&) {
   }
 }
 
+void NdtAmclNode3D::do_configure(const rclcpp_lifecycle::State&) {
+  RCLCPP_INFO(get_logger(), "Configuring");
+  const auto map_path = get_parameter("map_path").as_string();
+  RCLCPP_INFO(get_logger(), "Loading map from %s.", map_path.c_str());
+
+  // Load the map from hdf5 file
+  map_ = beluga::io::load_from_hdf5<NDTMapRepresentation>(get_parameter("map_path").as_string());
+
+  // Map visualization publisher
+  rclcpp::QoS qos_profile{rclcpp::KeepLast(1)};
+  qos_profile.reliable();
+  qos_profile.durability(rclcpp::DurabilityPolicy::TransientLocal);
+
+  map_visualization_pub_ = create_publisher<visualization_msgs::msg::MarkerArray>("obstacle_markers", qos_profile);
+}
+
 void NdtAmclNode3D::do_deactivate(const rclcpp_lifecycle::State&) {
   RCLCPP_INFO(get_logger(), "Cleaning up");
   particle_cloud_pub_.reset();
   laser_scan_connection_.disconnect();
   laser_scan_filter_.reset();
   laser_scan_sub_.reset();
+  map_visualization_pub_.reset();
 }
 
 void NdtAmclNode3D::do_cleanup(const rclcpp_lifecycle::State&) {
@@ -193,6 +227,7 @@ void NdtAmclNode3D::do_cleanup(const rclcpp_lifecycle::State&) {
   pose_pub_.reset();
   particle_filter_.reset();
   enable_tf_broadcast_ = false;
+  map_visualization_pub_.reset();
 }
 
 auto NdtAmclNode3D::get_initial_estimate() const -> std::optional<std::pair<Sophus::SE3d, Sophus::Matrix6d>> {
@@ -242,11 +277,8 @@ beluga::NDTSensorModel<NDTMapRepresentation> NdtAmclNode3D::get_sensor_model() c
   params.minimum_likelihood = get_parameter("minimum_likelihood").as_double();
   params.d1 = get_parameter("d1").as_double();
   params.d2 = get_parameter("d2").as_double();
-  const auto map_path = get_parameter("map_path").as_string();
-  RCLCPP_INFO(get_logger(), "Loading map from %s.", map_path.c_str());
 
-  return beluga::NDTSensorModel<NDTMapRepresentation>{
-      params, beluga::io::load_from_hdf5<NDTMapRepresentation>(get_parameter("map_path").as_string())};
+  return beluga::NDTSensorModel<NDTMapRepresentation>{params, map_};
 }
 auto NdtAmclNode3D::make_particle_filter() const -> std::unique_ptr<NdtAmclVariant> {
   auto amcl = std::visit(

beluga_example/rviz/ndt_amcl_3d.ros2.rviz

@@ -12,8 +12,10 @@ Panels:
         - /MarkerArray2/Topic1
         - /PoseArray1
         - /PoseArray1/Topic1
+        - /MarkerArray3
+        - /MarkerArray3/Topic1
       Splitter Ratio: 0.4967845678329468
-    Tree Height: 796
+    Tree Height: 529
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -80,7 +82,6 @@ Visualization Manager:
       Topic:
         Depth: 5
         Durability Policy: Volatile
-        Filter size: 10
         History Policy: Keep Last
         Reliability Policy: Reliable
         Value: /scan/points
@@ -167,6 +168,18 @@ Visualization Manager:
         Reliability Policy: System Default
         Value: /particle_cloud
       Value: true
+    - Class: rviz_default_plugins/MarkerArray
+      Enabled: true
+      Name: MarkerArray
+      Namespaces:
+        obstacles: true
+      Topic:
+        Depth: 5
+        Durability Policy: Transient Local
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /obstacle_markers
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -236,10 +249,10 @@ Visualization Manager:
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1080
+  Height: 831
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000156000003a5fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000003a5000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000100000003a5fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000003a5000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000037fc0100000002fb0000000800540069006d00650100000000000007800000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000051e000003a500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001560000029ffc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f0000029f000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001000000029ffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003f0000029f000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005fe0000003cfc0100000002fb0000000800540069006d00650100000000000005fe0000026f00fffffffb0000000800540069006d006501000000000000045000000000000000000000039c0000029f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -248,6 +261,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1920
-  X: 0
-  Y: 1080
+  Width: 1534
+  X: 66
+  Y: 32

beluga_ros/cmake/ament.cmake

@@ -24,7 +24,7 @@ find_package(tf2_geometry_msgs REQUIRED)
 find_package(visualization_msgs REQUIRED)
 
 add_library(beluga_ros)
-target_sources(beluga_ros PRIVATE src/amcl.cpp)
+target_sources(beluga_ros PRIVATE src/amcl.cpp src/ndt_ellipsoid.cpp)
 target_include_directories(
   beluga_ros PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
                     $<INSTALL_INTERFACE:include/${PROJECT_NAME}>)

beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp

@@ -0,0 +1,110 @@
+// 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>
+
+/**
+ * \file
+ * \brief Utilities for NDT I/O over ROS interfaces.
+ */
+
+namespace beluga_ros {
+
+namespace detail {
+
+/// Create an ellipoid based on NDT cell data (eigenvalues and eigenvectors) contained in the received marker.
+/**
+ * \param eigen_solver Precreated eigen solver of the NDT cell.
+ * \param cell NDT cell.
+ * \param marker Marker that will contain the message.
+ * \return A boolean set to false if the ellipsoid could not be created (because the covariances are
+ * non-diagonalizable).
+ */
+bool use_mean_covariance(
+    Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 3, 3>>& eigen_solver,
+    beluga::NDTCell<3> cell,
+    beluga_ros::msg::Marker& marker);
+
+/// Create a cube contained in the received marker.
+/**
+ * \param position Center of the cube.
+ * \param size Size of the edges.
+ * \param marker Marker that will contain the message.
+ */
+void use_cell_size(const Eigen::Vector<int, 3>& position, double size, beluga_ros::msg::Marker& marker);
+
+}  // namespace detail
+
+/// Assign an ellipsoid to each cell of a SparseValueGrid. A cube is used instead if the distribution of the
+/// cell is not suitable for the rotation matrix creation.
+/**
+ * \param grid A SparseValueGrid that contains cells representing obstacles.
+ * \param[out] message A MarkerArray that will contain the shapes
+ * \return A std::pair with the MarkerArray containing the shapes and a boolean indicating if at least one cube was
+ * generated. \tparam MapType Container that maps from Eigen::Vector<int, NDim> to the type of the cell. See
+ * [SparseValueGrid]
+ * (https://ekumen-os.github.io/beluga/packages/beluga/docs/_doxygen/generated/reference/html/classbeluga_1_1SparseValueGrid.html).
+ * \tparam NDim Dimension of the grid.
+ */
+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