Pose filtering

.gitignore

@@ -9,7 +9,6 @@ install/
 log/
 build/
 bin/
-lib/
 msg_gen/
 srv_gen/
 msg/*Action.msg

filtering/pose_action_server/src/pose_action_server_ros.cpp

@@ -133,7 +133,7 @@ Eigen::Quaterniond PoseActionServerNode::average_quaternions(
     const std::vector<Eigen::Quaterniond>& quaternions) {
     Eigen::Matrix4d M = Eigen::Matrix4d::Zero();
     std::ranges::for_each(quaternions, [&](const auto& q) {
-        M += q.coeffs() * q.coeffs().transpose();
+        M += q.normalized().coeffs() * q.normalized().coeffs().transpose();
     });
 
     Eigen::SelfAdjointEigenSolver<Eigen::Matrix4d> eigensolver(M);

filtering/pose_filtering/CMakeLists.txt

@@ -0,0 +1,112 @@
+cmake_minimum_required(VERSION 3.8)
+project(pose_filtering)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 20)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(message_filters REQUIRED)
+find_package(vortex_utils REQUIRED)
+find_package(vortex_utils_ros REQUIRED)
+find_package(vortex_utils_ros_tf REQUIRED)
+find_package(vortex_filtering REQUIRED)
+find_package(vortex_msgs REQUIRED)
+
+include_directories(include)
+
+set(CORE_LIB_NAME "${PROJECT_NAME}")
+
+add_library(${CORE_LIB_NAME} SHARED
+  src/lib/pose_track_manager.cpp
+  src/lib/orientation_filter.cpp
+)
+
+target_include_directories(${CORE_LIB_NAME}
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+)
+
+ament_target_dependencies(${CORE_LIB_NAME}
+  vortex_filtering
+  vortex_utils
+  vortex_utils_ros
+  vortex_utils_ros_tf
+  vortex_msgs
+)
+
+set(COMPONENT_LIB_NAME "${PROJECT_NAME}_component")
+
+add_library(${COMPONENT_LIB_NAME} SHARED
+  src/ros/${PROJECT_NAME}_ros.cpp
+  src/ros/${PROJECT_NAME}_ros_debug.cpp
+  src/ros/${PROJECT_NAME}_ros_conversions.cpp
+)
+
+ament_target_dependencies(${COMPONENT_LIB_NAME}
+  rclcpp
+  rclcpp_components
+  geometry_msgs
+  tf2
+  tf2_ros
+  tf2_geometry_msgs
+  message_filters
+  vortex_filtering
+  vortex_utils
+  vortex_utils_ros
+  vortex_utils_ros_tf
+)
+
+target_link_libraries(${COMPONENT_LIB_NAME} ${PROJECT_NAME})
+
+rclcpp_components_register_node(
+  ${COMPONENT_LIB_NAME}
+  PLUGIN "PoseFilteringNode"
+  EXECUTABLE ${PROJECT_NAME}_node
+)
+
+install(
+  TARGETS
+    ${CORE_LIB_NAME}
+    ${COMPONENT_LIB_NAME}
+  EXPORT export_${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+install(DIRECTORY
+  launch
+  config
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
+ament_export_dependencies(
+  vortex_filtering
+  vortex_utils
+)
+ament_export_include_directories(include)
+ament_export_libraries(${CORE_LIB_NAME})
+
+if(BUILD_TESTING)
+  add_subdirectory(test)
+endif()
+
+ament_package()

filtering/pose_filtering/README.md

@@ -0,0 +1,119 @@
+# pose_filtering
+
+ROS 2 package providing pose-based multi-target tracking using an IPDA
+filter for position together with independent orientation logic.
+
+This package transforms incoming pose-like messages into a common
+target frame, associates measurements to tracks using spatial and angular
+gating, maintains an IPDA filter per track for position estimation and
+existence management, and performs a separate orientation update by
+averaging measurement quaternions and applying spherical linear
+interpolation (slerp) to smoothly update the track orientation.
+
+## Features
+
+- Multi-target tracking using an IPDA (Integrated Probabilistic Data
+  Association) filter for 3D position state estimation.
+- Separate orientation handling: angular gating, quaternion averaging and
+  slerp-based smoothing so orientation updates remain numerically stable
+  and physically meaningful.
+- TF2-aware subscriptions (message_filters + tf2 message filter) so
+  incoming messages are automatically dropped until transform to the
+  configured target frame is available.
+- Supports several ROS message inputs (see below) and publishes a
+  `geometry_msgs/PoseArray` containing the current tracked poses.
+
+## Supported input messages
+
+The node accepts the following message types as input measurements:
+
+- `geometry_msgs/msg/PoseStamped`
+- `geometry_msgs/msg/PoseArray`
+- `geometry_msgs/msg/PoseWithCovarianceStamped`
+- `vortex_msgs/msg/LandmarkArray`
+
+The code uses a templated message type (PoseMsgT) and checks at
+compile-time which types are supported. Incoming measurements are
+transformed to the configured `target_frame` before being processed.
+
+## Outputs
+
+- `geometry_msgs/msg/PoseArray` — the current set of tracked poses in the
+  configured target frame. Each Pose contains the track position and a
+  quaternion orientation representing the track's current orientation.
+- (DEBUG) `vortex_msgs/msg/PoseEulerStamped` topics with
+  measurement-level and track-state-level pose messages when debug mode
+  is enabled. Only publishes the first element of incoming measurements and stored tracks.
+
+
+## How the filtering works
+
+Position (IPDA):
+
+- Each track runs a 3D IPDA filter instance (a Gaussian state: mean +
+  covariance) where the motion model is a small constant-dynamics model
+  and the sensor model maps state to direct position measurements.
+- IPDA takes care of measurement association probabilities, gating and
+  existence management (confirmation / deletion thresholds). This
+  provides robust data association for cluttered measurements and
+  handles tracks appearing/disappearing over time.
+
+Orientation (SO(3) PDAF):
+
+Orientation is handled by a dedicated PDAF-style filter operating on
+the SO(3) manifold. Each track
+maintains:
+
+- a quaternion mean q (the track orientation), and
+- a small Gaussian error state in the 3D tangent space (so(3)) around
+  that mean.
+
+
+The log and exp maps are mathematically defined for all unit quaternions
+but the Kalman/PDAF linearization on the tangent space assumes small
+errors. The implementation gates measurements and uses configurable
+covariances to keep tangent residuals small; if large-angle residuals
+occur they will either be gated out or yield small β (limited effect).
+
+## Library export and usage
+
+The core tracking logic (`PoseTrackManager`) is built and exported as a
+shared library by this package so other packages can reuse the track
+management functionality without pulling in the ROS node.
+
+CMake / ament usage
+
+In a consuming CMake-based package (ament_cmake) you can link against
+the library like this:
+
+```cmake
+find_package(pose_filtering REQUIRED)
+
+add_executable(my_node src/my_node.cpp)
+
+# Link to the exported library target (the package exports the target
+# with the same name as the package: `pose_filtering`)
+target_link_libraries(my_node PRIVATE pose_filtering)
+
+```
+
+The package installs its public headers under `include/` so simply
+including the header below in your code will work after linking:
+
+```cpp
+#include <pose_filtering/lib/pose_track_manager.hpp>
+
+int main() {
+  vortex::filtering::TrackManagerConfig cfg;
+  // configure cfg.ipda, cfg.dyn_mod, cfg.sensor_mod, cfg.existence, etc.
+
+  vortex::filtering::PoseTrackManager manager(cfg);
+
+  std::vector<vortex::utils::types::Pose> measurements;
+  double dt = 0.1; // seconds
+  manager.step(measurements, dt);
+
+  const auto& tracks = manager.get_tracks();
+  // iterate tracks for id, pose, existence probability, etc.
+}
+```

filtering/pose_filtering/config/pose_filtering.yaml

@@ -0,0 +1,54 @@
+/**:
+  ros__parameters:
+    pose_sub_topic: "/aruco_detector/board"
+    pose_array_pub_topic: "/filtered_pose_array"
+    landmark_pub_topic: "/filtered_landmarks"
+    target_frame: "odom"
+    timer_rate_ms: 100
+    enu_ned_rotation: true
+
+    max_angle_gate_threshold: 0.4 # in radians
+
+    # IPDA config
+    position:
+      ipda:
+        prob_of_survival: 0.95
+        estimate_clutter: false
+        prob_of_detection: 0.5
+        mahalanobis_gate_threshold: 2.5 # in std deviations
+        min_gate_threshold: 1.0 # in meters
+        max_gate_threshold: 10.0 # in meters
+        clutter_intensity: 0.01
+      initial_position_std: 0.1
+
+      # Dyn model config
+      dyn_mod:
+        std_dev: 0.2
+
+      # Sensor model config
+      sensor_mod:
+        std_dev: 0.2
+
+      # Existence management config
+      existence:
+        confirmation_threshold: 0.6
+        deletion_threshold: 0.2
+        initial_existence_probability: 0.4
+
+    orientation:
+      pdaf:
+        prob_of_detection: 0.95
+        clutter_intensity: 0.01
+        mahalanobis_gate_threshold: 2.0 # in std deviations
+      initial_orientation_std: 0.1
+
+      dyn_mod:
+        std_dev: 0.05
+
+      sensor_mod:
+        std_dev: 0.05
+
+    debug:
+      enable: true
+      topic_name_meas: "/pose_debug_meas"
+      topic_name_state: "/pose_debug_state"

filtering/pose_filtering/include/pose_filtering/lib/orientation_filter.hpp

@@ -0,0 +1,64 @@
+#ifndef POSE_FILTERING__LIB__ORIENTATION_FILTER_HPP_
+#define POSE_FILTERING__LIB__ORIENTATION_FILTER_HPP_
+
+#include <Eigen/Dense>
+#include <vector>
+#include <vortex_filtering/vortex_filtering.hpp>
+#include "typedefs.hpp"
+
+namespace vortex::filtering {
+
+/**
+ * @brief Orientation filter implementing prediction and PDAF update on SO(3).
+ *
+ * The filter represents the orientation mean as a quaternion and keeps
+ * an error-state Gaussian in the 3D tangent space. Measurement updates
+ * are performed by mapping quaternion residuals into the tangent space
+ * (log map), performing a PDAF-style weighted update, and applying the
+ * update via the exponential map back to the quaternion manifold.
+ */
+class OrientationFilter {
+   public:
+    /**
+     * @brief Construct an OrientationFilter
+     * @param config Configuration struct
+     */
+    explicit OrientationFilter(const OrientationFilterConfig& config);
+
+    /**
+     * @brief Run a single filter step (prediction + update) for orientation.
+     * @param measurements Quaternion measurements associated with the
+     *        current track
+     * @param dt Time step in seconds used for prediction / covariance growth
+     * @param state OrientationState instance that will be updated with the
+     *        new orientation mean and error-state
+     */
+    void step(const std::vector<Eigen::Quaterniond>& measurements,
+              double dt,
+              OrientationState& state);
+
+   private:
+    /**
+     * @brief Map a quaternion to the so(3) tangent vector (log map).
+     * @param q_in Quaternion representing the relative rotation
+     * @return Vector in R^3 representing the axis * angle
+     */
+    Eigen::Vector3d so3_log_quat(const Eigen::Quaterniond& q_in);
+
+    /**
+     * @brief Map a tangent vector back to a quaternion (exp map).
+     * @param rvec Tangent vector (axis * angle)
+     * @return Quaternion corresponding to the rotation
+     */
+    Eigen::Quaterniond so3_exp_quat(const Eigen::Vector3d& rvec);
+
+    DynMod dyn_mod_;
+
+    SensorMod sensor_mod_;
+
+    vortex::filter::PDAF<DynMod, SensorMod>::Config pdaf_config_;
+};
+
+}  // namespace vortex::filtering
+
+#endif  // POSE_FILTERING__LIB__ORIENTATION_FILTER_HPP_