Merge branch 'main' into likelihood_field_3d_model

.github/workflows/ci_pipeline.yml

@@ -37,7 +37,6 @@ jobs:
         ros_distro:
           - noetic
           - humble
-          - iron
           - jazzy
         include:
           - ros_distro: humble

.github/workflows/docker_ci_pipeline.yml

@@ -16,14 +16,14 @@ on:
 
 jobs:
   build-image:
+    if: github.repository == 'Ekumen-OS/beluga'
     runs-on: ubuntu-latest
     strategy:
       fail-fast: false
       matrix:
         ros_distro:
           - noetic
           - humble
-          - iron
           - jazzy
           - rolling
     steps:

.github/workflows/weekly_pipeline.yml

@@ -16,14 +16,14 @@ concurrency:
 
 jobs:
   build-test:
+    if: github.repository == 'Ekumen-OS/beluga'
     runs-on: ubuntu-latest
     strategy:
       fail-fast: false
       matrix:
         ros_distro:
           - noetic
           - humble
-          - iron
           - jazzy
           - rolling
     container:

DEVELOPING.md

@@ -38,7 +38,7 @@ To bring up a development environment:
    (cd beluga && ROSDISTRO=humble docker/run.sh)
    ```
 
-   Supported distributions include `noetic`, `humble`, `iron`, `jazzy`, and `rolling`.
+   Supported distributions include `noetic`, `humble`, `jazzy`, and `rolling`.
 
 ## Workflow
 

README.md

@@ -23,6 +23,10 @@ Beluga is an extensible C++17 library with a ground-up implementation of the Mon
 - Emphasis on the prevention of regressions and facilitation of code improvements through test coverage.
 - Semi-automated benchmarks that can be used to validate different configurations.
 
+https://github.com/user-attachments/assets/59bca7ea-fe78-4460-b058-96a5c75d09ec
+
+<p align="center"><i><b>Beluga AMCL</b> running on a <b>Turtlebot 2</b> robot (Raspberry Pi 4B)</i></p>
+
 https://github.com/Ekumen-OS/beluga/assets/33042669/98bda0ee-a633-4e35-8743-72a9ab30b494
 
 <p align="center"><i><b>Beluga AMCL</b> running on an <b>Andino</b> robot (Raspberry Pi 4B), go to <a href="https://github.com/Ekumen-OS/andino">Ekumen-OS/andino</a> for more details!</i></p>

beluga_ros/include/beluga_ros/particle_cloud.hpp

@@ -81,10 +81,34 @@ struct almost_equal_to<Sophus::SE2<Scalar>> {
 
   /// Compares `a` and `b` for near equality.
   bool operator()(const Sophus::SE2<Scalar>& a, const Sophus::SE2<Scalar>& b) const {
-    using std::abs, std::atan, std::tan;
-    return (abs(a.translation().x() - b.translation().x()) < linear_resolution) &&
-           (abs(a.translation().y() - b.translation().y()) < linear_resolution) &&
-           (abs(atan(tan(a.so2().log() - b.so2().log()))) < angular_resolution);
+    using std::abs;
+    const Sophus::SE2<Scalar> diff = a * b.inverse();
+    return (abs(diff.translation().x()) < linear_resolution) && (abs(diff.translation().y()) < linear_resolution) &&
+           (abs(diff.so2().log()) < angular_resolution);
+  }
+
+  const Scalar linear_resolution;   ///< Resolution for translational coordinates, in meters.
+  const Scalar angular_resolution;  ///< Resolution for rotational coordinates, in radians.
+};
+
+/// std::equal_to equivalent specialized for SE(3) types, with user resolutions.
+template <typename Scalar>
+struct almost_equal_to<Sophus::SE3<Scalar>> {
+  /// Constructs near equality functor.
+  /**
+   * \param _linear_resolution Resolution for translational coordinates, in meters.
+   * \param _angular_resolution Resolution for rotational coordinates, in radians.
+   */
+  explicit almost_equal_to(Scalar _linear_resolution, Scalar _angular_resolution)
+      : linear_resolution(_linear_resolution), angular_resolution(_angular_resolution) {}
+
+  /// Compares `a` and `b` for near equality.
+  bool operator()(const Sophus::SE3<Scalar>& a, const Sophus::SE3<Scalar>& b) const {
+    using std::abs;
+    const Sophus::SE3<Scalar> diff = a * b.inverse();
+    return (abs(diff.translation().x()) < linear_resolution) && (abs(diff.translation().y()) < linear_resolution) &&
+           (abs(diff.translation().z()) < linear_resolution) && (abs(diff.so3().angleX()) < angular_resolution) &&
+           (abs(diff.so3().angleY()) < angular_resolution) && (abs(diff.so3().angleZ()) < angular_resolution);
   }
 
   const Scalar linear_resolution;   ///< Resolution for translational coordinates, in meters.
@@ -149,7 +173,8 @@ template <
     class State = typename beluga::state_t<Particle>,
     class Weight = typename beluga::weight_t<Particle>,
     class Scalar = typename State::Scalar,
-    typename = std::enable_if_t<std::is_same_v<State, typename Sophus::SE2<Scalar>>>>
+    typename = std::enable_if_t<
+        std::is_same_v<State, typename Sophus::SE2<Scalar>> || std::is_same_v<State, typename Sophus::SE3<Scalar>>>>
 beluga_ros::msg::MarkerArray& assign_particle_cloud(
     Particles&& particles,
     Scalar linear_resolution,

beluga_ros/test/test_particle_cloud.cpp

@@ -23,6 +23,7 @@
 
 #include <sophus/common.hpp>
 #include <sophus/se2.hpp>
+#include <sophus/se3.hpp>
 
 #include "beluga/primitives.hpp"
 #include "beluga_ros/messages.hpp"
@@ -48,13 +49,45 @@ TEST(TestParticleCloud, Assign) {
     EXPECT_DOUBLE_EQ(pose.position.z, 0.0);
     EXPECT_DOUBLE_EQ(pose.orientation.x, 0.0);
     EXPECT_DOUBLE_EQ(pose.orientation.y, 0.0);
-    EXPECT_DOUBLE_EQ(pose.orientation.x, 0.0);
-    EXPECT_DOUBLE_EQ(pose.orientation.y, 0.0);
     EXPECT_DOUBLE_EQ(std::abs(pose.orientation.z), std::sin(Constants::pi() / 4.0));
     EXPECT_DOUBLE_EQ(pose.orientation.w, std::cos(Constants::pi() / 4.0));
   }
-  const auto num_particles_in_upper_half_xy_plane =
+  const auto num_particles_in_upper_half_xz_plane =
       ranges::count_if(message.poses, [](const auto& pose) { return pose.position.y > 0.0; });
+  const double upper_half_xz_plane_weight =
+      static_cast<double>(num_particles_in_upper_half_xz_plane) / static_cast<double>(message.poses.size());
+  EXPECT_NEAR(upper_half_xz_plane_weight, 0.5, kSampleSize * 0.01);  // allow 1% deviations
+}
+
+TEST(TestParticleCloud3, Assign) {
+  constexpr double kTolerance = 0.001;
+  const auto particles = std::vector{
+      std::make_tuple(
+          Sophus::SE3d{
+              Sophus::SO3d{Eigen::Quaternion<double>{0.8497105, 0.0, 0.3728217, 0.3728217}},
+              Eigen::Vector3d{0.0, 0.0, 1.0}},
+          beluga::Weight(0.5)),
+      std::make_tuple(
+          Sophus::SE3d{
+              Sophus::SO3d{Eigen::Quaternion<double>{0.8497105, 0.0, -0.3728217, 0.3728217}},
+              Eigen::Vector3d{0.0, 0.0, -1.0}},
+          beluga::Weight(0.5)),
+  };
+  constexpr std::size_t kSampleSize = 1000U;
+  auto message = beluga_ros::msg::PoseArray{};
+  beluga_ros::assign_particle_cloud(particles, kSampleSize, message);
+  EXPECT_EQ(message.poses.size(), kSampleSize);
+  for (const auto& pose : message.poses) {
+    EXPECT_DOUBLE_EQ(pose.position.x, 0.0);
+    EXPECT_DOUBLE_EQ(pose.position.y, 0.0);
+    EXPECT_DOUBLE_EQ(std::abs(pose.position.z), 1.0);
+    EXPECT_DOUBLE_EQ(pose.orientation.x, 0.0);
+    EXPECT_NEAR(std::abs(pose.orientation.y), 0.3728217, kTolerance);
+    EXPECT_NEAR(pose.orientation.z, 0.3728217, kTolerance);
+    EXPECT_NEAR(pose.orientation.w, 0.8497105, kTolerance);
+  }
+  const auto num_particles_in_upper_half_xy_plane =
+      ranges::count_if(message.poses, [](const auto& pose) { return pose.position.z > 0.0; });
   const double upper_half_xy_plane_weight =
       static_cast<double>(num_particles_in_upper_half_xy_plane) / static_cast<double>(message.poses.size());
   EXPECT_NEAR(upper_half_xy_plane_weight, 0.5, kSampleSize * 0.01);  // allow 1% deviations
@@ -69,6 +102,15 @@ TEST(TestParticleCloud, AssignNone) {
   EXPECT_EQ(message.poses.size(), 0U);
 }
 
+TEST(TestParticleCloud3, AssignNone) {
+  const auto particles = std::vector{
+      std::make_tuple(Sophus::SE3d{}, beluga::Weight(1.0)),
+  };
+  auto message = beluga_ros::msg::PoseArray{};
+  beluga_ros::assign_particle_cloud(particles, 0U, message);
+  EXPECT_EQ(message.poses.size(), 0U);
+}
+
 TEST(TestParticleCloud, AssignMatchingDistribution) {
   const auto particles = std::vector{
       std::make_tuple(Sophus::SE2d{}, beluga::Weight(1.0)),
@@ -78,13 +120,29 @@ TEST(TestParticleCloud, AssignMatchingDistribution) {
   EXPECT_EQ(message.poses.size(), particles.size());
 }
 
+TEST(TestParticleCloud3, AssignMatchingDistribution) {
+  const auto particles = std::vector{
+      std::make_tuple(Sophus::SE3d{}, beluga::Weight(1.0)),
+  };
+  auto message = beluga_ros::msg::PoseArray{};
+  beluga_ros::assign_particle_cloud(particles, message);
+  EXPECT_EQ(message.poses.size(), particles.size());
+}
+
 TEST(TestParticleCloud, AssignMatchingEmpty) {
   const auto particles = std::vector<std::tuple<Sophus::SE2d, beluga::Weight>>{};
   auto message = beluga_ros::msg::PoseArray{};
   beluga_ros::assign_particle_cloud(particles, message);
   EXPECT_EQ(message.poses.size(), 0U);
 }
 
+TEST(TestParticleCloud3, AssignMatchingEmpty) {
+  const auto particles = std::vector<std::tuple<Sophus::SE3d, beluga::Weight>>{};
+  auto message = beluga_ros::msg::PoseArray{};
+  beluga_ros::assign_particle_cloud(particles, message);
+  EXPECT_EQ(message.poses.size(), 0U);
+}
+
 TEST(TestParticleCloud, AssignMarkers) {
   using Constants = Sophus::Constants<double>;
   const auto particles = std::vector{
@@ -99,11 +157,33 @@ TEST(TestParticleCloud, AssignMarkers) {
   EXPECT_EQ(message.markers[1].points.size(), 3 * 2);  // 2 arrows, 3 vertices each
 }
 
+TEST(TestParticleCloud3, AssignMarkers) {
+  const auto particles = std::vector{
+      std::make_tuple(Sophus::SE3d{Sophus::SO3d{}, Eigen::Vector3d{0.0, 0.0, 1.0}}, beluga::Weight(0.35)),
+      std::make_tuple(Sophus::SE3d{Sophus::SO3d{}, Eigen::Vector3d{0.0, 0.0, 1.0}}, beluga::Weight(0.25)),
+      std::make_tuple(
+          Sophus::SE3d{Sophus::SO3d{Eigen::Quaternion<double>{1.0, 0.0, 0.0, 0.0}}, Eigen::Vector3d{0.0, 0.0, -1.0}},
+          beluga::Weight(0.4)),
+  };
+  auto message = beluga_ros::msg::MarkerArray{};
+  beluga_ros::assign_particle_cloud(particles, message);
+  ASSERT_EQ(message.markers.size(), 2U);
+  EXPECT_EQ(message.markers[0].points.size(), 2 * 2);  // 2 arrows, 2 vertices each
+  EXPECT_EQ(message.markers[1].points.size(), 3 * 2);  // 2 arrows, 3 vertices each
+}
+
 TEST(TestParticleCloud, AssignNoMarkers) {
   const auto particles = std::vector<std::tuple<Sophus::SE2d, beluga::Weight>>{};
   auto message = beluga_ros::msg::MarkerArray{};
   beluga_ros::assign_particle_cloud(particles, message);
   EXPECT_EQ(message.markers.size(), 0U);
 }
 
+TEST(TestParticleCloud3, AssignNoMarkers) {
+  const auto particles = std::vector<std::tuple<Sophus::SE3d, beluga::Weight>>{};
+  auto message = beluga_ros::msg::MarkerArray{};
+  beluga_ros::assign_particle_cloud(particles, message);
+  EXPECT_EQ(message.markers.size(), 0U);
+}
+
 }  // namespace

beluga_tools/package.xml

@@ -11,12 +11,12 @@
   <license>Apache License 2.0</license>
 
 
-  <!-- The released debian for h5py package when on humble or iron seems to have a bug where it's incompatible with numpy's 2.0.0 version.
+  <!-- The released debian for h5py package when on humble seems to have a bug where it's incompatible with numpy's 2.0.0 version.
   We will install newest package version using pip in the development images to circumvent this..
   See:
   https://github.com/h5py/h5py/issues/2353
   -->
-  <exec_depend condition="$ROS_VERSION == 2 and $ROS_DISTRO != 'humble' and $ROS_DISTRO != 'iron'">python3-h5py</exec_depend>
+  <exec_depend condition="$ROS_VERSION == 2 and $ROS_DISTRO != 'humble'">python3-h5py</exec_depend>
   <exec_depend condition="$ROS_VERSION == 2">python3-matplotlib</exec_depend>
   <exec_depend condition="$ROS_VERSION == 2">python3-numpy</exec_depend>
   <exec_depend condition="$ROS_VERSION == 2">python3-scipy</exec_depend>