Add NDT AMCL 3D node. (#422)

### Proposed changes Adds a new node for 3D NDT AMCL. Closes #422 Closes #311 Closes #313 #### Type of change - [ ] 🐛 Bugfix (change which fixes an issue) - [x] 🚀 Feature (change which adds functionality) - [ ] 📚 Documentation (change which fixes or extends documentation) ### Checklist _Put an `x` in the boxes that apply. This is simply a reminder of what we will require before merging your code._ - [x] Lint and unit tests (if any) pass locally with my changes - [x] I have added tests that prove my fix is effective or that my feature works - [x] I have added necessary documentation (if appropriate) - [x] All commits have been signed for [DCO](https://developercertificate.org/) --------- Signed-off-by: Ramiro Serra <[email protected]>
Ekumen-OS · Aug 16, 2024 · d96a927 · d96a927
1 parent d23d446
commit d96a927
Show file tree

Hide file tree

Showing 28 changed files with 2,173 additions and 94 deletions.
diff --git a/beluga/include/beluga/algorithm/amcl_core.hpp b/beluga/include/beluga/algorithm/amcl_core.hpp
@@ -26,6 +26,7 @@
 #include <range/v3/range/concepts.hpp>
 #include <range/v3/view/any_view.hpp>
 #include <range/v3/view/take_exactly.hpp>
+#include "beluga/policies/on_motion.hpp"
 
 namespace beluga {
 
@@ -115,7 +116,7 @@ class Amcl {
         execution_policy_{std::move(execution_policy)},
         spatial_hasher_{std::move(spatial_hasher)},
         random_probability_estimator_{params_.alpha_slow, params_.alpha_fast},
-        update_policy_{beluga::policies::on_motion(params_.update_min_d, params_.update_min_a)},
+        update_policy_{beluga::policies::on_motion<state_type>(params_.update_min_d, params_.update_min_a)},
         resample_policy_{beluga::policies::every_n(params_.resample_interval)},
         random_state_generator_(std::move(random_state_generator)) {
     if (params_.selective_resampling) {

diff --git a/beluga/include/beluga/algorithm/unscented_transform.hpp b/beluga/include/beluga/algorithm/unscented_transform.hpp
@@ -15,11 +15,11 @@
 #ifndef BELUGA_ALGORITHM_UNSCENTED_TRANSFORM_HPP
 #define BELUGA_ALGORITHM_UNSCENTED_TRANSFORM_HPP
 
-#include <Eigen/src/Core/util/Constants.h>
 #include <Eigen/Cholesky>
 #include <Eigen/Core>
 
 #include <algorithm>
+#include <functional>
 #include <numeric>
 #include <optional>
 #include <range/v3/numeric.hpp>
@@ -36,6 +36,23 @@
 #include "beluga/eigen_compatibility.hpp"
 namespace beluga {
 
+namespace detail {
+
+/// Templated functor to compute weighted mean for arbitrary. Assumes weights and vector's scalar types match.
+struct default_weighted_mean_fn {
+  /// Operator() overload for eigen types.
+  template <typename T, typename Scalar = typename T::Scalar>
+  T operator()(const std::vector<T>& samples, const std::vector<Scalar>& weights) const {
+    return std::transform_reduce(
+        samples.begin(), samples.end(), weights.begin(), T::Zero().eval(), std::plus<>{}, std::multiplies<>{});
+  }
+};
+
+}  // namespace detail
+
+/// Object for computing a euclidean weighted mean of a vector of elements.
+inline constexpr detail::default_weighted_mean_fn default_weighted_mean;
+
 /**
  * \brief Implements the unscented transform, a mathematical function used to estimate the result of applying a given
  *  a possibly nonlinear transformation to a probability distribution that is characterized with mean and covariance.
@@ -45,18 +62,34 @@ namespace beluga {
  * \tparam MeanDerived Concrete Eigen dense type used for the mean.
  * \tparam CovarianceDerived Concrete Eigen dense type used for the covariance.
  * \tparam TransferFn Callable that maps a vector from input space to output space. Possibly non-linear.
+ * \tparam TransformedT State representing the output space. Deduced from the transfer function.
+ * \tparam MeanFn Callable that converts a vector of elements in the output space, and weights, to its mean in the
+ * output space.
+ * \tparam ResidualFn Callable that computes a residual given two elements in the output space.
  * \param mean Mean in the input space.
  * \param covariance Covariance in the input space.
- * \param transfer_fn Callable that converts a vector from input space to output space.
+ * \param transfer_fn Callable that converts a* vector from input space to output space.
  * \param kappa Parameter used for sigma points selection. A sensible default will be used if not provided.
+ * \param mean_fn Callable that converts a vector of elements in the output space, and
+ * weights, to its mean in the output space. A sensible default is provided for the trivial (euclidian) case.
+ * \param residual_fn Callable that computes a residual given two elements in the output space.. A sensible default is
+ * provided for the trivial (euclidian) case.
  * \return A pair containing (mean, estimate) for the output space.
  */
-template <typename MeanDerived, typename CovarianceDerived, typename TransferFn>
+template <
+    typename MeanDerived,
+    typename CovarianceDerived,
+    typename TransferFn,
+    typename TransformedT = std::result_of_t<TransferFn(MeanDerived)>,
+    typename MeanFn = detail::default_weighted_mean_fn,
+    typename ResidualFn = std::minus<TransformedT>>
 auto unscented_transform(
     const Eigen::MatrixBase<MeanDerived>& mean,
     const Eigen::MatrixBase<CovarianceDerived>& covariance,
     TransferFn&& transfer_fn,
-    std::optional<typename MeanDerived::Scalar> kappa = std::nullopt) {
+    std::optional<typename MeanDerived::Scalar> kappa = std::nullopt,
+    MeanFn mean_fn = default_weighted_mean,
+    ResidualFn residual_fn = std::minus<TransformedT>{}) {
   using Scalar = typename MeanDerived::Scalar;
   static_assert(
       std::is_same_v<typename MeanDerived::Scalar, typename CovarianceDerived::Scalar>,
@@ -97,15 +130,13 @@ auto unscented_transform(
   constexpr int kNout = decltype(transfer_fn(mean))::RowsAtCompileTime;
   static_assert(decltype(transfer_fn(mean))::ColsAtCompileTime == 1, "Output mean should be a column vector");
 
-  const Eigen::Vector<Scalar, kNout> out_mean = std::transform_reduce(
-      transformed_sigma_points.begin(), transformed_sigma_points.end(), weights.begin(),
-      Eigen::Vector<Scalar, kNout>::Zero().eval(), std::plus<>{}, std::multiplies<>{});
+  const Eigen::Vector<Scalar, kNout> out_mean = mean_fn(transformed_sigma_points, weights);
 
   const Eigen::Matrix<Scalar, kNout, kNout> out_cov = std::transform_reduce(
       transformed_sigma_points.begin(), transformed_sigma_points.end(), weights.begin(),
       Eigen::Matrix<Scalar, kNout, kNout>::Zero().eval(), std::plus<>{},
       [&](const auto& sigma_point, const auto weight) {
-        const Eigen::Vector<Scalar, kNout> error = sigma_point - out_mean;
+        const Eigen::Vector<Scalar, kNout> error = residual_fn(sigma_point, out_mean);
         return (weight * (error * error.transpose())).eval();
       });
 

diff --git a/beluga/include/beluga/policies/on_motion.hpp b/beluga/include/beluga/policies/on_motion.hpp
@@ -15,8 +15,10 @@
 #ifndef BELUGA_POLICIES_ON_MOTION_HPP
 #define BELUGA_POLICIES_ON_MOTION_HPP
 
+#include <Eigen/src/Geometry/AngleAxis.h>
 #include <beluga/policies/policy.hpp>
 #include <sophus/se2.hpp>
+#include <sophus/se3.hpp>
 
 /**
  * \file
@@ -60,8 +62,7 @@ struct on_motion_policy_base<Sophus::SE2<Scalar>> {
    */
   constexpr bool operator()(const Sophus::SE2<Scalar>& prev, const Sophus::SE2<Scalar>& current) {
     const auto delta = prev.inverse() * current;
-    return std::abs(delta.translation().x()) > min_distance_ ||  //
-           std::abs(delta.translation().y()) > min_distance_ ||  //
+    return delta.translation().norm() > min_distance_ ||  //
            std::abs(delta.so2().log()) > min_angle_;
   }
 
@@ -70,6 +71,42 @@ struct on_motion_policy_base<Sophus::SE2<Scalar>> {
   Scalar min_angle_{0.0};     ///< The minimum rotation angle (in radians) to trigger the action.
 };
 
+/// Specialization for on_motion_policy_base in SE3 space.
+/**
+ * \tparam Scalar The scalar type for SE3 elements.
+ */
+template <class Scalar>
+struct on_motion_policy_base<Sophus::SE3<Scalar>> {
+ public:
+  /// Constructor.
+  /**
+   * \param min_distance The minimum translation distance to trigger the action.
+   * \param min_angle The minimum rotation angle (in radians) to trigger the action.
+   */
+  constexpr on_motion_policy_base(Scalar min_distance, Scalar min_angle)
+      : min_distance_(min_distance), min_angle_(min_angle) {}
+
+  /// Return true if motion has been detected.
+  /**
+   * \param prev The previous SE3 pose to check for motion.
+   * \param current The current SE3 pose to check for motion.
+   * \return True if the action should be triggered based on motion, false otherwise.
+   *
+   * Checks the motion based on the provided SE3 pose, and triggers the action if the
+   * motion surpasses the specified distance and angle thresholds.
+   */
+  constexpr bool operator()(const Sophus::SE3<Scalar>& prev, const Sophus::SE3<Scalar>& current) {
+    const auto delta = prev.inverse() * current;
+    const Eigen::AngleAxis<Scalar> angle_axis{delta.so3().unit_quaternion()};
+    return delta.translation().norm() > min_distance_ ||  //
+           std::abs(angle_axis.angle()) > min_angle_;
+  }
+
+ private:
+  Scalar min_distance_{0.0};  ///< The minimum translation distance to trigger the action.
+  Scalar min_angle_{0.0};     ///< The minimum rotation angle (in radians) to trigger the action.
+};
+
 /// Base implementation for the on_motion_policy algorithm.
 /**
  * \tparam Pose The pose type to check for motion.
@@ -100,14 +137,14 @@ struct on_motion_policy : public on_motion_policy_base<Pose> {
 };
 
 /// Implementation detail for the on_motion_fn object.
+template <class StateType = Sophus::SE2d>
 struct on_motion_fn {
-  /// Overload that creates the policy closure in SE2 space.
+  /// Overload that creates the policy closure in the state space.
   /**
    * This policy triggers an action based on motion.
    */
-  template <class Scalar>
-  constexpr auto operator()(Scalar min_distance, Scalar min_angle) const {
-    return beluga::make_policy(on_motion_policy<Sophus::SE2<Scalar>>{min_distance, min_angle});
+  constexpr auto operator()(typename StateType::Scalar min_distance, typename StateType::Scalar min_angle) const {
+    return beluga::make_policy(on_motion_policy<StateType>{min_distance, min_angle});
   }
 };
 
@@ -117,8 +154,11 @@ struct on_motion_fn {
 /**
  * This policy is designed to be used for scenarios where an action needs to be performed
  * based on the detected motion, considering specified distance and angle thresholds.
+
+ * \tparam StateType The state type used to check for motion.
  */
-inline constexpr detail::on_motion_fn on_motion;
+template <typename StateType = Sophus::SE2d>
+inline constexpr detail::on_motion_fn<StateType> on_motion;
 
 }  // namespace beluga::policies
 

diff --git a/beluga/test/beluga/algorithm/test_unscented_transform.cpp b/beluga/test/beluga/algorithm/test_unscented_transform.cpp
@@ -18,6 +18,9 @@
 #include <cmath>
 
 #include <Eigen/Core>
+#include <range/v3/view/zip.hpp>
+#include <sophus/common.hpp>
+#include <sophus/so2.hpp>
 
 #include "beluga/algorithm/unscented_transform.hpp"
 
@@ -118,6 +121,66 @@ TEST_F(UnscentedTransformTests, WorksWithEigenExpressions) {
   ASSERT_TRUE(transformed_cov.isApprox(Eigen::Matrix<double, 2, 2>::Identity()));
 }
 
+double angular_distance(double angle1, double angle2) {
+  const Sophus::SO2<double> rot1(angle1);
+  const Sophus::SO2<double> rot2(angle2);
+
+  const Sophus::SO2<double> relative_rotation = rot1.inverse() * rot2;
+  const double distance = relative_rotation.log();
+
+  // Ensure the distance is the minimal distance
+  return std::min(std::abs(distance), 2 * Sophus::Constants<double>::pi() - std::abs(distance));
+}
+
+TEST_F(UnscentedTransformTests, DifferentMeanFn) {
+  constexpr double kTolerance = 1e-1;
+  // This mean in polar coordinates is {1.4, pi}. This is specifically chosen so that we're close to the wrap around
+  // point.
+  const Eigen::Vector2d mean{-1.4, 0.};
+  const Eigen::Vector2d expected_mean = {1.4, Sophus::Constants<double>::pi()};
+
+  Eigen::Matrix<double, 2, 2> cov;
+  cov << 1e-1, 0.,  //
+      0., 1e-1;
+
+  // Demonstrate why using default mean and residual functions can yield bad results, even bad means.
+  {
+    const auto [transformed_mean, transformed_cov] =
+        beluga::unscented_transform(mean, cov, [](const Eigen::Vector2d& in) -> Eigen::Vector2d {
+          return Eigen::Vector2d{std::hypot(in.x(), in.y()), std::atan2(in.y(), in.x())};
+        });
+
+    const Eigen::Vector2d expected_wrong_mean = {1.4348469228349534, 1.5707963267948966};
+
+    ASSERT_FALSE(transformed_mean.isApprox(expected_mean, kTolerance)) << transformed_mean.transpose();
+    ASSERT_TRUE(transformed_mean.isApprox(expected_wrong_mean, kTolerance)) << transformed_mean.transpose();
+  }
+  {
+    const auto [transformed_mean, transformed_cov] = beluga::unscented_transform(
+        mean, cov,
+        [](const Eigen::Vector2d& in) -> Eigen::Vector2d {
+          return Eigen::Vector2d{std::hypot(in.x(), in.y()), std::atan2(in.y(), in.x())};
+        },
+        std::nullopt,
+        [](const std::vector<Eigen::Vector2d>& samples, const std::vector<double>& weights) -> Eigen::Vector2d {
+          Eigen::Vector3d aux = Eigen::Vector3d::Zero();
+          for (const auto& [s, w] : ranges::views::zip(samples, weights)) {
+            aux.x() += s.x() * w;
+            aux.y() += std::sin(s.y()) * w;
+            aux.z() += std::cos(s.y()) * w;
+          }
+          return Eigen::Vector2d{aux.x(), std::atan2(aux.y(), aux.z())};
+        },
+        [](const Eigen::Vector2d& sample, const Eigen::Vector2d& mean) {
+          Eigen::Vector2d error;
+          error.x() = sample.x() - mean.x();
+          error.y() = angular_distance(sample.y(), mean.y());
+          return error;
+        });
+    ASSERT_TRUE(transformed_mean.isApprox(expected_mean, kTolerance)) << transformed_mean.transpose();
+  }
+}
+
 TEST_F(UnscentedTransformTests, DifferentKappa) {
   constexpr double kTolerance = 1e-4;
   const auto [transformed_mean, transformed_cov] = beluga::unscented_transform(

diff --git a/beluga/test/beluga/policies/test_on_motion.cpp b/beluga/test/beluga/policies/test_on_motion.cpp
@@ -21,8 +21,8 @@
 
 namespace {
 
-TEST(OnMotionPolicy, TriggerOnMotion) {
-  auto policy = beluga::policies::on_motion(0.1, 0.05);
+TEST(OnMotionPolicy, TriggerOnMotion2D) {
+  auto policy = beluga::policies::on_motion<Sophus::SE2d>(0.1, 0.05);
   const Sophus::SE2d pose1(Sophus::SO2d(0.2), Eigen::Vector2d(1.0, 2.0));
   const Sophus::SE2d pose2(Sophus::SO2d(0.25), Eigen::Vector2d(1.2, 2.2));
 
@@ -31,13 +31,32 @@ TEST(OnMotionPolicy, TriggerOnMotion) {
   ASSERT_TRUE(policy(pose2));   // Second pose triggers the policy
 }
 
-TEST(OnMotionPolicy, NoTriggerWithoutMotion) {
-  auto policy = beluga::policies::on_motion(0.1, 0.05);
+TEST(OnMotionPolicy, NoTriggerWithoutMotion2D) {
+  auto policy = beluga::policies::on_motion<Sophus::SE2d>(0.1, 0.05);
   const Sophus::SE2d pose1(Sophus::SO2d(0.1), Eigen::Vector2d(1.0, 2.0));
   const Sophus::SE2d pose2(Sophus::SO2d(0.1), Eigen::Vector2d(1.05, 2.05));
 
   ASSERT_TRUE(policy(pose1));   // First pose triggers the policy
   ASSERT_FALSE(policy(pose2));  // Small motion should not trigger the policy
 }
 
+TEST(OnMotionPolicy, TriggerOnMotion3D) {
+  auto policy = beluga::policies::on_motion<Sophus::SE3d>(0.1, 0.05);
+  const Sophus::SE3d pose1(Sophus::SO3d::rotX(0.2), Eigen::Vector3d(1.0, 2.0, 0.));
+  const Sophus::SE3d pose2(Sophus::SO3d::rotX(0.25), Eigen::Vector3d(1.2, 2.2, 0.));
+
+  ASSERT_TRUE(policy(pose1));   // First pose triggers the policy
+  ASSERT_FALSE(policy(pose1));  // Same pose should not trigger again
+  ASSERT_TRUE(policy(pose2));   // Second pose triggers the policy
+}
+
+TEST(OnMotionPolicy, NoTriggerWithoutMotion3D) {
+  auto policy = beluga::policies::on_motion<Sophus::SE3d>(0.1, 0.05);
+  const Sophus::SE3d pose1(Sophus::SO3d::rotX(0.1), Eigen::Vector3d(1.0, 2.0, 0.));
+  const Sophus::SE3d pose2(Sophus::SO3d::rotX(0.1), Eigen::Vector3d(1.05, 2.05, 0.));
+
+  ASSERT_TRUE(policy(pose1));   // First pose triggers the policy
+  ASSERT_FALSE(policy(pose2));  // Small motion should not trigger the policy
+}
+
 }  // namespace
diff --git a/beluga_amcl/cmake/ament.cmake b/beluga_amcl/cmake/ament.cmake
@@ -139,6 +139,53 @@ install(TARGETS ndt_amcl_node DESTINATION lib/${PROJECT_NAME})
 
 install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME})
 
+add_library(ndt_amcl_node_3d_component SHARED)
+target_sources(ndt_amcl_node_3d_component PRIVATE src/ndt_amcl_node_3d.cpp)
+
+target_include_directories(
+  ndt_amcl_node_3d_component
+  PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+         $<INSTALL_INTERFACE:include/${PROJECT_NAME}>)
+
+target_compile_features(ndt_amcl_node_3d_component PUBLIC cxx_std_17)
+
+target_link_libraries(ndt_amcl_node_3d_component PUBLIC beluga_amcl_ros2_common)
+
+ament_target_dependencies(
+  ndt_amcl_node_3d_component
+  PUBLIC beluga
+         beluga_ros
+         bondcpp
+         rclcpp
+         rclcpp_components
+         rclcpp_lifecycle
+         std_srvs)
+
+rclcpp_components_register_node(
+  ndt_amcl_node_3d_component
+  PLUGIN "beluga_amcl::NdtAmclNode3D"
+  EXECUTABLE ndt_amcl_node_3d)
+
+install(
+  TARGETS ndt_amcl_node_3d_component
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin)
+
+install(TARGETS ndt_amcl_node_3d DESTINATION lib/${PROJECT_NAME})
+
+install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME})
+
+ament_export_dependencies(
+  beluga
+  beluga_ros
+  bondcpp
+  rclcpp
+  rclcpp_components
+  rclcpp_lifecycle
+  std_srvs)
+ament_export_include_directories("include/${PROJECT_NAME}")
+
 if(BUILD_TESTING)
   enable_testing()
   add_subdirectory(test)