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,18 @@ 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 flag;
+  beluga_ros::assign_obstacle_map(map_, obstacle_markers, flag);
+  if (flag) {
+    RCLCPP_WARN(get_logger(), "Covariances from map representation cells seem to be non-diagonalizable");
+  }
+  map_visualization_pub_->publish(obstacle_markers);
+
   particle_filter_ = make_particle_filter();
   {
     using LaserScanSubscriber =
@@ -179,12 +193,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 +224,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 +274,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,93 @@
+// 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_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 {
+
+bool use_mean_covariance(
+    Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 3, 3>>& eigenSolver,
+    const 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);
+
+/// 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.
+ * \return A message with the ellipsoids or cubes.
+ * \param[out] message Markers message to be assigned.
+ * \param[out] cubesGenerated Is set to true if there were problems with covariance matrices from cells.
+ * \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>
+beluga_ros::msg::MarkerArray assign_obstacle_map(
+    const beluga::SparseValueGrid<MapType, NDim>& grid,
+    beluga_ros::msg::MarkerArray& message,
+    bool& cubesGenerated) {
+  cubesGenerated = 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>> eigenSolver;
+  for (auto [index, entry] : ranges::views::enumerate(map)) {
+    const auto& [cellCenter, 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 (!use_mean_covariance(eigenSolver, cell, marker)) {
+      // Create a cube based on the resolution of the grid
+      cubesGenerated = true;
+      use_cell_size(cellCenter, grid.resolution(), marker);
+    }
+
+    // Add the marker
+    message.markers.push_back(marker);
+  }
+
+  return message;
+}
+
+}  // namespace beluga_ros
+
+#endif  // BELUGA_ROS_NDT_ELLIPSOID_HPP

beluga_ros/src/ndt_ellipsoid.cpp

@@ -0,0 +1,105 @@
+// 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.
+
+#include <beluga_ros/ndt_ellipsoid.hpp>
+
+#include <Eigen/Core>
+#include <beluga/eigen_compatibility.hpp>
+
+namespace beluga_ros {
+
+namespace {
+
+template <typename RealScalar>
+inline bool isApprox(RealScalar x, RealScalar y, RealScalar prec = Eigen::NumTraits<RealScalar>::dummy_precision()) {
+  return std::abs(x - y) < prec;
+}
+
+}  // namespace
+
+bool use_mean_covariance(
+    Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 3, 3>>& eigenSolver,
+    const beluga::NDTCell<3> cell,
+    beluga_ros::msg::Marker& marker) {
+  eigenSolver.compute(cell.covariance);
+  if (eigenSolver.info() != Eigen::Success) {
+    return false;
+  }
+
+  // Compute the rotation based on the covariance matrix
+  const auto eigenvectors = eigenSolver.eigenvectors().real();
+  const auto eigenvalues = eigenSolver.eigenvalues();
+
+  Eigen::Matrix3d rotationMatrix;
+  Eigen::Vector3d scalevector;
+  rotationMatrix << eigenvectors.col(0), eigenvectors.col(1), eigenvectors.col(2);
+  scalevector = Eigen::Vector3d{eigenvalues.x(), eigenvalues.y(), eigenvalues.z()};
+
+  if (!isApprox(std::abs(rotationMatrix.determinant()), 1.0)) {
+    return false;
+  }
+
+  // Permute columns to ensure the rotation is in fact a rotation
+  if (rotationMatrix.determinant() < 0) {
+    rotationMatrix << eigenvectors.col(1), eigenvectors.col(0), eigenvectors.col(2);
+    scalevector = Eigen::Vector3d{eigenvalues.y(), eigenvalues.x(), eigenvalues.z()};
+  }
+
+  const auto rotation = Eigen::Quaterniond{rotationMatrix};
+
+  // Fill in the message
+  marker.type = beluga_ros::msg::Marker::SPHERE;
+  marker.action = beluga_ros::msg::Marker::ADD;
+
+  marker.pose.position.x = cell.mean[0];
+  marker.pose.position.y = cell.mean[1];
+  marker.pose.position.z = cell.mean[2];
+
+  marker.pose.orientation.x = rotation.x();
+  marker.pose.orientation.y = rotation.y();
+  marker.pose.orientation.z = rotation.z();
+  marker.pose.orientation.w = rotation.w();
+
+  constexpr float kScaleFactor = 4.0f;
+  marker.scale.x = std::sqrt(scalevector.x()) * kScaleFactor;
+  marker.scale.y = std::sqrt(scalevector.y()) * kScaleFactor;
+  marker.scale.z = std::sqrt(scalevector.z()) * kScaleFactor;
+
+  marker.color.r = 0.0f;
+  marker.color.g = 1.0f;
+  marker.color.b = 0.0f;
+  marker.color.a = 1.0f;
+
+  return true;
+}
+
+void use_cell_size(const Eigen::Vector<int, 3>& position, double size, beluga_ros::msg::Marker& marker) {
+  marker.type = beluga_ros::msg::Marker::CUBE;
+  marker.action = beluga_ros::msg::Marker::ADD;
+
+  marker.pose.position.x = position[0];
+  marker.pose.position.y = position[1];
+  marker.pose.position.z = position[2];
+
+  marker.scale.x = size;
+  marker.scale.y = size;
+  marker.scale.z = size;
+
+  marker.color.r = 1.0f;
+  marker.color.g = 0.0f;
+  marker.color.b = 0.0f;
+  marker.color.a = 1.0f;
+}
+
+}  // namespace beluga_ros