From ce99980ee6f27905d59639506fd4bb2793be99be Mon Sep 17 00:00:00 2001 From: Fernando Date: Wed, 6 Nov 2024 14:38:12 +0000 Subject: [PATCH 01/19] Update README (test commit) Signed-off-by: Fernando --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index a4ad2306a..0903d5386 100644 --- a/README.md +++ b/README.md @@ -17,6 +17,8 @@ ## 🌐 Overview +This is a fork from the Ekumen's project Beluga. + Beluga is an extensible C++17 library with a ground-up implementation of the Monte Carlo Localization (MCL) family of estimation algorithms featuring: - A modular design based on orthogonal components. From cea0e8165551ccca2bba1da7096defcdf1009542 Mon Sep 17 00:00:00 2001 From: Fernando Date: Mon, 2 Dec 2024 15:09:16 +0000 Subject: [PATCH 02/19] Initial map visualization Signed-off-by: Fernando --- README.md | 2 - .../include/beluga_amcl/ndt_amcl_node_3d.hpp | 3 + beluga_amcl/src/ndt_amcl_node_3d.cpp | 23 +++++- .../include/beluga_ros/ndt_ellipsoid.hpp | 72 +++++++++++++++++++ 4 files changed, 97 insertions(+), 3 deletions(-) create mode 100644 beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp diff --git a/README.md b/README.md index 0903d5386..a4ad2306a 100644 --- a/README.md +++ b/README.md @@ -17,8 +17,6 @@ ## 🌐 Overview -This is a fork from the Ekumen's project Beluga. - Beluga is an extensible C++17 library with a ground-up implementation of the Monte Carlo Localization (MCL) family of estimation algorithms featuring: - A modular design based on orthogonal components. diff --git a/beluga_amcl/include/beluga_amcl/ndt_amcl_node_3d.hpp b/beluga_amcl/include/beluga_amcl/ndt_amcl_node_3d.hpp index f5edb2ad3..7a6daa5c9 100644 --- a/beluga_amcl/include/beluga_amcl/ndt_amcl_node_3d.hpp +++ b/beluga_amcl/include/beluga_amcl/ndt_amcl_node_3d.hpp @@ -145,6 +145,9 @@ class NdtAmclNode3D : public BaseAMCLNode { std::optional last_known_odom_transform_in_map_; /// Whether to broadcast transforms or not. bool enable_tf_broadcast_{false}; + + /// Map representation publisher. + rclcpp_lifecycle::LifecyclePublisher::SharedPtr map_visualization_pub_; }; } // namespace beluga_amcl diff --git a/beluga_amcl/src/ndt_amcl_node_3d.cpp b/beluga_amcl/src/ndt_amcl_node_3d.cpp index 7b8ac2b3c..487e9a723 100644 --- a/beluga_amcl/src/ndt_amcl_node_3d.cpp +++ b/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,12 @@ NdtAmclNode3D::NdtAmclNode3D(const rclcpp::NodeOptions& options) : BaseAMCLNode{ void NdtAmclNode3D::do_activate(const rclcpp_lifecycle::State&) { RCLCPP_INFO(get_logger(), "Making particle filter"); + + // Map visualization publisher + map_visualization_pub_ = + create_publisher("obstacle_markers", rclcpp::SystemDefaultsQoS()); + RCLCPP_INFO(get_logger(), "Map visualization publisher created"); + particle_filter_ = make_particle_filter(); { using LaserScanSubscriber = @@ -176,6 +184,7 @@ void NdtAmclNode3D::do_activate(const rclcpp_lifecycle::State&) { last_known_odom_transform_in_map_.reset(); initialize_from_estimate(initial_estimate.value()); } + } } @@ -245,8 +254,20 @@ beluga::NDTSensorModel NdtAmclNode3D::get_sensor_model() c 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{ + // params, beluga::io::load_from_hdf5(get_parameter("map_path").as_string())}; + + // Store the map (sparse_value_grid?) + auto map = + beluga::io::load_from_hdf5(get_parameter("map_path").as_string()); + + // Get the markers + beluga_ros::msg::MarkerArray obstacle_markers = beluga_ros::assign_obstacle_map(map); + // Publish the message + map_visualization_pub_->publish(obstacle_markers); + return beluga::NDTSensorModel{ - params, beluga::io::load_from_hdf5(get_parameter("map_path").as_string())}; + params, map}; } auto NdtAmclNode3D::make_particle_filter() const -> std::unique_ptr { auto amcl = std::visit( diff --git a/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp b/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp new file mode 100644 index 000000000..795ed7288 --- /dev/null +++ b/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp @@ -0,0 +1,72 @@ +// 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 + +/** + * \file + * \brief Utilities for NDT I/O over ROS interfaces. + */ + +namespace beluga_ros { + +/// +template +beluga_ros::msg::MarkerArray& assign_obstacle_map(beluga::SparseValueGrid grid){ + + // Get data from the grid + auto& map = grid.data(); + + beluga_ros::msg::MarkerArray message{}; + + int idCount = 0; + for (const auto& [key, cell] : map) + { + beluga_ros::msg::Marker marker; + marker.header.frame_id = "map"; + marker.id = idCount++; // TODO: Compile and check if this is the way to set id + marker.ns = "obstacles"; + marker.type = beluga_ros::msg::Marker::SPHERE; + marker.action = beluga_ros::msg::Marker::ADD; + + // Set the position + // TODO: check the mean's data type and how to access to its members + marker.pose.position.x = cell.mean[0]; + marker.pose.position.y = cell.mean[1]; + marker.pose.position.z = cell.mean[2]; + + // Set the scale based on the covariance + // TODO: check what values of the covariance should be used and if they need be changed before + marker.scale.x = 1; + marker.scale.y = 1; + marker.scale.z = 1; + + marker.color.r = 1.0f; + marker.color.g = 0.0f; + marker.color.b = 0.0f; + marker.color.a = 1.0f; + + // Add the marker + message.markers.push_back(marker); + } + + return message; +} + +} // namespace beluga_ros + +#endif // BELUGA_ROS_NDT_ELLIPSOID_HPP From 5e2c033ad7a9caa25c4ef219817ccecf870ba874 Mon Sep 17 00:00:00 2001 From: Fernando Date: Tue, 3 Dec 2024 16:04:34 +0000 Subject: [PATCH 03/19] Complete ndt map visualization publisher Signed-off-by: Fernando --- .../include/beluga_amcl/ndt_amcl_node_3d.hpp | 3 +- beluga_amcl/src/ndt_amcl_node_3d.cpp | 11 +++- .../include/beluga_ros/ndt_ellipsoid.hpp | 56 ++++++++++--------- 3 files changed, 39 insertions(+), 31 deletions(-) diff --git a/beluga_amcl/include/beluga_amcl/ndt_amcl_node_3d.hpp b/beluga_amcl/include/beluga_amcl/ndt_amcl_node_3d.hpp index 7a6daa5c9..f77a13bb5 100644 --- a/beluga_amcl/include/beluga_amcl/ndt_amcl_node_3d.hpp +++ b/beluga_amcl/include/beluga_amcl/ndt_amcl_node_3d.hpp @@ -147,7 +147,8 @@ class NdtAmclNode3D : public BaseAMCLNode { bool enable_tf_broadcast_{false}; /// Map representation publisher. - rclcpp_lifecycle::LifecyclePublisher::SharedPtr map_visualization_pub_; + //rclcpp_lifecycle::LifecyclePublisher::SharedPtr map_visualization_pub_; + rclcpp::Publisher::SharedPtr map_visualization_pub_; }; } // namespace beluga_amcl diff --git a/beluga_amcl/src/ndt_amcl_node_3d.cpp b/beluga_amcl/src/ndt_amcl_node_3d.cpp index 487e9a723..9594b56ce 100644 --- a/beluga_amcl/src/ndt_amcl_node_3d.cpp +++ b/beluga_amcl/src/ndt_amcl_node_3d.cpp @@ -156,9 +156,12 @@ void NdtAmclNode3D::do_activate(const rclcpp_lifecycle::State&) { RCLCPP_INFO(get_logger(), "Making particle filter"); // Map visualization publisher + rclcpp::QoS qos_profile{rclcpp::KeepLast(1)}; + qos_profile.reliable(); + qos_profile.durability(rclcpp::DurabilityPolicy::TransientLocal); + map_visualization_pub_ = - create_publisher("obstacle_markers", rclcpp::SystemDefaultsQoS()); - RCLCPP_INFO(get_logger(), "Map visualization publisher created"); + create_publisher("obstacle_markers", qos_profile); particle_filter_ = make_particle_filter(); { @@ -258,13 +261,15 @@ beluga::NDTSensorModel NdtAmclNode3D::get_sensor_model() c // params, beluga::io::load_from_hdf5(get_parameter("map_path").as_string())}; // Store the map (sparse_value_grid?) - auto map = + const auto map = beluga::io::load_from_hdf5(get_parameter("map_path").as_string()); // Get the markers beluga_ros::msg::MarkerArray obstacle_markers = beluga_ros::assign_obstacle_map(map); // Publish the message + RCLCPP_INFO(get_logger(), "Publishing obstacle markers"); map_visualization_pub_->publish(obstacle_markers); + RCLCPP_INFO(get_logger(), "Markers published"); return beluga::NDTSensorModel{ params, map}; diff --git a/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp b/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp index 795ed7288..6f49d329f 100644 --- a/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp +++ b/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp @@ -17,6 +17,7 @@ #include + /** * \file * \brief Utilities for NDT I/O over ROS interfaces. @@ -26,7 +27,8 @@ namespace beluga_ros { /// template -beluga_ros::msg::MarkerArray& assign_obstacle_map(beluga::SparseValueGrid grid){ +//beluga_ros::msg::MarkerArray& assign_obstacle_map(beluga::SparseValueGrid grid){ +beluga_ros::msg::MarkerArray assign_obstacle_map(const beluga::SparseValueGrid& grid){ // Get data from the grid auto& map = grid.data(); @@ -36,32 +38,32 @@ beluga_ros::msg::MarkerArray& assign_obstacle_map(beluga::SparseValueGrid Date: Tue, 3 Dec 2024 16:11:18 +0000 Subject: [PATCH 04/19] Suggested changes Signed-off-by: Fernando --- beluga_amcl/src/ndt_amcl_node_3d.cpp | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) diff --git a/beluga_amcl/src/ndt_amcl_node_3d.cpp b/beluga_amcl/src/ndt_amcl_node_3d.cpp index 9594b56ce..27b08c8bb 100644 --- a/beluga_amcl/src/ndt_amcl_node_3d.cpp +++ b/beluga_amcl/src/ndt_amcl_node_3d.cpp @@ -187,7 +187,6 @@ void NdtAmclNode3D::do_activate(const rclcpp_lifecycle::State&) { last_known_odom_transform_in_map_.reset(); initialize_from_estimate(initial_estimate.value()); } - } } @@ -257,19 +256,13 @@ beluga::NDTSensorModel NdtAmclNode3D::get_sensor_model() c 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{ - // params, beluga::io::load_from_hdf5(get_parameter("map_path").as_string())}; - - // Store the map (sparse_value_grid?) + // Load the map from hdf5 file const auto map = beluga::io::load_from_hdf5(get_parameter("map_path").as_string()); - // Get the markers + // Publish markers for map visualization beluga_ros::msg::MarkerArray obstacle_markers = beluga_ros::assign_obstacle_map(map); - // Publish the message - RCLCPP_INFO(get_logger(), "Publishing obstacle markers"); map_visualization_pub_->publish(obstacle_markers); - RCLCPP_INFO(get_logger(), "Markers published"); return beluga::NDTSensorModel{ params, map}; From d383820c1fe2cd6f927c8fbe5a9bfd3a63847aa7 Mon Sep 17 00:00:00 2001 From: Fernando Date: Wed, 4 Dec 2024 15:23:20 +0000 Subject: [PATCH 05/19] Suggested changes Signed-off-by: Fernando --- beluga_amcl/include/beluga_amcl/ndt_amcl_node_3d.hpp | 1 - beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp | 10 ++++++---- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/beluga_amcl/include/beluga_amcl/ndt_amcl_node_3d.hpp b/beluga_amcl/include/beluga_amcl/ndt_amcl_node_3d.hpp index f77a13bb5..5040321f8 100644 --- a/beluga_amcl/include/beluga_amcl/ndt_amcl_node_3d.hpp +++ b/beluga_amcl/include/beluga_amcl/ndt_amcl_node_3d.hpp @@ -147,7 +147,6 @@ class NdtAmclNode3D : public BaseAMCLNode { bool enable_tf_broadcast_{false}; /// Map representation publisher. - //rclcpp_lifecycle::LifecyclePublisher::SharedPtr map_visualization_pub_; rclcpp::Publisher::SharedPtr map_visualization_pub_; }; diff --git a/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp b/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp index 6f49d329f..008971208 100644 --- a/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp +++ b/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp @@ -27,32 +27,34 @@ namespace beluga_ros { /// template -//beluga_ros::msg::MarkerArray& assign_obstacle_map(beluga::SparseValueGrid grid){ beluga_ros::msg::MarkerArray assign_obstacle_map(const beluga::SparseValueGrid& grid){ // Get data from the grid auto& map = grid.data(); beluga_ros::msg::MarkerArray message{}; + { + beluga_ros::msg::Marker marker; + marker.ns = "obstacles"; + marker.action = beluga_ros::msg::Marker::DELETE; + } int idCount = 0; for (const auto& [key, cell] : map) { beluga_ros::msg::Marker marker; marker.header.frame_id = "map"; - marker.id = idCount++; // TODO: Compile and check if this is the way to set id + marker.id = idCount++; marker.ns = "obstacles"; marker.type = beluga_ros::msg::Marker::SPHERE; marker.action = beluga_ros::msg::Marker::ADD; // Set the position - // TODO: check the mean's data type and how to access to its members marker.pose.position.x = cell.mean[0]; marker.pose.position.y = cell.mean[1]; marker.pose.position.z = cell.mean[2]; // Set the scale based on the covariance - // TODO: check what values of the covariance should be used and if they need be changed before marker.scale.x = 0.5f; marker.scale.y = 0.5f; marker.scale.z = 0.5f; From 330f8a93e760d5c2e9dc20d533827c17bbe0cca8 Mon Sep 17 00:00:00 2001 From: Fernando Date: Thu, 5 Dec 2024 16:45:45 +0000 Subject: [PATCH 06/19] Use covariance matrix for rotation and scale Signed-off-by: Fernando --- beluga_amcl/src/ndt_amcl_node_3d.cpp | 9 +-- .../include/beluga_ros/ndt_ellipsoid.hpp | 56 ++++++++++++++----- 2 files changed, 46 insertions(+), 19 deletions(-) diff --git a/beluga_amcl/src/ndt_amcl_node_3d.cpp b/beluga_amcl/src/ndt_amcl_node_3d.cpp index 27b08c8bb..2697e0518 100644 --- a/beluga_amcl/src/ndt_amcl_node_3d.cpp +++ b/beluga_amcl/src/ndt_amcl_node_3d.cpp @@ -160,8 +160,7 @@ void NdtAmclNode3D::do_activate(const rclcpp_lifecycle::State&) { qos_profile.reliable(); qos_profile.durability(rclcpp::DurabilityPolicy::TransientLocal); - map_visualization_pub_ = - create_publisher("obstacle_markers", qos_profile); + map_visualization_pub_ = create_publisher("obstacle_markers", qos_profile); particle_filter_ = make_particle_filter(); { @@ -257,15 +256,13 @@ beluga::NDTSensorModel NdtAmclNode3D::get_sensor_model() c RCLCPP_INFO(get_logger(), "Loading map from %s.", map_path.c_str()); // Load the map from hdf5 file - const auto map = - beluga::io::load_from_hdf5(get_parameter("map_path").as_string()); + const auto map = beluga::io::load_from_hdf5(get_parameter("map_path").as_string()); // Publish markers for map visualization beluga_ros::msg::MarkerArray obstacle_markers = beluga_ros::assign_obstacle_map(map); map_visualization_pub_->publish(obstacle_markers); - return beluga::NDTSensorModel{ - params, map}; + return beluga::NDTSensorModel{params, map}; } auto NdtAmclNode3D::make_particle_filter() const -> std::unique_ptr { auto amcl = std::visit( diff --git a/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp b/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp index 008971208..56b98f3f3 100644 --- a/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp +++ b/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp @@ -17,7 +17,6 @@ #include - /** * \file * \brief Utilities for NDT I/O over ROS interfaces. @@ -25,23 +24,23 @@ namespace beluga_ros { -/// +/// template -beluga_ros::msg::MarkerArray assign_obstacle_map(const beluga::SparseValueGrid& grid){ - +beluga_ros::msg::MarkerArray assign_obstacle_map(const beluga::SparseValueGrid& grid) { // Get data from the grid auto& map = grid.data(); + // Clean up the message beluga_ros::msg::MarkerArray message{}; { beluga_ros::msg::Marker marker; marker.ns = "obstacles"; - marker.action = beluga_ros::msg::Marker::DELETE; + marker.action = beluga_ros::msg::Marker::DELETEALL; + message.markers.push_back(marker); } int idCount = 0; - for (const auto& [key, cell] : map) - { + for (const auto& [key, cell] : map) { beluga_ros::msg::Marker marker; marker.header.frame_id = "map"; marker.id = idCount++; @@ -54,14 +53,45 @@ beluga_ros::msg::MarkerArray assign_obstacle_map(const beluga::SparseValueGrid>{cell.covariance}; + assert(eigenSolver.info() == Eigen::Success); + const auto eigenvectors = eigenSolver.eigenvectors().real(); + const auto eigenvalues = eigenSolver.eigenvalues(); + + // TODO: Discuss a better way to do this (threshold and permutation) + Eigen::Matrix3d rotationMatrix; + rotationMatrix << eigenvectors.col(0), eigenvectors.col(1), eigenvectors.col(2); + const auto scalevector = Eigen::Vector3d{eigenvalues.x(), eigenvalues.y(), eigenvalues.z()}; + + // Permutation + if (std::abs(rotationMatrix.determinant() - 1.0) > 1e-6) { + rotationMatrix << eigenvectors.col(1), eigenvectors.col(0), eigenvectors.col(2); + scalevector = Eigen::Vector3d{eigenvalues.y(), eigenvalues.x(), eigenvalues.z()}; + + if (std::abs(rotationMatrix.determinant() - 1.0) > 1e-6) { + rotationMatrix << eigenvectors.col(0), eigenvectors.col(2), eigenvectors.col(1); + scalevector = Eigen::Vector3d{eigenvalues.x(), eigenvalues.z(), eigenvalues.y()}; + } + } + + Eigen::Quaterniond rotation; + if (std::abs(rotationMatrix.determinant() - 1.0) < 1e-6) { + rotation = Eigen::Quaterniond{rotationMatrix}; + } + + marker.pose.orientation.x = rotation.x(); + marker.pose.orientation.y = rotation.y(); + marker.pose.orientation.z = rotation.z(); + marker.pose.orientation.w = rotation.w(); + + marker.scale.x = scalevector.x(); + marker.scale.y = scalevector.y(); + marker.scale.z = scalevector.z(); marker.color.r = 0.0f; - marker.color.g = 0.0f; - marker.color.b = 1.0f; + marker.color.g = 1.0f; + marker.color.b = 0.0f; marker.color.a = 1.0f; // Add the marker From a46ddbd38101ec374b00eb2e5982ac51abfa0a4c Mon Sep 17 00:00:00 2001 From: Fernando Date: Mon, 9 Dec 2024 16:11:13 +0000 Subject: [PATCH 07/19] Fix error and suggested changes Signed-off-by: Fernando --- beluga_example/rviz/ndt_amcl_3d.ros2.rviz | 27 ++++++++++++++----- .../include/beluga_ros/ndt_ellipsoid.hpp | 13 +++------ 2 files changed, 24 insertions(+), 16 deletions(-) diff --git a/beluga_example/rviz/ndt_amcl_3d.ros2.rviz b/beluga_example/rviz/ndt_amcl_3d.ros2.rviz index 4cd67d958..e84fd62a7 100644 --- a/beluga_example/rviz/ndt_amcl_3d.ros2.rviz +++ b/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 diff --git a/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp b/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp index 56b98f3f3..f9cbb9f24 100644 --- a/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp +++ b/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp @@ -59,24 +59,19 @@ beluga_ros::msg::MarkerArray assign_obstacle_map(const beluga::SparseValueGrid 1e-6) { + if (std::abs(rotationMatrix.determinant() - 1.0) > Eigen::NumTraits::dummy_precision()) { rotationMatrix << eigenvectors.col(1), eigenvectors.col(0), eigenvectors.col(2); scalevector = Eigen::Vector3d{eigenvalues.y(), eigenvalues.x(), eigenvalues.z()}; - - if (std::abs(rotationMatrix.determinant() - 1.0) > 1e-6) { - rotationMatrix << eigenvectors.col(0), eigenvectors.col(2), eigenvectors.col(1); - scalevector = Eigen::Vector3d{eigenvalues.x(), eigenvalues.z(), eigenvalues.y()}; - } } Eigen::Quaterniond rotation; - if (std::abs(rotationMatrix.determinant() - 1.0) < 1e-6) { + if (std::abs(rotationMatrix.determinant() - 1.0) < Eigen::NumTraits::dummy_precision()) { rotation = Eigen::Quaterniond{rotationMatrix}; } From 3eef273a6ef7bc48c1a48f0409f0176f82671d6a Mon Sep 17 00:00:00 2001 From: Fernando Date: Mon, 9 Dec 2024 17:01:03 +0000 Subject: [PATCH 08/19] Add documentation Signed-off-by: Fernando --- beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp b/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp index f9cbb9f24..c3d8a3b61 100644 --- a/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp +++ b/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp @@ -24,7 +24,15 @@ namespace beluga_ros { -/// +/// 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. + * \tparam MapType Container that maps from Eigen::Vector 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 beluga_ros::msg::MarkerArray assign_obstacle_map(const beluga::SparseValueGrid& grid) { // Get data from the grid From 1c62da97057ab3227405015671fbfa87b596dcc4 Mon Sep 17 00:00:00 2001 From: Fernando Date: Tue, 10 Dec 2024 15:08:03 +0000 Subject: [PATCH 09/19] Add eigen error handling Signed-off-by: Fernando --- .../include/beluga_ros/ndt_ellipsoid.hpp | 93 ++++++++++++------- 1 file changed, 58 insertions(+), 35 deletions(-) diff --git a/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp b/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp index c3d8a3b61..b7a6dc39b 100644 --- a/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp +++ b/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp @@ -24,6 +24,57 @@ namespace beluga_ros { +static void use_mean_covariance(const auto& mean, const auto& eigenSolver, auto& marker){ + marker.type = beluga_ros::msg::Marker::SPHERE; + marker.action = beluga_ros::msg::Marker::ADD; + + // 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()}; + + // Permutation + if (std::abs(rotationMatrix.determinant() - 1.0) > Eigen::NumTraits::dummy_precision()) { + rotationMatrix << eigenvectors.col(1), eigenvectors.col(0), eigenvectors.col(2); + scalevector = Eigen::Vector3d{eigenvalues.y(), eigenvalues.x(), eigenvalues.z()}; + } + + Eigen::Quaterniond rotation{1.0f, 0.0f, 0.0f, 0.0f}; + if (std::abs(rotationMatrix.determinant() - 1.0) < Eigen::NumTraits::dummy_precision()) { + rotation = Eigen::Quaterniond{rotationMatrix}; + } + + marker.pose.position.x = mean[0]; + marker.pose.position.y = mean[1]; + marker.pose.position.z = 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(); + + marker.scale.x = scalevector.x(); + marker.scale.y = scalevector.y(); + marker.scale.z = scalevector.z(); +} + +static void use_cell_size(const auto& position, const auto& size, auto& 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; +} + /// 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. /** @@ -47,51 +98,23 @@ beluga_ros::msg::MarkerArray assign_obstacle_map(const beluga::SparseValueGrid>{cell.covariance}; - assert(eigenSolver.info() == Eigen::Success); - 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()}; - - // Permutation - if (std::abs(rotationMatrix.determinant() - 1.0) > Eigen::NumTraits::dummy_precision()) { - rotationMatrix << eigenvectors.col(1), eigenvectors.col(0), eigenvectors.col(2); - scalevector = Eigen::Vector3d{eigenvalues.y(), eigenvalues.x(), eigenvalues.z()}; + if(eigenSolver.info() == Eigen::Success){ + // Create an ellipsoid with values of the cell + use_mean_covariance(cell.mean, eigenSolver, marker); + }else{ + // Create a cube based on the resolution of the grid + use_cell_size(key, grid.resolution(), marker); } - Eigen::Quaterniond rotation; - if (std::abs(rotationMatrix.determinant() - 1.0) < Eigen::NumTraits::dummy_precision()) { - rotation = Eigen::Quaterniond{rotationMatrix}; - } - - marker.pose.orientation.x = rotation.x(); - marker.pose.orientation.y = rotation.y(); - marker.pose.orientation.z = rotation.z(); - marker.pose.orientation.w = rotation.w(); - - marker.scale.x = scalevector.x(); - marker.scale.y = scalevector.y(); - marker.scale.z = scalevector.z(); - marker.color.r = 0.0f; marker.color.g = 1.0f; marker.color.b = 0.0f; From 72aeac32e2adbc59a6584d9da2e35da39ec58908 Mon Sep 17 00:00:00 2001 From: Fernando Date: Wed, 11 Dec 2024 16:19:29 +0000 Subject: [PATCH 10/19] Scale factor and other suggested changes Signed-off-by: Fernando --- .../include/beluga_ros/ndt_ellipsoid.hpp | 28 ++++++++++--------- 1 file changed, 15 insertions(+), 13 deletions(-) diff --git a/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp b/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp index b7a6dc39b..2919be723 100644 --- a/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp +++ b/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp @@ -24,7 +24,7 @@ namespace beluga_ros { -static void use_mean_covariance(const auto& mean, const auto& eigenSolver, auto& marker){ +inline void use_mean_covariance(const auto& mean, const auto& eigenSolver, auto& marker){ marker.type = beluga_ros::msg::Marker::SPHERE; marker.action = beluga_ros::msg::Marker::ADD; @@ -57,12 +57,13 @@ static void use_mean_covariance(const auto& mean, const auto& eigenSolver, auto& marker.pose.orientation.z = rotation.z(); marker.pose.orientation.w = rotation.w(); - marker.scale.x = scalevector.x(); - marker.scale.y = scalevector.y(); - marker.scale.z = scalevector.z(); + float scaleConstant = 5.0f; + marker.scale.x = scalevector.x() * scaleConstant; + marker.scale.y = scalevector.y() * scaleConstant; + marker.scale.z = scalevector.z() * scaleConstant; } -static void use_cell_size(const auto& position, const auto& size, auto& marker){ +inline void use_cell_size(const auto& position, const auto& size, auto& marker){ marker.type = beluga_ros::msg::Marker::CUBE; marker.action = beluga_ros::msg::Marker::ADD; @@ -99,11 +100,11 @@ beluga_ros::msg::MarkerArray assign_obstacle_map(const beluga::SparseValueGrid>{cell.covariance}; @@ -112,13 +113,14 @@ beluga_ros::msg::MarkerArray assign_obstacle_map(const beluga::SparseValueGrid Date: Thu, 12 Dec 2024 13:53:57 +0000 Subject: [PATCH 11/19] Update documentation and eigen solver usage Signed-off-by: Fernando --- beluga_amcl/src/ndt_amcl_node_3d.cpp | 3 +- .../include/beluga_ros/ndt_ellipsoid.hpp | 34 ++++++++++--------- 2 files changed, 20 insertions(+), 17 deletions(-) diff --git a/beluga_amcl/src/ndt_amcl_node_3d.cpp b/beluga_amcl/src/ndt_amcl_node_3d.cpp index 2697e0518..5bfde6bab 100644 --- a/beluga_amcl/src/ndt_amcl_node_3d.cpp +++ b/beluga_amcl/src/ndt_amcl_node_3d.cpp @@ -259,7 +259,8 @@ beluga::NDTSensorModel NdtAmclNode3D::get_sensor_model() c const auto map = beluga::io::load_from_hdf5(get_parameter("map_path").as_string()); // Publish markers for map visualization - beluga_ros::msg::MarkerArray obstacle_markers = beluga_ros::assign_obstacle_map(map); + beluga_ros::msg::MarkerArray obstacle_markers{}; + beluga_ros::assign_obstacle_map(map, obstacle_markers); map_visualization_pub_->publish(obstacle_markers); return beluga::NDTSensorModel{params, map}; diff --git a/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp b/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp index 2919be723..8d8ac6c2a 100644 --- a/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp +++ b/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp @@ -78,28 +78,30 @@ inline void use_cell_size(const auto& position, const auto& size, auto& 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. - * \tparam MapType Container that maps from Eigen::Vector 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. - */ +/** + * \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. + * \tparam MapType Container that maps from Eigen::Vector 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 -beluga_ros::msg::MarkerArray assign_obstacle_map(const beluga::SparseValueGrid& grid) { +beluga_ros::msg::MarkerArray assign_obstacle_map( + const beluga::SparseValueGrid& grid, + beluga_ros::msg::MarkerArray& message) { + // Get data from the grid auto& map = grid.data(); // Clean up the message - beluga_ros::msg::MarkerArray message{}; - { - beluga_ros::msg::Marker marker; - marker.ns = "obstacles"; - marker.action = beluga_ros::msg::Marker::DELETEALL; - message.markers.push_back(marker); - } + 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> eigenSolver; for (auto [index, entry] : ranges::views::enumerate(map)) { const auto [cellCenter, cell] = entry; beluga_ros::msg::Marker marker; @@ -107,7 +109,7 @@ beluga_ros::msg::MarkerArray assign_obstacle_map(const beluga::SparseValueGrid>{cell.covariance}; + eigenSolver.compute(cell.covariance); if(eigenSolver.info() == Eigen::Success){ // Create an ellipsoid with values of the cell use_mean_covariance(cell.mean, eigenSolver, marker); From 36dd062434159790c3e621f8107604830588ad58 Mon Sep 17 00:00:00 2001 From: Fernando Date: Thu, 12 Dec 2024 16:22:41 +0000 Subject: [PATCH 12/19] Boolean function and suggested changes Signed-off-by: Fernando --- .../include/beluga_ros/ndt_ellipsoid.hpp | 43 +++++++++++-------- 1 file changed, 24 insertions(+), 19 deletions(-) diff --git a/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp b/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp index 8d8ac6c2a..d25b2b3c9 100644 --- a/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp +++ b/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp @@ -24,9 +24,11 @@ namespace beluga_ros { -inline void use_mean_covariance(const auto& mean, const auto& eigenSolver, auto& marker){ - marker.type = beluga_ros::msg::Marker::SPHERE; - marker.action = beluga_ros::msg::Marker::ADD; +inline bool use_mean_covariance(auto& eigenSolver, const auto& cell, auto& 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(); @@ -43,24 +45,30 @@ inline void use_mean_covariance(const auto& mean, const auto& eigenSolver, auto& scalevector = Eigen::Vector3d{eigenvalues.y(), eigenvalues.x(), eigenvalues.z()}; } - Eigen::Quaterniond rotation{1.0f, 0.0f, 0.0f, 0.0f}; - if (std::abs(rotationMatrix.determinant() - 1.0) < Eigen::NumTraits::dummy_precision()) { - rotation = Eigen::Quaterniond{rotationMatrix}; + if (std::abs(rotationMatrix.determinant() - 1.0) > Eigen::NumTraits::dummy_precision()) { + return false; } + const auto rotation = Eigen::Quaterniond{rotationMatrix}; - marker.pose.position.x = mean[0]; - marker.pose.position.y = mean[1]; - marker.pose.position.z = mean[2]; + // 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(); - float scaleConstant = 5.0f; - marker.scale.x = scalevector.x() * scaleConstant; - marker.scale.y = scalevector.y() * scaleConstant; - marker.scale.z = scalevector.z() * scaleConstant; + float scaleConstant = 2.0f; + marker.scale.x = std::sqrt(scalevector.x()) * scaleConstant; + marker.scale.y = std::sqrt(scalevector.y()) * scaleConstant; + marker.scale.z = std::sqrt(scalevector.z()) * scaleConstant; + + return true; } inline void use_cell_size(const auto& position, const auto& size, auto& marker){ @@ -103,17 +111,14 @@ beluga_ros::msg::MarkerArray assign_obstacle_map( // Add the markers Eigen::SelfAdjointEigenSolver> eigenSolver; for (auto [index, entry] : ranges::views::enumerate(map)) { - const auto [cellCenter, cell] = entry; + const auto& [cellCenter, cell] = entry; beluga_ros::msg::Marker marker; marker.header.frame_id = "map"; marker.id = index; marker.ns = "obstacles"; - eigenSolver.compute(cell.covariance); - if(eigenSolver.info() == Eigen::Success){ - // Create an ellipsoid with values of the cell - use_mean_covariance(cell.mean, eigenSolver, marker); - }else{ + // 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 use_cell_size(cellCenter, grid.resolution(), marker); } From 1d30dcfe5c38312983bcdbc6870a59cb974e3b95 Mon Sep 17 00:00:00 2001 From: Fernando Date: Thu, 12 Dec 2024 16:33:04 +0000 Subject: [PATCH 13/19] isApprox function Signed-off-by: Fernando --- beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp b/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp index d25b2b3c9..486d9e854 100644 --- a/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp +++ b/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp @@ -24,6 +24,11 @@ namespace beluga_ros { +template +inline bool isApprox(RealScalar x, RealScalar y, RealScalar prec = Eigen::NumTraits::dummy_precision()){ + return std::abs(x-y) < prec; +} + inline bool use_mean_covariance(auto& eigenSolver, const auto& cell, auto& marker){ eigenSolver.compute(cell.covariance); if(eigenSolver.info() != Eigen::Success){ @@ -40,12 +45,12 @@ inline bool use_mean_covariance(auto& eigenSolver, const auto& cell, auto& marke scalevector = Eigen::Vector3d{eigenvalues.x(), eigenvalues.y(), eigenvalues.z()}; // Permutation - if (std::abs(rotationMatrix.determinant() - 1.0) > Eigen::NumTraits::dummy_precision()) { + if(!isApprox(rotationMatrix.determinant(), 1.0)) { rotationMatrix << eigenvectors.col(1), eigenvectors.col(0), eigenvectors.col(2); scalevector = Eigen::Vector3d{eigenvalues.y(), eigenvalues.x(), eigenvalues.z()}; } - if (std::abs(rotationMatrix.determinant() - 1.0) > Eigen::NumTraits::dummy_precision()) { + if(!isApprox(rotationMatrix.determinant(), 1.0)) { return false; } const auto rotation = Eigen::Quaterniond{rotationMatrix}; From 57e25141d0d13d18882375f4edfcb41aaaf5e7e2 Mon Sep 17 00:00:00 2001 From: Fernando Date: Fri, 13 Dec 2024 17:01:58 +0000 Subject: [PATCH 14/19] Warning message, scale factor and other suggested changes Signed-off-by: Fernando --- beluga_amcl/src/ndt_amcl_node_3d.cpp | 6 +- .../include/beluga_ros/ndt_ellipsoid.hpp | 55 +++++++++++-------- 2 files changed, 38 insertions(+), 23 deletions(-) diff --git a/beluga_amcl/src/ndt_amcl_node_3d.cpp b/beluga_amcl/src/ndt_amcl_node_3d.cpp index 5bfde6bab..60e1f2fb2 100644 --- a/beluga_amcl/src/ndt_amcl_node_3d.cpp +++ b/beluga_amcl/src/ndt_amcl_node_3d.cpp @@ -260,7 +260,11 @@ beluga::NDTSensorModel NdtAmclNode3D::get_sensor_model() c // Publish markers for map visualization beluga_ros::msg::MarkerArray obstacle_markers{}; - beluga_ros::assign_obstacle_map(map, 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); return beluga::NDTSensorModel{params, map}; diff --git a/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp b/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp index 486d9e854..0c7190178 100644 --- a/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp +++ b/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp @@ -24,17 +24,21 @@ namespace beluga_ros { +namespace { + template -inline bool isApprox(RealScalar x, RealScalar y, RealScalar prec = Eigen::NumTraits::dummy_precision()){ - return std::abs(x-y) < prec; +inline bool isApprox(RealScalar x, RealScalar y, RealScalar prec = Eigen::NumTraits::dummy_precision()) { + return std::abs(x - y) < prec; } -inline bool use_mean_covariance(auto& eigenSolver, const auto& cell, auto& marker){ +} // namespace + +inline bool use_mean_covariance(auto& eigenSolver, const auto& cell, auto& marker) { eigenSolver.compute(cell.covariance); - if(eigenSolver.info() != Eigen::Success){ + 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(); @@ -45,12 +49,12 @@ inline bool use_mean_covariance(auto& eigenSolver, const auto& cell, auto& marke scalevector = Eigen::Vector3d{eigenvalues.x(), eigenvalues.y(), eigenvalues.z()}; // Permutation - if(!isApprox(rotationMatrix.determinant(), 1.0)) { + if (!isApprox(rotationMatrix.determinant(), 1.0)) { rotationMatrix << eigenvectors.col(1), eigenvectors.col(0), eigenvectors.col(2); scalevector = Eigen::Vector3d{eigenvalues.y(), eigenvalues.x(), eigenvalues.z()}; } - if(!isApprox(rotationMatrix.determinant(), 1.0)) { + if (!isApprox(rotationMatrix.determinant(), 1.0)) { return false; } const auto rotation = Eigen::Quaterniond{rotationMatrix}; @@ -68,15 +72,20 @@ inline bool use_mean_covariance(auto& eigenSolver, const auto& cell, auto& marke marker.pose.orientation.z = rotation.z(); marker.pose.orientation.w = rotation.w(); - float scaleConstant = 2.0f; - marker.scale.x = std::sqrt(scalevector.x()) * scaleConstant; - marker.scale.y = std::sqrt(scalevector.y()) * scaleConstant; - marker.scale.z = std::sqrt(scalevector.z()) * scaleConstant; + 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; } -inline void use_cell_size(const auto& position, const auto& size, auto& marker){ +inline void use_cell_size(const auto& position, const auto& size, auto& marker) { marker.type = beluga_ros::msg::Marker::CUBE; marker.action = beluga_ros::msg::Marker::ADD; @@ -87,14 +96,20 @@ inline void use_cell_size(const auto& position, const auto& size, auto& marker){ 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; } /// 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 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. @@ -102,8 +117,9 @@ inline void use_cell_size(const auto& position, const auto& size, auto& marker){ template beluga_ros::msg::MarkerArray assign_obstacle_map( const beluga::SparseValueGrid& grid, - beluga_ros::msg::MarkerArray& message) { - + beluga_ros::msg::MarkerArray& message, + bool& cubesGenerated) { + cubesGenerated = false; // Get data from the grid auto& map = grid.data(); @@ -123,17 +139,12 @@ beluga_ros::msg::MarkerArray assign_obstacle_map( marker.ns = "obstacles"; // Try to create an ellipsoid with values of the cell - if(!use_mean_covariance(eigenSolver, cell, marker)){ + 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); } - float color[] = {0.0f, 1.0f, 0.0f, 1.0f}; - marker.color.r = color[0]; - marker.color.g = color[1]; - marker.color.b = color[2]; - marker.color.a = color[3]; - // Add the marker message.markers.push_back(marker); } From c28940f4f1ae194c0a06b596a9ae1d6c58b6de8f Mon Sep 17 00:00:00 2001 From: Fernando Date: Tue, 7 Jan 2025 17:03:33 +0000 Subject: [PATCH 15/19] Change to LifecyclePublisher Signed-off-by: Fernando --- .../include/beluga_amcl/ndt_amcl_node_3d.hpp | 5 ++++- beluga_amcl/src/ndt_amcl_node_3d.cpp | 20 +++++++++++++------ 2 files changed, 18 insertions(+), 7 deletions(-) diff --git a/beluga_amcl/include/beluga_amcl/ndt_amcl_node_3d.hpp b/beluga_amcl/include/beluga_amcl/ndt_amcl_node_3d.hpp index 5040321f8..c5ea482a0 100644 --- a/beluga_amcl/include/beluga_amcl/ndt_amcl_node_3d.hpp +++ b/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; @@ -147,7 +150,7 @@ class NdtAmclNode3D : public BaseAMCLNode { bool enable_tf_broadcast_{false}; /// Map representation publisher. - rclcpp::Publisher::SharedPtr map_visualization_pub_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr map_visualization_pub_; }; } // namespace beluga_amcl diff --git a/beluga_amcl/src/ndt_amcl_node_3d.cpp b/beluga_amcl/src/ndt_amcl_node_3d.cpp index 60e1f2fb2..b658e4284 100644 --- a/beluga_amcl/src/ndt_amcl_node_3d.cpp +++ b/beluga_amcl/src/ndt_amcl_node_3d.cpp @@ -155,12 +155,7 @@ NdtAmclNode3D::NdtAmclNode3D(const rclcpp::NodeOptions& options) : BaseAMCLNode{ void NdtAmclNode3D::do_activate(const rclcpp_lifecycle::State&) { RCLCPP_INFO(get_logger(), "Making particle filter"); - // Map visualization publisher - rclcpp::QoS qos_profile{rclcpp::KeepLast(1)}; - qos_profile.reliable(); - qos_profile.durability(rclcpp::DurabilityPolicy::TransientLocal); - - map_visualization_pub_ = create_publisher("obstacle_markers", qos_profile); + map_visualization_pub_->on_activate(); particle_filter_ = make_particle_filter(); { @@ -189,12 +184,24 @@ void NdtAmclNode3D::do_activate(const rclcpp_lifecycle::State&) { } } +void NdtAmclNode3D::do_configure(const rclcpp_lifecycle::State&) { + RCLCPP_INFO(get_logger(), "Configuring"); + + // Map visualization publisher + rclcpp::QoS qos_profile{rclcpp::KeepLast(1)}; + qos_profile.reliable(); + qos_profile.durability(rclcpp::DurabilityPolicy::TransientLocal); + + map_visualization_pub_ = create_publisher("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&) { @@ -203,6 +210,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> { From 8f398f6824f49c271e4ca9717c4dd9ea1434092c Mon Sep 17 00:00:00 2001 From: Fernando Date: Wed, 8 Jan 2025 15:08:09 +0000 Subject: [PATCH 16/19] Create .cpp file Signed-off-by: Fernando --- beluga_ros/cmake/ament.cmake | 5 +- .../include/beluga_ros/ndt_ellipsoid.hpp | 95 +++------------ beluga_ros/src/ndt_ellipsoid.cpp | 108 ++++++++++++++++++ 3 files changed, 129 insertions(+), 79 deletions(-) create mode 100644 beluga_ros/src/ndt_ellipsoid.cpp diff --git a/beluga_ros/cmake/ament.cmake b/beluga_ros/cmake/ament.cmake index f61121218..67e55aad1 100644 --- a/beluga_ros/cmake/ament.cmake +++ b/beluga_ros/cmake/ament.cmake @@ -24,7 +24,10 @@ 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 $ $) diff --git a/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp b/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp index 0c7190178..db7081929 100644 --- a/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp +++ b/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp @@ -17,6 +17,14 @@ #include +#include +#include + +#include + +#include +#include + /** * \file * \brief Utilities for NDT I/O over ROS interfaces. @@ -24,84 +32,15 @@ namespace beluga_ros { -namespace { +bool use_mean_covariance( + Eigen::SelfAdjointEigenSolver>& eigenSolver, + const beluga::NDTCell<3> cell, + beluga_ros::msg::Marker& marker); -template -inline bool isApprox(RealScalar x, RealScalar y, RealScalar prec = Eigen::NumTraits::dummy_precision()) { - return std::abs(x - y) < prec; -} - -} // namespace - -inline bool use_mean_covariance(auto& eigenSolver, const auto& cell, auto& 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()}; - - // Permutation - if (!isApprox(rotationMatrix.determinant(), 1.0)) { - rotationMatrix << eigenvectors.col(1), eigenvectors.col(0), eigenvectors.col(2); - scalevector = Eigen::Vector3d{eigenvalues.y(), eigenvalues.x(), eigenvalues.z()}; - } - - if (!isApprox(rotationMatrix.determinant(), 1.0)) { - return false; - } - 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; -} - -inline void use_cell_size(const auto& position, const auto& size, auto& 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; -} +void use_cell_size( + const Eigen::Vector& 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. @@ -135,7 +74,7 @@ beluga_ros::msg::MarkerArray assign_obstacle_map( const auto& [cellCenter, cell] = entry; beluga_ros::msg::Marker marker; marker.header.frame_id = "map"; - marker.id = index; + marker.id = index+1; marker.ns = "obstacles"; // Try to create an ellipsoid with values of the cell diff --git a/beluga_ros/src/ndt_ellipsoid.cpp b/beluga_ros/src/ndt_ellipsoid.cpp new file mode 100644 index 000000000..4bae834fa --- /dev/null +++ b/beluga_ros/src/ndt_ellipsoid.cpp @@ -0,0 +1,108 @@ +// 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 + +#include +#include + +namespace beluga_ros { + +namespace { + +template +inline bool isApprox(RealScalar x, RealScalar y, RealScalar prec = Eigen::NumTraits::dummy_precision()) { + return std::abs(x - y) < prec; +} + +} + +bool use_mean_covariance( + Eigen::SelfAdjointEigenSolver>& 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()}; + + // Permutation + if(!isApprox(rotationMatrix.determinant(), 1.0)) { + rotationMatrix << eigenvectors.col(1), eigenvectors.col(0), eigenvectors.col(2); + scalevector = Eigen::Vector3d{eigenvalues.y(), eigenvalues.x(), eigenvalues.z()}; + } + + if(!isApprox(rotationMatrix.determinant(), 1.0)) { + return false; + } + 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& 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 From f2c4fb86139572ff061e63f1c9f66fff17cab4b5 Mon Sep 17 00:00:00 2001 From: Fernando Date: Wed, 8 Jan 2025 15:35:02 +0000 Subject: [PATCH 17/19] Separate creation, loading and publishing Signed-off-by: Fernando --- .../include/beluga_amcl/ndt_amcl_node_3d.hpp | 5 +++- beluga_amcl/src/ndt_amcl_node_3d.cpp | 30 +++++++++---------- 2 files changed, 19 insertions(+), 16 deletions(-) diff --git a/beluga_amcl/include/beluga_amcl/ndt_amcl_node_3d.hpp b/beluga_amcl/include/beluga_amcl/ndt_amcl_node_3d.hpp index c5ea482a0..3cb4f1474 100644 --- a/beluga_amcl/include/beluga_amcl/ndt_amcl_node_3d.hpp +++ b/beluga_amcl/include/beluga_amcl/ndt_amcl_node_3d.hpp @@ -140,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 particle_filter_; /// Last known pose estimate, if any. @@ -149,7 +152,7 @@ class NdtAmclNode3D : public BaseAMCLNode { /// Whether to broadcast transforms or not. bool enable_tf_broadcast_{false}; - /// Map representation publisher. + /// Map visualization publisher. rclcpp_lifecycle::LifecyclePublisher::SharedPtr map_visualization_pub_; }; diff --git a/beluga_amcl/src/ndt_amcl_node_3d.cpp b/beluga_amcl/src/ndt_amcl_node_3d.cpp index b658e4284..e1405077e 100644 --- a/beluga_amcl/src/ndt_amcl_node_3d.cpp +++ b/beluga_amcl/src/ndt_amcl_node_3d.cpp @@ -157,6 +157,15 @@ void NdtAmclNode3D::do_activate(const rclcpp_lifecycle::State&) { 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 = @@ -186,6 +195,11 @@ 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(get_parameter("map_path").as_string()); // Map visualization publisher rclcpp::QoS qos_profile{rclcpp::KeepLast(1)}; @@ -260,22 +274,8 @@ beluga::NDTSensorModel 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()); - - // Load the map from hdf5 file - const auto map = beluga::io::load_from_hdf5(get_parameter("map_path").as_string()); - - // 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); - return beluga::NDTSensorModel{params, map}; + return beluga::NDTSensorModel{params, map_}; } auto NdtAmclNode3D::make_particle_filter() const -> std::unique_ptr { auto amcl = std::visit( From 3c0516ea3ad31216458422d17dd4c82e3595790e Mon Sep 17 00:00:00 2001 From: Fernando Date: Wed, 8 Jan 2025 16:07:24 +0000 Subject: [PATCH 18/19] Change permutation order Signed-off-by: Fernando --- beluga_ros/src/ndt_ellipsoid.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/beluga_ros/src/ndt_ellipsoid.cpp b/beluga_ros/src/ndt_ellipsoid.cpp index 4bae834fa..54ea69b14 100644 --- a/beluga_ros/src/ndt_ellipsoid.cpp +++ b/beluga_ros/src/ndt_ellipsoid.cpp @@ -46,15 +46,16 @@ bool use_mean_covariance( rotationMatrix << eigenvectors.col(0), eigenvectors.col(1), eigenvectors.col(2); scalevector = Eigen::Vector3d{eigenvalues.x(), eigenvalues.y(), eigenvalues.z()}; - // Permutation - if(!isApprox(rotationMatrix.determinant(), 1.0)) { + 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()}; } - if(!isApprox(rotationMatrix.determinant(), 1.0)) { - return false; - } const auto rotation = Eigen::Quaterniond{rotationMatrix}; // Fill in the message From 6421a3f15fb66d766d47efcd64d4667664b84ec0 Mon Sep 17 00:00:00 2001 From: Fernando Date: Fri, 10 Jan 2025 14:14:25 +0000 Subject: [PATCH 19/19] Fix linter errors Signed-off-by: Fernando --- beluga_ros/cmake/ament.cmake | 5 +---- .../include/beluga_ros/ndt_ellipsoid.hpp | 9 +++------ beluga_ros/src/ndt_ellipsoid.cpp | 20 ++++++++----------- 3 files changed, 12 insertions(+), 22 deletions(-) diff --git a/beluga_ros/cmake/ament.cmake b/beluga_ros/cmake/ament.cmake index 67e55aad1..756a07301 100644 --- a/beluga_ros/cmake/ament.cmake +++ b/beluga_ros/cmake/ament.cmake @@ -24,10 +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 - src/ndt_ellipsoid.cpp) +target_sources(beluga_ros PRIVATE src/amcl.cpp src/ndt_ellipsoid.cpp) target_include_directories( beluga_ros PUBLIC $ $) diff --git a/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp b/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp index db7081929..4f2f4a089 100644 --- a/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp +++ b/beluga_ros/include/beluga_ros/ndt_ellipsoid.hpp @@ -22,8 +22,8 @@ #include -#include #include +#include /** * \file @@ -37,10 +37,7 @@ bool use_mean_covariance( const beluga::NDTCell<3> cell, beluga_ros::msg::Marker& marker); -void use_cell_size( - const Eigen::Vector& position, - double size, - beluga_ros::msg::Marker& marker); +void use_cell_size(const Eigen::Vector& 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. @@ -74,7 +71,7 @@ beluga_ros::msg::MarkerArray assign_obstacle_map( const auto& [cellCenter, cell] = entry; beluga_ros::msg::Marker marker; marker.header.frame_id = "map"; - marker.id = index+1; + marker.id = index + 1; marker.ns = "obstacles"; // Try to create an ellipsoid with values of the cell diff --git a/beluga_ros/src/ndt_ellipsoid.cpp b/beluga_ros/src/ndt_ellipsoid.cpp index 54ea69b14..2e1aca82b 100644 --- a/beluga_ros/src/ndt_ellipsoid.cpp +++ b/beluga_ros/src/ndt_ellipsoid.cpp @@ -26,17 +26,17 @@ inline bool isApprox(RealScalar x, RealScalar y, RealScalar prec = Eigen::NumTra return std::abs(x - y) < prec; } -} +} // namespace bool use_mean_covariance( Eigen::SelfAdjointEigenSolver>& eigenSolver, const beluga::NDTCell<3> cell, - beluga_ros::msg::Marker& marker){ + beluga_ros::msg::Marker& marker) { eigenSolver.compute(cell.covariance); - if(eigenSolver.info() != Eigen::Success){ + 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(); @@ -46,12 +46,12 @@ bool use_mean_covariance( 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)) { + 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) { + if (rotationMatrix.determinant() < 0) { rotationMatrix << eigenvectors.col(1), eigenvectors.col(0), eigenvectors.col(2); scalevector = Eigen::Vector3d{eigenvalues.y(), eigenvalues.x(), eigenvalues.z()}; } @@ -84,10 +84,7 @@ bool use_mean_covariance( return true; } -void use_cell_size( - const Eigen::Vector& position, - double size, - beluga_ros::msg::Marker& marker){ +void use_cell_size(const Eigen::Vector& position, double size, beluga_ros::msg::Marker& marker) { marker.type = beluga_ros::msg::Marker::CUBE; marker.action = beluga_ros::msg::Marker::ADD; @@ -105,5 +102,4 @@ void use_cell_size( marker.color.a = 1.0f; } - } // namespace beluga_ros