diff --git a/.gitignore b/.gitignore index 9af41aca7..52ae0a3c0 100644 --- a/.gitignore +++ b/.gitignore @@ -9,7 +9,6 @@ install/ log/ build/ bin/ -lib/ msg_gen/ srv_gen/ msg/*Action.msg diff --git a/filtering/pose_action_server/src/pose_action_server_ros.cpp b/filtering/pose_action_server/src/pose_action_server_ros.cpp index bf31ad621..115bcefe0 100644 --- a/filtering/pose_action_server/src/pose_action_server_ros.cpp +++ b/filtering/pose_action_server/src/pose_action_server_ros.cpp @@ -133,7 +133,7 @@ Eigen::Quaterniond PoseActionServerNode::average_quaternions( const std::vector& 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 eigensolver(M); diff --git a/filtering/pose_filtering/CMakeLists.txt b/filtering/pose_filtering/CMakeLists.txt new file mode 100644 index 000000000..4a9a2cb87 --- /dev/null +++ b/filtering/pose_filtering/CMakeLists.txt @@ -0,0 +1,111 @@ +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 +) + +target_include_directories(${CORE_LIB_NAME} + PUBLIC + $ + $ +) + +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() diff --git a/filtering/pose_filtering/README.md b/filtering/pose_filtering/README.md new file mode 100644 index 000000000..63abc463f --- /dev/null +++ b/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 + +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 measurements; + double dt = 0.1; // seconds + manager.step(measurements, dt); + + const auto& tracks = manager.get_tracks(); + // iterate tracks for id, pose, existence probability, etc. +} +``` diff --git a/filtering/pose_filtering/config/pose_filtering.yaml b/filtering/pose_filtering/config/pose_filtering.yaml new file mode 100644 index 000000000..02993427b --- /dev/null +++ b/filtering/pose_filtering/config/pose_filtering.yaml @@ -0,0 +1,37 @@ +/**: + 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: 200 + enu_ned_rotation: true + + config: + existence: + confirmation_threshold: 0.6 + deletion_threshold: 0.2 + initial_existence_probability: 0.4 + + gate: + min_pos_error: 0.0 # in meters + max_pos_error: 0.5 # in meters + min_ori_error: 0.0 # in radians + max_ori_error: 0.5 # in radians + + dyn_mod_std_dev: 0.2 + sens_mod_std_dev: 0.2 + + init_pos_std_dev: 0.1 + init_ori_std_dev: 0.05 + + mahalanobis_gate_threshold: 2.5 # in std deviations + prob_of_detection: 0.5 + clutter_intensity: 0.01 + prob_of_survival: 0.95 + estimate_clutter: false + + debug: + enable: true + topic_name_meas: "/pose_debug_meas" + topic_name_state: "/pose_debug_state" diff --git a/filtering/pose_filtering/include/pose_filtering/lib/pose_track_manager.hpp b/filtering/pose_filtering/include/pose_filtering/lib/pose_track_manager.hpp new file mode 100644 index 000000000..fbb4f2b83 --- /dev/null +++ b/filtering/pose_filtering/include/pose_filtering/lib/pose_track_manager.hpp @@ -0,0 +1,248 @@ +#ifndef POSE_FILTERING__LIB__POSE_TRACK_MANAGER_HPP_ +#define POSE_FILTERING__LIB__POSE_TRACK_MANAGER_HPP_ + +#include +#include +#include +#include "typedefs.hpp" + +namespace vortex::filtering { + +/** + * @file pose_track_manager.hpp + * @brief Track management utilities for pose-based measurements. + * + * The PoseTrackManager implements track creation, confirmation, + * deletion and update logic using an IPDA filter for each track. It + * handles spatial and angular gating for associating pose measurements + * to existing tracks. + */ + +/** + * @brief Struct representing a 6D pose gating mechanism. + * + * This struct is used to determine whether a given pose measurement + * falls within acceptable positional and orientational error bounds. + */ +struct PoseGate6D { + double min_pos_error = 0.0; + double max_pos_error = std::numeric_limits::infinity(); + double min_ori_error = 0.0; + double max_ori_error = std::numeric_limits::infinity(); + + double mahalanobis_threshold = std::numeric_limits::infinity(); + + using Vec_z = IPDA::Vec_z; + using Gauss_z = IPDA::Gauss_z; + + bool operator()(const Vec_z& z, const Gauss_z& z_pred) const { + const Vec_z r = z - z_pred.mean(); + const double pos = r.template head<3>().norm(); + const double ori = r.template tail<3>().norm(); + + if (pos > max_pos_error || ori > max_ori_error) { + return false; + } + + if (pos <= min_pos_error && ori <= min_ori_error) { + return true; + } + + return z_pred.mahalanobis_distance(z) <= mahalanobis_threshold; + } +}; + +/** + * @brief Class responsible for maintaining a set of tracks based on + * pose measurements. + * + * Public API: + * - construct with a TrackManagerConfig + * - call step() each frame with a list of measurements and the time step + * - retrieve current tracks with get_tracks() + */ +class PoseTrackManager { + public: + /** + * @brief Construct a PoseTrackManager + * @param config Configuration parameters for IPDA, models and existence + * management + */ + explicit PoseTrackManager(const TrackManagerConfig& config); + + /** + * @brief Perform a single tracking step: associate measurements, update + * filters and manage track lifecycle. + * @param measurements Vector of pose measurements (will be modified by + * the manager to remove consumed measurements) + * @param dt Time step in seconds + */ + void step(std::vector& measurements, double dt); + + /** + * @brief Get the list of currently maintained tracks. + * @return const reference to internal track vector + */ + const std::vector& get_tracks() const { return tracks_; } + + /** + * @brief Return whether we have a track of the specified type and subtype. + * @param class_key LandmarkClassKey to match + * @return Whether at least one confirmed track with the specified class key + * exists. + */ + bool has_track(const LandmarkClassKey& class_key) const { + return std::any_of(tracks_.begin(), tracks_.end(), [&](const Track& t) { + return t.confirmed && t.class_key == class_key; + }); + } + + /** + * @brief Return whether we have a track with the specified ID. + * @param id of the track + * @return Whether at least one track with the specified ID exists. + */ + bool has_track(int id) const { + return std::any_of(tracks_.begin(), tracks_.end(), [&](const Track& t) { + return t.confirmed && t.id == id; + }); + } + + /** + * @brief Get all confirmed tracks of the specified class key. + * @param class_key LandmarkClassKey to match + * @return Vector of pointers to tracks matching the specified class key. + */ + std::vector get_tracks_by_type( + const LandmarkClassKey& class_key) const { + std::vector out; + out.reserve(tracks_.size()); + for (const auto& t : tracks_) { + if (t.class_key == class_key && t.confirmed) { + out.push_back(&t); + } + } + return out; + } + + private: + /** + * @brief Gate measurements for a track based on class key and gating + * parameters. + * @param track Track for which to gate measurements + * @param measurements Vector of measurements to gate against + * @return Vector of indices of measurements that passed the gate + */ + std::vector gate_measurements_by_class( + const Track& track, + const std::vector& measurements) const; + + /** + * @brief Compute measurement residuals for a track against a set of + * measurements. + * @param track Track for which to compute residuals + * @param measurements Vector of measurements to compute residuals against + * @param indices Indices of measurements to use from the measurements + * vector + * @return 6xN array of residuals (position and orientation) for each + * measurement + */ + Eigen::Array compute_measurement_residuals( + const Track& track, + const std::vector& measurements, + const std::vector& indices) const; + + /** + * @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) const; + + /** + * @brief Inject the error state into the nominal state and reset the error + * state. + * @param track Track for which to perform the injection and reset + */ + void inject_and_reset(Track& track); + + /** + * @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) const; + + /** + * @brief Confirm tracks which have exceeded the confirmation threshold. + */ + void confirm_tracks(); + + /** + * @brief Delete tracks which have fallen below the deletion threshold. + */ + void delete_tracks(); + + /** + * @brief Create new tracks from unassociated measurements. + * @param measurements Measurements to be used for new track creation + */ + void create_tracks(const std::vector& measurements); + + /** + * @brief Sort tracks by priority to determine processing / deletion order. + */ + void sort_tracks_by_priority(); + + /** + * @brief Erase measurements which have been gated / consumed. + * @param measurements Measurements vector (modified in place) + * @param indices Indices of measurements to erase + */ + void erase_gated_measurements( + std::vector& measurements, + const std::vector& global_indices, + const Eigen::Array& mask) const; + + /** + * @brief Get the configuration for a specific landmark class key. + * @param key Landmark class key + * @return Configuration for the specified class key + */ + const LandmarkClassConfig& cfg_for(const LandmarkClassKey& key) const { + for (const auto& [k, cfg] : config_.per_class_configs) { + if (k == key) + return cfg; + } + return config_.default_class_config; + } + + /** + * @brief Get the configuration for a specific track. + * @param t Track for which to get the configuration + * @return Configuration for the specified track + */ + const LandmarkClassConfig& cfg_for(const Track& t) const { + return cfg_for(t.class_key); + } + + /** + * @brief Get the configuration for a specific landmark. + * @param m Landmark for which to get the configuration + * @return Configuration for the specified landmark + */ + const LandmarkClassConfig& cfg_for(const Landmark& m) const { + return cfg_for(m.class_key); + } + + // Internal bookkeeping + int track_id_counter_{0}; + + std::vector tracks_; + + TrackManagerConfig config_; +}; + +} // namespace vortex::filtering + +#endif // POSE_FILTERING__LIB__POSE_TRACK_MANAGER_HPP_ diff --git a/filtering/pose_filtering/include/pose_filtering/lib/typedefs.hpp b/filtering/pose_filtering/include/pose_filtering/lib/typedefs.hpp new file mode 100644 index 000000000..736f73076 --- /dev/null +++ b/filtering/pose_filtering/include/pose_filtering/lib/typedefs.hpp @@ -0,0 +1,165 @@ +#ifndef POSE_FILTERING__LIB__TYPEDEFS_HPP_ +#define POSE_FILTERING__LIB__TYPEDEFS_HPP_ + +#include +#include +#include +#include +#include +#include + +namespace vortex::filtering { + +/** + * @file typedefs.hpp + * @brief Common type aliases and configuration structs used by the + * pose_filtering library. + * + * This header exposes convenient using-aliases for commonly used types + * (state, dynamic and sensor models, IPDA filter instance, and Pose type) + * and configuration structs that are passed to the track manager. + */ + +/** + * @brief Gaussian 6d state representation (mean + covariance). + */ +using State6d = vortex::prob::Gauss6d; + +/** + * @brief Discrete-time constant dynamic model of order 6. + */ +using DynMod = vortex::models::ConstantDynamicModel<6>; + +/** + * @brief Identity sensor model mapping state to measurement space. + */ +using SensorMod = vortex::models::IdentitySensorModel<6, 6>; + +/** + * @brief IPDA filter type specialized for the chosen dynamic and sensor models. + */ +using IPDA = vortex::filter::IPDA; + +/** + * @brief PDAF filter type specialized for the chosen dynamic and sensor models. + */ +using PDAF = vortex::filter::PDAF; + +/** + * @brief Pose type (position + quaternion) from vortex utils. + */ +using Pose = vortex::utils::types::Pose; + +/** + * @brief Parameters for existence management (track confirmation / deletion). + */ +struct ExistenceManagementConfig { + double confirmation_threshold{0.6}; + double deletion_threshold{0.2}; + double initial_existence_probability{0.5}; +}; + +struct LandmarkClassKey { + uint16_t type{}; + uint16_t subtype{}; + + bool operator==(const LandmarkClassKey&) const = default; +}; + +struct LandmarkClassConfig { + // noise (simple version: one std_dev for all 6 dims) + double dyn_std_dev = 1.0; + double sens_std_dev = 1.0; + + // initial cov (separate pos/orientation) + double init_pos_std = 0.1; + double init_ori_std = 0.05; + + // gating thresholds (your min-pass + hard max + mahal) + double min_pos_error = 0.0; // meters + double max_pos_error = 1.0; // meters + double min_ori_error = 0.0; // radians + double max_ori_error = 0.5; // radians + + double mahalanobis_threshold = 2.5; + double prob_of_detection = 0.5; + double clutter_intensity = 0.01; + double prob_of_survival = 0.95; + bool estimate_clutter = false; +}; + +/** + * @brief High-level track manager configuration struct. + * + * Contains IPDA-specific configuration and parameters for dynamics, + * sensor model, existence management and gating thresholds. + */ +struct TrackManagerConfig { + ExistenceManagementConfig existence; + LandmarkClassConfig default_class_config; + std::vector> + per_class_configs; +}; + +/** + * @brief Struct representing the error state of a track (position and + * orientation errors in local tangent space). + */ +struct ErrorState { + Eigen::Vector3d pos_error; + Eigen::Vector3d ori_error; +}; + +/** + * @brief Struct representing the nominal state of a track (position and + * orientation). + */ +struct NominalState { + Eigen::Vector3d pos; + Eigen::Quaterniond ori; +}; + +struct Landmark { + vortex::utils::types::Pose pose{}; + LandmarkClassKey class_key{}; +}; + +/** + * @brief Representation of a single track maintained by the track manager. + * + * The Track stores the filter state, orientation quaternions and bookkeeping + * fields used for existence management and confirmation. + */ +struct Track { + /// Unique track identifier + int id{}; + + /// Landmark class (type + subtype) + LandmarkClassKey class_key{}; + + /// Nominal state representation (position and orientation) + NominalState nominal_state; + + /// Error state representation (position and orientation errors) + State6d error_state; + + /// Probability that the track exists (0..1) + double existence_probability{0.0}; + + /// Whether the track has been confirmed (passed confirmation threshold) + bool confirmed{false}; + + /** + * @brief Convert the track position + orientation to a Pose object. + * @return vortex::utils::types::Pose constructed from the state mean and + * current_orientation + */ + Pose to_pose() const { + return vortex::utils::types::Pose::from_eigen(nominal_state.pos, + nominal_state.ori); + } +}; + +} // namespace vortex::filtering + +#endif // POSE_FILTERING__LIB__TYPEDEFS_HPP_ diff --git a/filtering/pose_filtering/include/pose_filtering/ros/pose_filtering_ros.hpp b/filtering/pose_filtering/include/pose_filtering/ros/pose_filtering_ros.hpp new file mode 100644 index 000000000..e42f93a50 --- /dev/null +++ b/filtering/pose_filtering/include/pose_filtering/ros/pose_filtering_ros.hpp @@ -0,0 +1,89 @@ +#ifndef POSE_FILTERING__ROS__POSE_FILTERING_ROS_HPP_ +#define POSE_FILTERING__ROS__POSE_FILTERING_ROS_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "pose_filtering/lib/pose_track_manager.hpp" + +namespace vortex::filtering { + +using PoseMsgT = geometry_msgs::msg::PoseStamped; + +template +concept ValidPoseMsg = + std::same_as || + std::same_as || + std::same_as || + std::same_as; + +static_assert(ValidPoseMsg, + "PoseMsgT must be a supported pose message type"); + +class PoseFilteringNode : public rclcpp::Node { + public: + explicit PoseFilteringNode(const rclcpp::NodeOptions& options); + + ~PoseFilteringNode() {} + + private: + void setup_publishers_and_subscribers(); + + void setup_track_manager(); + + void create_pose_subscription(const std::string& topic_name, + const rmw_qos_profile_t& qos_profile); + + void timer_callback(); + + void setup_debug_publishers(); + + void publish_meas_debug(); + + void publish_state_debug(); + + std::shared_ptr> subscriber_; + + std::shared_ptr> tf_filter_; + + std::string target_frame_; + std::shared_ptr tf2_buffer_; + std::shared_ptr tf2_listener_; + + rclcpp::Publisher::SharedPtr pose_array_pub_; + + rclcpp::Publisher::SharedPtr + landmark_array_pub_; + + rclcpp::TimerBase::SharedPtr pub_timer_; + std::chrono::milliseconds filter_dt_{0}; + + std::unique_ptr track_manager_; + + std::vector measurements_; + bool enu_ned_rotation_{false}; + + bool debug_{false}; + rclcpp::Publisher::SharedPtr + pose_meas_debug_pub_; + rclcpp::Publisher::SharedPtr + pose_state_debug_pub_; +}; + +} // namespace vortex::filtering + +#endif // POSE_FILTERING__ROS__POSE_FILTERING_ROS_HPP_ diff --git a/filtering/pose_filtering/include/pose_filtering/ros/pose_filtering_ros_conversions.hpp b/filtering/pose_filtering/include/pose_filtering/ros/pose_filtering_ros_conversions.hpp new file mode 100644 index 000000000..8d3be638d --- /dev/null +++ b/filtering/pose_filtering/include/pose_filtering/ros/pose_filtering_ros_conversions.hpp @@ -0,0 +1,63 @@ +#ifndef POSE_FILTERING__ROS__POSE_FILTERING_ROS_CONVERSIONS_HPP_ +#define POSE_FILTERING__ROS__POSE_FILTERING_ROS_CONVERSIONS_HPP_ + +#include +#include +#include +#include +#include +#include +#include "pose_filtering/lib/typedefs.hpp" + +namespace vortex::filtering::ros_conversions { + +/** + * @brief Convert a Track object to a PoseWithCovariance ROS message. + * @param track The Track object to convert. + * @return geometry_msgs::msg::PoseWithCovariance message representing the + * track. + */ +geometry_msgs::msg::PoseWithCovariance track_to_pose_with_covariance( + const Track& track); + +/** + * @brief Convert a vector of Track objects to a LandmarkArray ROS message. + * @param tracks The vector of Track objects to convert. + * @param timestamp The timestamp to set in the LandmarkArray header. + * @param frame_id The frame ID to set in the LandmarkArray header. + * @return vortex_msgs::msg::LandmarkArray message representing the tracks. + */ +vortex_msgs::msg::LandmarkArray tracks_to_landmark_array_msg( + const std::vector& tracks, + const rclcpp::Time& timestamp, + const std::string& frame_id); + +/** + * @brief Create a Landmark object from a Pose message and a class key. + * @param pose The Pose message to convert. + * @param key The class key for the landmark. + * @return Landmark object representing the pose. + */ +Landmark make_landmark_from_pose(const geometry_msgs::msg::Pose& pose, + const LandmarkClassKey& key); + +std::vector ros_to_landmarks( + const geometry_msgs::msg::PoseStamped& msg); + +std::vector ros_to_landmarks( + const geometry_msgs::msg::PoseWithCovarianceStamped& msg); + +std::vector ros_to_landmarks( + const geometry_msgs::msg::PoseWithCovariance& msg); + +std::vector ros_to_landmarks(const geometry_msgs::msg::Pose& msg); + +std::vector ros_to_landmarks( + const geometry_msgs::msg::PoseArray& msg); + +std::vector ros_to_landmarks( + const vortex_msgs::msg::LandmarkArray& msg); + +} // namespace vortex::filtering::ros_conversions + +#endif // POSE_FILTERING__ROS__POSE_FILTERING_ROS_CONVERSIONS_HPP_ diff --git a/filtering/pose_filtering/launch/pose_filtering.launch.py b/filtering/pose_filtering/launch/pose_filtering.launch.py new file mode 100644 index 000000000..f7e2d5cf2 --- /dev/null +++ b/filtering/pose_filtering/launch/pose_filtering.launch.py @@ -0,0 +1,30 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + config = os.path.join( + get_package_share_directory('pose_filtering'), + 'config', + 'pose_filtering.yaml', + ) + + return LaunchDescription( + [ + Node( + package='pose_filtering', + executable='pose_filtering_node', + name='pose_filtering', + output='screen', + parameters=[ + config, + { + 'use_sim_time': False, + }, # If testing with rosbags sim_time might be preferred if bag is looped + ], + ), + ] + ) diff --git a/filtering/pose_filtering/package.xml b/filtering/pose_filtering/package.xml new file mode 100644 index 000000000..bbd9167e8 --- /dev/null +++ b/filtering/pose_filtering/package.xml @@ -0,0 +1,29 @@ + + + + pose_filtering + 0.0.0 + Pose filtering and tracking package, with a C++ track manager and ROS 2 interface node. + + jorgen + MIT + + ament_cmake + + rclcpp + rclcpp_components + geometry_msgs + tf2 + tf2_ros + tf2_geometry_msgs + message_filters + vortex_utils + vortex_utils_ros + vortex_utils_ros_tf + vortex_filtering + vortex_msgs + + + ament_cmake + + diff --git a/filtering/pose_filtering/src/lib/pose_track_manager.cpp b/filtering/pose_filtering/src/lib/pose_track_manager.cpp new file mode 100644 index 000000000..424c6d83e --- /dev/null +++ b/filtering/pose_filtering/src/lib/pose_track_manager.cpp @@ -0,0 +1,234 @@ +#include "pose_filtering/lib/pose_track_manager.hpp" +#include +#include +#include +#include +#include "pose_filtering/lib/typedefs.hpp" + +namespace vortex::filtering { + +PoseTrackManager::PoseTrackManager(const TrackManagerConfig& config) + : track_id_counter_(0), config_(config) {} + +void PoseTrackManager::step(std::vector& measurements, double dt) { + sort_tracks_by_priority(); + for (Track& track : tracks_) { + const auto& cfg = cfg_for(track); + auto type_gate_indices = + gate_measurements_by_class(track, measurements); + auto type_matched_measurements = compute_measurement_residuals( + track, measurements, type_gate_indices); + + PoseGate6D gate; + gate.min_pos_error = cfg.min_pos_error; + gate.max_pos_error = cfg.max_pos_error; + gate.min_ori_error = cfg.min_ori_error; + gate.max_ori_error = cfg.max_ori_error; + gate.mahalanobis_threshold = cfg.mahalanobis_threshold; + + DynMod dyn_mod(cfg.dyn_std_dev); + SensorMod sensor_mod(cfg.sens_std_dev); + + vortex::filter::IPDA::Config ipda_cfg; + ipda_cfg.ipda.estimate_clutter = cfg.estimate_clutter; + ipda_cfg.ipda.prob_of_survival = cfg.prob_of_survival; + ipda_cfg.pdaf.mahalanobis_threshold = cfg.mahalanobis_threshold; + ipda_cfg.pdaf.prob_of_detection = cfg.prob_of_detection; + ipda_cfg.pdaf.clutter_intensity = cfg.clutter_intensity; + + const IPDA::State state_est_prev{ + .x_estimate = track.error_state, + .existence_probability = track.existence_probability}; + + auto ipda_output = + IPDA::step(dyn_mod, sensor_mod, dt, state_est_prev, + type_matched_measurements, ipda_cfg, gate); + + track.error_state = ipda_output.state.x_estimate; + track.existence_probability = ipda_output.state.existence_probability; + inject_and_reset(track); + erase_gated_measurements(measurements, type_gate_indices, + ipda_output.gated_measurements); + } + confirm_tracks(); + delete_tracks(); + create_tracks(measurements); +} + +std::vector PoseTrackManager::gate_measurements_by_class( + const Track& track, + const std::vector& measurements) const { + std::vector idx; + idx.reserve(measurements.size()); + + for (Eigen::Index i = 0; i < static_cast(measurements.size()); + ++i) { + const auto& m = measurements[i]; + if (m.class_key == track.class_key) { + idx.push_back(i); + } + } + return idx; +} + +Eigen::Array +PoseTrackManager::compute_measurement_residuals( + const Track& track, + const std::vector& measurements, + const std::vector& indices) const { + IPDA::Arr_zXd Z(6, indices.size()); + + for (Eigen::Index k = 0; k < static_cast(indices.size()); + ++k) { + const Pose& meas_pose = measurements[indices[k]].pose; + + const Eigen::Vector3d dp = + meas_pose.pos_vector() - track.nominal_state.pos; + const Eigen::Vector3d dtheta = so3_log_quat( + meas_pose.ori_quaternion() * track.nominal_state.ori.conjugate()); + + Z.matrix().col(k).head<3>() = dp; + Z.matrix().col(k).tail<3>() = dtheta; + } + + return Z; +} + +void PoseTrackManager::inject_and_reset(Track& track) { + const Eigen::Matrix delta = track.error_state.mean(); + + track.nominal_state.pos += delta.head<3>(); + + track.nominal_state.ori = + so3_exp_quat(delta.tail<3>()) * track.nominal_state.ori; + track.nominal_state.ori.normalize(); + + // First-order reset Jacobian for left-injected SO(3) error: + // G_theta ≈ I - 0.5 * skew(delta_theta) + Eigen::Matrix G = Eigen::Matrix::Identity(); + G.block<3, 3>(3, 3) = + Eigen::Matrix3d::Identity() - + 0.5 * vortex::utils::math::get_skew_symmetric_matrix(delta.tail<3>()); + + track.error_state.cov() = G * track.error_state.cov() * G.transpose(); + + track.error_state.mean().setZero(); +} + +Eigen::Vector3d PoseTrackManager::so3_log_quat( + const Eigen::Quaterniond& q_in) const { + Eigen::Quaterniond q = q_in.normalized(); + + if (q.w() < 0.0) { + // Quat sign ambiguity q == -q + // Enforce consistent sign convention + q.coeffs() *= -1.0; + } + + double norm_v = q.vec().norm(); + + if (norm_v < 1e-6) { + // Small-angle approximation of log map: + // for θ → 0, log(q) ≈ 2 * v since q ≈ [1, v] + return 2.0 * q.vec(); + } + + double theta = 2.0 * std::atan2(norm_v, q.w()); + return theta * q.vec() / norm_v; +} + +Eigen::Quaterniond PoseTrackManager::so3_exp_quat( + const Eigen::Vector3d& rvec) const { + double theta = rvec.norm(); + + if (theta < 1e-6) { + return Eigen::Quaterniond(1.0, 0.5 * rvec.x(), 0.5 * rvec.y(), + 0.5 * rvec.z()) + .normalized(); + } + + Eigen::Vector3d axis = rvec / theta; + double half = 0.5 * theta; + + return Eigen::Quaterniond(std::cos(half), axis.x() * std::sin(half), + axis.y() * std::sin(half), + axis.z() * std::sin(half)); +} + +void PoseTrackManager::erase_gated_measurements( + std::vector& measurements, + const std::vector& global_indices, + const Eigen::Array& mask) const { + std::vector to_erase; + to_erase.reserve(global_indices.size()); + + for (Eigen::Index k = 0; + k < static_cast(global_indices.size()); ++k) { + if (mask(k)) { // <-- confirm semantics: true == "use/associated" + to_erase.push_back(global_indices[k]); + } + } + + std::sort(to_erase.begin(), to_erase.end(), std::greater()); + for (Eigen::Index idx : to_erase) { + measurements.erase(measurements.begin() + idx); + } +} + +void PoseTrackManager::create_tracks( + const std::vector& measurements) { + tracks_.reserve(tracks_.size() + measurements.size()); + + auto make_track = [this](const Landmark& measurement) { + const auto& cfg = cfg_for(measurement); + + Eigen::Matrix P0 = Eigen::Matrix::Zero(); + P0.block<3, 3>(0, 0).setIdentity(); + P0.block<3, 3>(3, 3).setIdentity(); + + P0.block<3, 3>(0, 0) *= cfg.init_pos_std * cfg.init_pos_std; + P0.block<3, 3>(3, 3) *= cfg.init_ori_std * cfg.init_ori_std; + + return Track{.id = track_id_counter_++, + .class_key = measurement.class_key, + .nominal_state = + NominalState{.pos = measurement.pose.pos_vector(), + .ori = measurement.pose.ori_quaternion()}, + .error_state = vortex::prob::Gauss6d( + Eigen::Matrix::Zero(), P0), + .existence_probability = + config_.existence.initial_existence_probability, + .confirmed = false}; + }; + + for (const Landmark& m : measurements) { + tracks_.push_back(make_track(m)); + } +} + +void PoseTrackManager::confirm_tracks() { + for (Track& track : tracks_) { + if (!track.confirmed && track.existence_probability >= + config_.existence.confirmation_threshold) { + track.confirmed = true; + } + } +} + +void PoseTrackManager::delete_tracks() { + auto new_end = std::ranges::remove_if(tracks_, [this](Track& track) { + return track.existence_probability < + config_.existence.deletion_threshold; + }); + tracks_.erase(new_end.begin(), new_end.end()); +} + +void PoseTrackManager::sort_tracks_by_priority() { + std::ranges::sort(tracks_, [](const Track& a, const Track& b) { + if (a.confirmed != b.confirmed) + return a.confirmed > b.confirmed; + return a.existence_probability > b.existence_probability; + }); +} + +} // namespace vortex::filtering diff --git a/filtering/pose_filtering/src/ros/pose_filtering_ros.cpp b/filtering/pose_filtering/src/ros/pose_filtering_ros.cpp new file mode 100644 index 000000000..e156f9ba4 --- /dev/null +++ b/filtering/pose_filtering/src/ros/pose_filtering_ros.cpp @@ -0,0 +1,182 @@ +#include "pose_filtering/ros/pose_filtering_ros.hpp" +#include +#include +#include +#include +#include "pose_filtering/ros/pose_filtering_ros_conversions.hpp" +#include "vortex/utils/ros/ros_conversions.hpp" +#include "vortex/utils/ros/ros_transforms.hpp" + +namespace vortex::filtering { + +PoseFilteringNode::PoseFilteringNode(const rclcpp::NodeOptions& options) + : rclcpp::Node("pose_filtering_node", options) { + setup_publishers_and_subscribers(); + setup_track_manager(); +} + +void PoseFilteringNode::setup_publishers_and_subscribers() { + enu_ned_rotation_ = this->declare_parameter("enu_ned_rotation"); + const auto qos_sensor_data_pub{ + vortex::utils::qos_profiles::sensor_data_profile(1)}; + std::string pub_topic_name = + this->declare_parameter("pose_array_pub_topic"); + + std::string landmark_pub_topic = + this->declare_parameter("landmark_pub_topic"); + + pose_array_pub_ = this->create_publisher( + pub_topic_name, qos_sensor_data_pub); + + landmark_array_pub_ = + this->create_publisher( + landmark_pub_topic, qos_sensor_data_pub); + + filter_dt_ = std::chrono::milliseconds( + this->declare_parameter("timer_rate_ms")); + + pub_timer_ = this->create_wall_timer(filter_dt_, + [this]() { this->timer_callback(); }); + + target_frame_ = this->declare_parameter("target_frame"); + + tf2_buffer_ = std::make_shared(this->get_clock()); + + auto timer_interface = std::make_shared( + this->get_node_base_interface(), this->get_node_timers_interface()); + + tf2_buffer_->setCreateTimerInterface(timer_interface); + tf2_listener_ = std::make_shared(*tf2_buffer_); + + const std::string pose_sub_topic = + this->declare_parameter("pose_sub_topic"); + + const auto qos_sensor_data_sub{ + vortex::utils::qos_profiles::sensor_data_profile(10)}; + + create_pose_subscription(pose_sub_topic, + qos_sensor_data_sub.get_rmw_qos_profile()); + + debug_ = this->declare_parameter("debug.enable"); + if (debug_) { + setup_debug_publishers(); + } +} + +void PoseFilteringNode::create_pose_subscription( + const std::string& topic_name, + const rmw_qos_profile_t& qos_profile) { + auto sub = std::make_shared>( + this, topic_name, qos_profile); + + auto filter = std::make_shared>( + *sub, *tf2_buffer_, target_frame_, 10, + this->get_node_logging_interface(), this->get_node_clock_interface()); + + filter->registerCallback( + [this](const typename PoseMsgT::ConstSharedPtr msg) { + PoseMsgT pose_tf; + try { + vortex::utils::ros_transforms::transform_pose( + *tf2_buffer_, *msg, target_frame_, pose_tf); + } catch (const tf2::TransformException& ex) { + spdlog::warn("TF transform failed from '{}' to '{}': {}", + msg->header.frame_id.c_str(), + target_frame_.c_str(), ex.what()); + return; + } + + this->measurements_ = + vortex::filtering::ros_conversions::ros_to_landmarks(pose_tf); + if (enu_ned_rotation_) { + std::ranges::for_each(this->measurements_, [](auto& m) { + m.pose.set_ori(vortex::utils::math::enu_ned_rotation( + m.pose.ori_quaternion())); + }); + } + }); + + subscriber_ = sub; + tf_filter_ = filter; +} + +void PoseFilteringNode::setup_track_manager() { + TrackManagerConfig config; + + config.existence.confirmation_threshold = this->declare_parameter( + "config.existence.confirmation_threshold"); + config.existence.deletion_threshold = + this->declare_parameter("config.existence.deletion_threshold"); + config.existence.initial_existence_probability = + this->declare_parameter( + "config.existence.initial_existence_probability"); + + config.default_class_config.min_pos_error = + this->declare_parameter("config.gate.min_pos_error"); + config.default_class_config.max_pos_error = + this->declare_parameter("config.gate.max_pos_error"); + config.default_class_config.min_ori_error = + this->declare_parameter("config.gate.min_ori_error"); + config.default_class_config.max_ori_error = + this->declare_parameter("config.gate.max_ori_error"); + + config.default_class_config.dyn_std_dev = + this->declare_parameter("config.dyn_mod_std_dev"); + config.default_class_config.sens_std_dev = + this->declare_parameter("config.sens_mod_std_dev"); + + config.default_class_config.init_pos_std = + this->declare_parameter("config.init_pos_std_dev"); + config.default_class_config.init_ori_std = + this->declare_parameter("config.init_ori_std_dev"); + + config.default_class_config.mahalanobis_threshold = + this->declare_parameter("config.mahalanobis_gate_threshold"); + config.default_class_config.prob_of_detection = + this->declare_parameter("config.prob_of_detection"); + config.default_class_config.clutter_intensity = + this->declare_parameter("config.clutter_intensity"); + config.default_class_config.prob_of_survival = + this->declare_parameter("config.prob_of_survival"); + config.default_class_config.estimate_clutter = + this->declare_parameter("config.estimate_clutter"); + + track_manager_ = std::make_unique(config); +} + +void PoseFilteringNode::timer_callback() { + if (debug_) { + publish_meas_debug(); + } + track_manager_->step(measurements_, + std::chrono::duration(filter_dt_).count()); + measurements_.clear(); + if (debug_) { + publish_state_debug(); + } + + geometry_msgs::msg::PoseArray pose_array; + pose_array.header.frame_id = target_frame_; + pose_array.header.stamp = this->get_clock()->now(); + for (const Track& track : track_manager_->get_tracks()) { + if (!track.confirmed) + continue; + pose_array.poses.push_back( + vortex::utils::ros_conversions::to_pose_msg(track.to_pose())); + } + + if (pose_array.poses.empty()) { + return; + } + pose_array_pub_->publish(pose_array); + + vortex_msgs::msg::LandmarkArray landmark_array_msg = + vortex::filtering::ros_conversions::tracks_to_landmark_array_msg( + track_manager_->get_tracks(), this->get_clock()->now(), + target_frame_); + landmark_array_pub_->publish(landmark_array_msg); +} + +RCLCPP_COMPONENTS_REGISTER_NODE(PoseFilteringNode); + +} // namespace vortex::filtering diff --git a/filtering/pose_filtering/src/ros/pose_filtering_ros_conversions.cpp b/filtering/pose_filtering/src/ros/pose_filtering_ros_conversions.cpp new file mode 100644 index 000000000..f1c6fdd94 --- /dev/null +++ b/filtering/pose_filtering/src/ros/pose_filtering_ros_conversions.cpp @@ -0,0 +1,125 @@ +#include "pose_filtering/ros/pose_filtering_ros_conversions.hpp" + +namespace vortex::filtering::ros_conversions { + +geometry_msgs::msg::PoseWithCovariance track_to_pose_with_covariance( + const Track& track) { + geometry_msgs::msg::PoseWithCovariance msg; + + msg.pose.position.x = track.nominal_state.pos(0); + msg.pose.position.y = track.nominal_state.pos(1); + msg.pose.position.z = track.nominal_state.pos(2); + + msg.pose.orientation.w = track.nominal_state.ori.w(); + msg.pose.orientation.x = track.nominal_state.ori.x(); + msg.pose.orientation.y = track.nominal_state.ori.y(); + msg.pose.orientation.z = track.nominal_state.ori.z(); + + msg.covariance.fill(0.0); + + const auto& Pp = track.error_state.cov(); + msg.covariance[0 * 6 + 0] = Pp(0, 0); // xx + msg.covariance[0 * 6 + 1] = Pp(0, 1); // xy + msg.covariance[0 * 6 + 2] = Pp(0, 2); // xz + + msg.covariance[1 * 6 + 0] = Pp(1, 0); // yx + msg.covariance[1 * 6 + 1] = Pp(1, 1); // yy + msg.covariance[1 * 6 + 2] = Pp(1, 2); // yz + + msg.covariance[2 * 6 + 0] = Pp(2, 0); // zx + msg.covariance[2 * 6 + 1] = Pp(2, 1); // zy + msg.covariance[2 * 6 + 2] = Pp(2, 2); // zz + + msg.covariance[3 * 6 + 3] = Pp(3, 3); // RR + msg.covariance[3 * 6 + 4] = Pp(3, 4); // RP + msg.covariance[3 * 6 + 5] = Pp(3, 5); // RY + + msg.covariance[4 * 6 + 3] = Pp(4, 3); // PR + msg.covariance[4 * 6 + 4] = Pp(4, 4); // PP + msg.covariance[4 * 6 + 5] = Pp(4, 5); // PY + + msg.covariance[5 * 6 + 3] = Pp(5, 3); // YR + msg.covariance[5 * 6 + 4] = Pp(5, 4); // YP + msg.covariance[5 * 6 + 5] = Pp(5, 5); // YY + + return msg; +} + +vortex_msgs::msg::LandmarkArray tracks_to_landmark_array_msg( + const std::vector& tracks, + const rclcpp::Time& timestamp, + const std::string& frame_id) { + vortex_msgs::msg::LandmarkArray landmark_array_msg; + landmark_array_msg.header.stamp = timestamp; + landmark_array_msg.header.frame_id = frame_id; + + for (const auto& track : tracks) { + vortex_msgs::msg::Landmark landmark; + landmark.pose = track_to_pose_with_covariance(track); + landmark.type.value = track.class_key.type; + landmark.subtype.value = track.class_key.subtype; + landmark.id = track.id; + landmark_array_msg.landmarks.push_back(landmark); + } + + return landmark_array_msg; +} + +Landmark make_landmark_from_pose(const geometry_msgs::msg::Pose& pose, + const LandmarkClassKey& key) { + Landmark lm; + lm.pose = vortex::utils::ros_conversions::ros_pose_to_pose(pose); + lm.class_key = key; + return lm; +} + +std::vector ros_to_landmarks( + const geometry_msgs::msg::PoseStamped& msg) { + return {make_landmark_from_pose(msg.pose, + LandmarkClassKey{.type = 0, .subtype = 0})}; +} + +std::vector ros_to_landmarks( + const geometry_msgs::msg::PoseWithCovarianceStamped& msg) { + return {make_landmark_from_pose(msg.pose.pose, + LandmarkClassKey{.type = 0, .subtype = 0})}; +} + +std::vector ros_to_landmarks( + const geometry_msgs::msg::PoseWithCovariance& msg) { + return {make_landmark_from_pose(msg.pose, + LandmarkClassKey{.type = 0, .subtype = 0})}; +} + +std::vector ros_to_landmarks(const geometry_msgs::msg::Pose& msg) { + return {make_landmark_from_pose(msg, + LandmarkClassKey{.type = 0, .subtype = 0})}; +} + +std::vector ros_to_landmarks( + const geometry_msgs::msg::PoseArray& msg) { + std::vector out; + out.reserve(msg.poses.size()); + for (const auto& p : msg.poses) { + out.push_back(make_landmark_from_pose( + p, LandmarkClassKey{.type = 0, .subtype = 0})); + } + return out; +} + +std::vector ros_to_landmarks( + const vortex_msgs::msg::LandmarkArray& msg) { + std::vector out; + out.reserve(msg.landmarks.size()); + for (const auto& lm_msg : msg.landmarks) { + Landmark lm; + lm.pose = + vortex::utils::ros_conversions::ros_pose_to_pose(lm_msg.pose.pose); + lm.class_key = + LandmarkClassKey{lm_msg.type.value, lm_msg.subtype.value}; + out.push_back(lm); + } + return out; +} + +} // namespace vortex::filtering::ros_conversions diff --git a/filtering/pose_filtering/src/ros/pose_filtering_ros_debug.cpp b/filtering/pose_filtering/src/ros/pose_filtering_ros_debug.cpp new file mode 100644 index 000000000..2268a0bc3 --- /dev/null +++ b/filtering/pose_filtering/src/ros/pose_filtering_ros_debug.cpp @@ -0,0 +1,69 @@ +#include +#include +#include "pose_filtering/ros/pose_filtering_ros.hpp" + +namespace vortex::filtering { + +void PoseFilteringNode::setup_debug_publishers() { + std::string meas_topic_name = + this->declare_parameter("debug.topic_name_meas"); + std::string state_topic_name = + this->declare_parameter("debug.topic_name_state"); + const auto qos_sensor_data{ + vortex::utils::qos_profiles::sensor_data_profile(10)}; + pose_meas_debug_pub_ = + this->create_publisher( + meas_topic_name, qos_sensor_data); + pose_state_debug_pub_ = + this->create_publisher( + state_topic_name, qos_sensor_data); +} + +void PoseFilteringNode::publish_meas_debug() { + if (measurements_.empty()) { + return; + } + Landmark meas = measurements_.at(0); + + vortex_msgs::msg::PoseEulerStamped msg; + msg.header.frame_id = target_frame_; + msg.header.stamp = this->get_clock()->now(); + msg.x = meas.pose.x; + msg.y = meas.pose.y; + msg.z = meas.pose.z; + + Eigen::Vector3d euler = + vortex::utils::math::quat_to_euler(meas.pose.ori_quaternion()); + msg.roll = euler(0); + msg.pitch = euler(1); + msg.yaw = euler(2); + + pose_meas_debug_pub_->publish(msg); +} + +void PoseFilteringNode::publish_state_debug() { + if (track_manager_->get_tracks().empty()) { + return; + } + Track track = track_manager_->get_tracks().at(0); + Eigen::Vector3d pos = track.nominal_state.pos; + Eigen::Quaterniond quat = track.nominal_state.ori; + + Eigen::Vector3d euler = vortex::utils::math::quat_to_euler(quat); + + vortex_msgs::msg::PoseEulerStamped msg; + msg.header.frame_id = target_frame_; + msg.header.stamp = this->get_clock()->now(); + + msg.x = pos.x(); + msg.y = pos.y(); + msg.z = pos.z(); + + msg.roll = euler(0); + msg.pitch = euler(1); + msg.yaw = euler(2); + + pose_state_debug_pub_->publish(msg); +} + +} // namespace vortex::filtering diff --git a/filtering/pose_filtering/test/CMakeLists.txt b/filtering/pose_filtering/test/CMakeLists.txt new file mode 100644 index 000000000..1408297d9 --- /dev/null +++ b/filtering/pose_filtering/test/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 3.8) + +find_package(ament_cmake REQUIRED) +find_package(GTest REQUIRED) +find_package(vortex_filtering REQUIRED) +find_package(vortex_utils REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(tf2 REQUIRED) +include(GoogleTest) + +set(TEST_BINARY_NAME ${PROJECT_NAME}_test) +add_executable( + ${TEST_BINARY_NAME} + test_pose_filtering.cpp +) + +target_link_libraries( + ${TEST_BINARY_NAME} + ${PROJECT_NAME} + GTest::GTest + GTest::gtest_main +) + +ament_target_dependencies( + ${TEST_BINARY_NAME} + vortex_filtering + vortex_utils + Eigen3 + tf2 +) + +gtest_discover_tests(${TEST_BINARY_NAME}) diff --git a/filtering/pose_filtering/test/test_pose_filtering.cpp b/filtering/pose_filtering/test/test_pose_filtering.cpp new file mode 100644 index 000000000..6451aa715 --- /dev/null +++ b/filtering/pose_filtering/test/test_pose_filtering.cpp @@ -0,0 +1,146 @@ +#include + +#include + +#include +#include "pose_filtering/lib/pose_track_manager.hpp" + +namespace vortex::filtering { + +class PoseTrackManagerTests : public ::testing::Test { + protected: + TrackManagerConfig make_default_config() { + TrackManagerConfig cfg{}; + cfg.default_class_config.dyn_std_dev = 0.1; + cfg.default_class_config.sens_std_dev = 0.1; + return cfg; + } + + Landmark make_landmark( + const Eigen::Vector3d& pos, + const Eigen::Quaterniond& q = Eigen::Quaterniond::Identity()) { + return Landmark{Pose::from_eigen(pos, q), LandmarkClassKey{0, 0}}; + } +}; + +TEST_F(PoseTrackManagerTests, creates_tracks_from_measurements) { + PoseTrackManager mgr(make_default_config()); + + std::vector measurements{ + make_landmark({0.0, 0.0, 0.0}), + make_landmark({1.0, 0.0, 0.0}), + make_landmark({0.0, 1.0, 0.0}), + }; + + mgr.step(measurements, 0.1); + + const auto& tracks = mgr.get_tracks(); + ASSERT_EQ(tracks.size(), 3); + + for (const auto& t : tracks) { + EXPECT_FALSE(t.confirmed); + EXPECT_NEAR(t.existence_probability, 0.5, 1e-12); + EXPECT_NEAR(t.nominal_state.ori.w(), 1.0, 1e-12); + } +} + +TEST_F(PoseTrackManagerTests, angular_gate_filters_measurements) { + auto cfg = make_default_config(); + cfg.default_class_config.max_ori_error = 0.2; // strict gate + PoseTrackManager mgr(cfg); + + Eigen::Quaterniond q_ref = Eigen::Quaterniond::Identity(); + + Eigen::Quaterniond q_far(Eigen::AngleAxisd(1.0, Eigen::Vector3d::UnitZ())); + + std::vector measurements{ + make_landmark({0, 0, 1}, q_ref), + make_landmark({0, 0, 1}, q_far), + }; + + mgr.step(measurements, 0.1); + + ASSERT_EQ(mgr.get_tracks().size(), 2); + + double t1_exist_prob_step1 = mgr.get_tracks().at(0).existence_probability; + double t2_exist_prob_step1 = mgr.get_tracks().at(1).existence_probability; + + measurements = { + make_landmark({0, 0.25, 1}, q_ref), + make_landmark({0, 0.25, 1}, q_far), + }; + + mgr.step(measurements, 0.1); + + ASSERT_EQ(mgr.get_tracks().size(), 2); + + double t1_exist_prob_step2 = mgr.get_tracks().at(0).existence_probability; + double t2_exist_prob_step2 = mgr.get_tracks().at(1).existence_probability; + + ASSERT_GT(t1_exist_prob_step2, t1_exist_prob_step1); + ASSERT_GT(t2_exist_prob_step2, t2_exist_prob_step1); +} + +TEST_F(PoseTrackManagerTests, single_target_existance_increase) { + auto cfg = make_default_config(); + + Eigen::Quaterniond q = Eigen::Quaterniond::Identity(); + + std::vector z = {make_landmark({0, 0, 0}, q)}; + + PoseTrackManager mgr(cfg); + + mgr.step(z, 0.1); + + double exist_prob1 = mgr.get_tracks().front().existence_probability; + + ASSERT_EQ(mgr.get_tracks().size(), 1); + ASSERT_EQ(exist_prob1, 0.5); + + z = {make_landmark({0, 0, 0}, q)}; + + mgr.step(z, 0.1); + + double exist_prob2 = mgr.get_tracks().front().existence_probability; + + ASSERT_EQ(mgr.get_tracks().size(), 1); + ASSERT_GT(exist_prob2, exist_prob1); + + z = {make_landmark({0, 0, 0}, q)}; + + mgr.step(z, 0.1); + + double exist_prob3 = mgr.get_tracks().front().existence_probability; + + ASSERT_EQ(mgr.get_tracks().size(), 1); + ASSERT_GT(exist_prob3, exist_prob2); +} + +TEST_F(PoseTrackManagerTests, existence_decrease_on_no_detection) { + auto cfg = make_default_config(); + + Eigen::Quaterniond q = Eigen::Quaterniond::Identity(); + + std::vector z = {make_landmark({0, 0, 0}, q)}; + + PoseTrackManager mgr(cfg); + + mgr.step(z, 0.1); + + double exist_prob1 = mgr.get_tracks().front().existence_probability; + ASSERT_EQ(mgr.get_tracks().size(), 1); + + z = {}; + + mgr.step(z, 0.1); + + const auto& tracks_after = mgr.get_tracks(); + if (tracks_after.empty()) { + SUCCEED(); + } else { + double exist_prob2 = tracks_after.front().existence_probability; + ASSERT_LT(exist_prob2, exist_prob1); + } +} + +} // namespace vortex::filtering