From 9812d1062eab1acd35af94da201054e6b680e3b5 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Sun, 19 Oct 2025 21:52:56 +0200 Subject: [PATCH 01/26] basic package setup --- filtering/ipda_pose_filtering/CMakeLists.txt | 97 +++++++++++++++++++ .../config/ipda_pose_filtering.yaml | 6 ++ .../ros/ipda_pose_filtering_ros.hpp | 47 +++++++++ .../launch/ipda_pose_filtering.launch.py | 25 +++++ filtering/ipda_pose_filtering/package.xml | 26 +++++ .../src/ros/ipda_pose_filtering_ros.cpp | 75 ++++++++++++++ 6 files changed, 276 insertions(+) create mode 100644 filtering/ipda_pose_filtering/CMakeLists.txt create mode 100644 filtering/ipda_pose_filtering/config/ipda_pose_filtering.yaml create mode 100644 filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp create mode 100644 filtering/ipda_pose_filtering/launch/ipda_pose_filtering.launch.py create mode 100644 filtering/ipda_pose_filtering/package.xml create mode 100644 filtering/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros.cpp diff --git a/filtering/ipda_pose_filtering/CMakeLists.txt b/filtering/ipda_pose_filtering/CMakeLists.txt new file mode 100644 index 000000000..d5ef8ee4c --- /dev/null +++ b/filtering/ipda_pose_filtering/CMakeLists.txt @@ -0,0 +1,97 @@ +cmake_minimum_required(VERSION 3.8) +project(ipda_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_filtering REQUIRED) + +include_directories(include) + +set(CORE_LIB_NAME "${PROJECT_NAME}") + +add_library(${CORE_LIB_NAME} SHARED + src/lib/ipda_pose_track_manager.cpp +) + +target_include_directories(${CORE_LIB_NAME} + PUBLIC + $ + $ +) + +ament_target_dependencies(${CORE_LIB_NAME} + vortex_filtering + vortex_utils +) + +set(COMPONENT_LIB_NAME "${PROJECT_NAME}_component") + +add_library(${COMPONENT_LIB_NAME} SHARED + src/ros/${PROJECT_NAME}_ros.cpp +) + +ament_target_dependencies(${COMPONENT_LIB_NAME} + rclcpp + rclcpp_components + geometry_msgs + tf2 + tf2_ros + tf2_geometry_msgs + message_filters + vortex_filtering + vortex_utils +) + +target_link_libraries(${COMPONENT_LIB_NAME} ${PROJECT_NAME}) + +rclcpp_components_register_node( + ${COMPONENT_LIB_NAME} + PLUGIN "IPDAPoseFilteringNode" + 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}) + +ament_package() diff --git a/filtering/ipda_pose_filtering/config/ipda_pose_filtering.yaml b/filtering/ipda_pose_filtering/config/ipda_pose_filtering.yaml new file mode 100644 index 000000000..11a35e5bc --- /dev/null +++ b/filtering/ipda_pose_filtering/config/ipda_pose_filtering.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + pose_sub_topic: "pose" + pose_array_sub_topic: "pose_array" + pose_array_pub_topic: "filtered_pose_array" + target_frame: "odom" diff --git a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp b/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp new file mode 100644 index 000000000..abf121f6e --- /dev/null +++ b/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp @@ -0,0 +1,47 @@ +#ifndef IPDA_POSE_FILTERING_ROS_HPP +#define IPDA_POSE_FILTERING_ROS_HPP + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace vortex::filtering { + +class IPDAPoseFilteringNode : public rclcpp::Node { + public: + IPDAPoseFilteringNode(const rclcpp::NodeOptions& options); + + ~IPDAPoseFilteringNode() {}; + + private: + void setup_publishers_and_subscribers(); + + void pose_callback( + const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg); + + void pose_array_callback( + const geometry_msgs::msg::PoseArray::ConstSharedPtr msg); + + message_filters::Subscriber pose_sub_; + message_filters::Subscriber pose_array_sub_; + std::shared_ptr> + tf2_filter_pose_; + std::shared_ptr> + tf2_filter_pose_array_; + + std::string target_frame_; + std::shared_ptr tf2_buffer_; + std::shared_ptr tf2_listener_; + + rclcpp::Publisher::SharedPtr pose_array_pub_; +}; + +} // namespace vortex::filtering + +#endif // IPDA_POSE_FILTERING_ROS_HPP diff --git a/filtering/ipda_pose_filtering/launch/ipda_pose_filtering.launch.py b/filtering/ipda_pose_filtering/launch/ipda_pose_filtering.launch.py new file mode 100644 index 000000000..f347a1472 --- /dev/null +++ b/filtering/ipda_pose_filtering/launch/ipda_pose_filtering.launch.py @@ -0,0 +1,25 @@ +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('ipda_pose_filtering'), + 'config', + 'ipda_pose_filtering.yaml', + ) + + return LaunchDescription( + [ + Node( + package='ipda_pose_filtering', + executable='ipda_pose_filtering_node', + name='ipda_pose_filtering', + output='screen', + parameters=[config, {'use_sim_time': False}], + ), + ] + ) diff --git a/filtering/ipda_pose_filtering/package.xml b/filtering/ipda_pose_filtering/package.xml new file mode 100644 index 000000000..637ed099c --- /dev/null +++ b/filtering/ipda_pose_filtering/package.xml @@ -0,0 +1,26 @@ + + + + ipda_pose_filtering + 0.0.0 + IPDA 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_filtering + + + ament_cmake + + diff --git a/filtering/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros.cpp b/filtering/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros.cpp new file mode 100644 index 000000000..21185ed32 --- /dev/null +++ b/filtering/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros.cpp @@ -0,0 +1,75 @@ +#include "ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp" +#include +#include + +using std::placeholders::_1; +using std::placeholders::_2; + +namespace vortex::filtering { + +IPDAPoseFilteringNode::IPDAPoseFilteringNode(const rclcpp::NodeOptions& options) + : rclcpp::Node("ipda_pose_filtering_node", options) { + setup_publishers_and_subscribers(); +} + +void IPDAPoseFilteringNode::setup_publishers_and_subscribers() { + 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"); + + pose_array_pub_ = this->create_publisher( + pub_topic_name, qos_sensor_data_pub); + + target_frame_ = this->declare_parameter("target_frame"); + + std::chrono::duration buffer_timeout(1); + + 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_); + + std::string pose_sub_topic = + this->declare_parameter("pose_sub_topic"); + std::string pose_array_sub_topic = + this->declare_parameter("pose_array_sub_topic"); + + const auto qos_sensor_data_sub{ + vortex::utils::qos_profiles::sensor_data_profile(10)}; + + pose_sub_.subscribe(this, pose_sub_topic, + qos_sensor_data_sub.get_rmw_qos_profile()); + pose_array_sub_.subscribe(this, pose_array_sub_topic, + qos_sensor_data_sub.get_rmw_qos_profile()); + + tf2_filter_pose_ = std::make_shared< + tf2_ros::MessageFilter>( + pose_sub_, *tf2_buffer_, target_frame_, 100, + this->get_node_logging_interface(), this->get_node_clock_interface()); + + tf2_filter_pose_array_ = + std::make_shared>( + pose_array_sub_, *tf2_buffer_, target_frame_, 100, + this->get_node_logging_interface(), + this->get_node_clock_interface()); + + tf2_filter_pose_->registerCallback( + std::bind(&IPDAPoseFilteringNode::pose_callback, this, _1)); + + tf2_filter_pose_array_->registerCallback( + std::bind(&IPDAPoseFilteringNode::pose_array_callback, this, _1)); +} + +void IPDAPoseFilteringNode::pose_callback( + const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg) {} + +void IPDAPoseFilteringNode::pose_array_callback( + const geometry_msgs::msg::PoseArray::ConstSharedPtr msg) {} + +RCLCPP_COMPONENTS_REGISTER_NODE(IPDAPoseFilteringNode); + +} // namespace vortex::filtering From e4e0639d37d92135a7f8da34138c4d35fe52fd5d Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Sun, 19 Oct 2025 23:09:23 +0200 Subject: [PATCH 02/26] removed 'lib' from gitignore --- .gitignore | 1 - .../ipda_pose_filtering/src/lib/ipda_pose_track_manager.cpp | 0 2 files changed, 1 deletion(-) create mode 100644 filtering/ipda_pose_filtering/src/lib/ipda_pose_track_manager.cpp diff --git a/.gitignore b/.gitignore index f86dbd841..2a085ac8d 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/ipda_pose_filtering/src/lib/ipda_pose_track_manager.cpp b/filtering/ipda_pose_filtering/src/lib/ipda_pose_track_manager.cpp new file mode 100644 index 000000000..e69de29bb From acaba30d5235c34a74a49651417c9e5b504145b9 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Wed, 22 Oct 2025 23:27:15 +0200 Subject: [PATCH 03/26] generic pose sub setup --- .../config/ipda_pose_filtering.yaml | 2 +- .../ros/ipda_pose_filtering_ros.hpp | 31 +++++--- .../src/ros/ipda_pose_filtering_ros.cpp | 77 +++++++++++++------ 3 files changed, 75 insertions(+), 35 deletions(-) diff --git a/filtering/ipda_pose_filtering/config/ipda_pose_filtering.yaml b/filtering/ipda_pose_filtering/config/ipda_pose_filtering.yaml index 11a35e5bc..4c813de09 100644 --- a/filtering/ipda_pose_filtering/config/ipda_pose_filtering.yaml +++ b/filtering/ipda_pose_filtering/config/ipda_pose_filtering.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: + pose_message_type: "PoseStamped" pose_sub_topic: "pose" - pose_array_sub_topic: "pose_array" pose_array_pub_topic: "filtered_pose_array" target_frame: "odom" diff --git a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp b/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp index abf121f6e..d7e52456e 100644 --- a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp +++ b/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp @@ -6,8 +6,10 @@ #include #include #include +#include #include #include +#include #include #include @@ -22,18 +24,27 @@ class IPDAPoseFilteringNode : public rclcpp::Node { private: void setup_publishers_and_subscribers(); - void pose_callback( - const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg); + template + void create_pose_subscription( + const std::string& topic_name, const rmw_qos_profile_t& qos_profile); - void pose_array_callback( - const geometry_msgs::msg::PoseArray::ConstSharedPtr msg); + template + void pose_callback(const typename MsgT::ConstSharedPtr& msg); - message_filters::Subscriber pose_sub_; - message_filters::Subscriber pose_array_sub_; - std::shared_ptr> - tf2_filter_pose_; - std::shared_ptr> - tf2_filter_pose_array_; + std::variant< + std::shared_ptr>, + std::shared_ptr>, + std::shared_ptr> + > subscriber_; + + std::variant< + std::shared_ptr>, + std::shared_ptr>, + std::shared_ptr> + > tf_filter_; + + rclcpp::SubscriptionBase::SharedPtr sub_; + std::shared_ptr tf2_filter_; std::string target_frame_; std::shared_ptr tf2_buffer_; diff --git a/filtering/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros.cpp b/filtering/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros.cpp index 21185ed32..12fa32b05 100644 --- a/filtering/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros.cpp +++ b/filtering/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros.cpp @@ -1,6 +1,9 @@ #include "ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp" #include +#include "vortex/utils/ros_conversions.hpp" #include +#include +#include using std::placeholders::_1; using std::placeholders::_2; @@ -33,42 +36,68 @@ void IPDAPoseFilteringNode::setup_publishers_and_subscribers() { tf2_buffer_->setCreateTimerInterface(timer_interface); tf2_listener_ = std::make_shared(*tf2_buffer_); - std::string pose_sub_topic = + const std::string msg_type = + this->declare_parameter("pose_message_type"); + const std::string pose_sub_topic = this->declare_parameter("pose_sub_topic"); - std::string pose_array_sub_topic = - this->declare_parameter("pose_array_sub_topic"); const auto qos_sensor_data_sub{ vortex::utils::qos_profiles::sensor_data_profile(10)}; - pose_sub_.subscribe(this, pose_sub_topic, - qos_sensor_data_sub.get_rmw_qos_profile()); - pose_array_sub_.subscribe(this, pose_array_sub_topic, - qos_sensor_data_sub.get_rmw_qos_profile()); + if (msg_type == "PoseStamped") { + create_pose_subscription( + pose_sub_topic, qos_sensor_data_sub.get_rmw_qos_profile()); + } + else if (msg_type == "PoseArray") { + create_pose_subscription( + pose_sub_topic, qos_sensor_data_sub.get_rmw_qos_profile()); + } + else if (msg_type == "PoseWithCovarianceStamped") { + create_pose_subscription( + pose_sub_topic, qos_sensor_data_sub.get_rmw_qos_profile()); + } + else { + spdlog::error( + "IPDAPoseFilteringNode: Unsupported pose message type: {} \n" + "Supported types are: Pose, PoseStamped, PoseArray, PoseWithCovarianceStamped", + msg_type); + } +} + +template +void IPDAPoseFilteringNode::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); - tf2_filter_pose_ = std::make_shared< - tf2_ros::MessageFilter>( - pose_sub_, *tf2_buffer_, target_frame_, 100, - this->get_node_logging_interface(), this->get_node_clock_interface()); + auto filter = std::make_shared>( + *sub, *tf2_buffer_, target_frame_, 100, + this->get_node_logging_interface(), + this->get_node_clock_interface()); - tf2_filter_pose_array_ = - std::make_shared>( - pose_array_sub_, *tf2_buffer_, target_frame_, 100, - this->get_node_logging_interface(), - this->get_node_clock_interface()); + filter->registerCallback([this](const typename MsgT::ConstSharedPtr msg) { + this->pose_callback(msg); + }); - tf2_filter_pose_->registerCallback( - std::bind(&IPDAPoseFilteringNode::pose_callback, this, _1)); + subscriber_ = sub; + tf_filter_ = filter; +} - tf2_filter_pose_array_->registerCallback( - std::bind(&IPDAPoseFilteringNode::pose_array_callback, this, _1)); +template +void IPDAPoseFilteringNode::pose_callback(const typename MsgT::ConstSharedPtr& msg) { + Eigen::Matrix measurements = + vortex::utils::ros_conversions::ros_to_eigen6d(*msg); } -void IPDAPoseFilteringNode::pose_callback( - const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg) {} -void IPDAPoseFilteringNode::pose_array_callback( - const geometry_msgs::msg::PoseArray::ConstSharedPtr msg) {} +template void IPDAPoseFilteringNode::pose_callback( + const geometry_msgs::msg::PoseStamped::ConstSharedPtr& msg); + +template void IPDAPoseFilteringNode::pose_callback( + const geometry_msgs::msg::PoseArray::ConstSharedPtr& msg); + +template void IPDAPoseFilteringNode::pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr& msg); RCLCPP_COMPONENTS_REGISTER_NODE(IPDAPoseFilteringNode); From 72e821196d0d671371b4de579929194e2ede2883 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Wed, 22 Oct 2025 23:33:10 +0200 Subject: [PATCH 04/26] removed unused member in favor of variant --- .../ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp b/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp index d7e52456e..c46b68312 100644 --- a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp +++ b/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp @@ -43,9 +43,6 @@ class IPDAPoseFilteringNode : public rclcpp::Node { std::shared_ptr> > tf_filter_; - rclcpp::SubscriptionBase::SharedPtr sub_; - std::shared_ptr tf2_filter_; - std::string target_frame_; std::shared_ptr tf2_buffer_; std::shared_ptr tf2_listener_; From 8a117c897c41d7395efc9a31d78b2d263a696831 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Sun, 26 Oct 2025 18:13:53 +0100 Subject: [PATCH 05/26] config setup --- .../config/ipda_pose_filtering.yaml | 27 ++++++ .../lib/ipda_pose_track_manager.hpp | 32 +++++++ .../ipda_pose_filtering/lib/typedefs.hpp | 63 ++++++++++++++ .../ros/ipda_pose_filtering_ros.hpp | 33 +++++-- .../src/lib/ipda_pose_track_manager.cpp | 18 ++++ .../src/ros/ipda_pose_filtering_ros.cpp | 85 +++++++++++++++---- 6 files changed, 231 insertions(+), 27 deletions(-) create mode 100644 filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/ipda_pose_track_manager.hpp create mode 100644 filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/typedefs.hpp diff --git a/filtering/ipda_pose_filtering/config/ipda_pose_filtering.yaml b/filtering/ipda_pose_filtering/config/ipda_pose_filtering.yaml index 4c813de09..380ed0009 100644 --- a/filtering/ipda_pose_filtering/config/ipda_pose_filtering.yaml +++ b/filtering/ipda_pose_filtering/config/ipda_pose_filtering.yaml @@ -4,3 +4,30 @@ pose_sub_topic: "pose" pose_array_pub_topic: "filtered_pose_array" target_frame: "odom" + update_interval_ms: 100 + + # IPDA config + ipda: + prob_of_survival: 0.99 + prob_of_detection: 0.7 + 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.001 + + # Dyn model config + dyn_mod: + std_pos: 0.2 + std_orient: 0.2 + + # Sensor model config + sensor_mod: + std_pos: 0.1 + std_orient: 0.1 + max_angle_gate_threshold: 0.4 # in radians + + # Existence management config + existence: + confirmation_threshold: 0.6 + deletion_threshold: 0.2 + initial_existence_probability: 0.4 diff --git a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/ipda_pose_track_manager.hpp b/filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/ipda_pose_track_manager.hpp new file mode 100644 index 000000000..b7272234d --- /dev/null +++ b/filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/ipda_pose_track_manager.hpp @@ -0,0 +1,32 @@ +#ifndef IPDA_POSE_TRACK_MANAGER_HPP +#define IPDA_POSE_TRACK_MANAGER_HPP + +#include +#include +#include +#include "typedefs.hpp" + +namespace vortex::filtering { + +class IPDAPoseTrackManager { + public: + IPDAPoseTrackManager(const TrackManagerConfig& config); + + void predict_tracks(const Measurements& measurements, double dt); + + private: + int track_id_counter_; + + std::vector tracks_; + + DynMod dyn_mod_; + SensorMod sensor_mod_; + + vortex::filter::IPDA::Config ipda_config_; + + ExistenceManagementConfig existence_config_; +}; + +} // namespace vortex::filtering + +#endif // IPDA_POSE_TRACK_MANAGER_HPP diff --git a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/typedefs.hpp b/filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/typedefs.hpp new file mode 100644 index 000000000..15297b892 --- /dev/null +++ b/filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/typedefs.hpp @@ -0,0 +1,63 @@ +#ifndef TYPEDEFS_HPP +#define TYPEDEFS_HPP + +#include +#include +#include + +namespace vortex::filtering { + +using Measurements = Eigen::Matrix; +using State6d = vortex::prob::Gauss6d; +using DynMod = vortex::models::ConstantPose; +using SensorMod = vortex::models::IdentityPoseSensor<6, 6>; +using IPDA = vortex::filter::IPDA; + +struct DynModConfig { + double std_pos; + double std_orient; +}; + +struct SensorModConfig { + double std_pos; + double std_orient; + double max_angle_gate_threshold; +}; + +struct ExistenceManagementConfig { + double confirmation_threshold; + double deletion_threshold; + double initial_existence_probability; +}; + +struct TrackManagerConfig { + vortex::filter::IPDA::Config ipda; + DynModConfig dyn_mod; + SensorModConfig sensor_mod; + ExistenceManagementConfig existence; +}; + +struct Track { + int id; + State6d state; + double existence_probability; + bool confirmed; + + // For sorting tracks based on existence probability and confirmed track + bool operator<(const Track& other) const { + if (confirmed != other.confirmed) { + return confirmed > other.confirmed; + } else { + return existence_probability > other.existence_probability; + } + } + + bool operator==(const Track& other) const { + return confirmed == other.confirmed && + existence_probability == other.existence_probability; + } +}; + +} // namespace vortex::filtering + +#endif // TYPEDEFS_HPP diff --git a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp b/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp index c46b68312..e187dd1cc 100644 --- a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp +++ b/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp @@ -13,6 +13,8 @@ #include #include +#include "ipda_pose_filtering/lib/ipda_pose_track_manager.hpp" + namespace vortex::filtering { class IPDAPoseFilteringNode : public rclcpp::Node { @@ -24,30 +26,43 @@ class IPDAPoseFilteringNode : public rclcpp::Node { private: void setup_publishers_and_subscribers(); + void setup_track_manager(); + template - void create_pose_subscription( - const std::string& topic_name, const rmw_qos_profile_t& qos_profile); + void create_pose_subscription(const std::string& topic_name, + const rmw_qos_profile_t& qos_profile); template void pose_callback(const typename MsgT::ConstSharedPtr& msg); + void timer_callback(); + std::variant< - std::shared_ptr>, - std::shared_ptr>, - std::shared_ptr> - > subscriber_; + std::shared_ptr< + message_filters::Subscriber>, + std::shared_ptr< + message_filters::Subscriber>, + std::shared_ptr>> + subscriber_; std::variant< - std::shared_ptr>, + std::shared_ptr< + tf2_ros::MessageFilter>, std::shared_ptr>, - std::shared_ptr> - > tf_filter_; + 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::TimerBase::SharedPtr pub_timer_; + + std::unique_ptr track_manager_; }; } // namespace vortex::filtering diff --git a/filtering/ipda_pose_filtering/src/lib/ipda_pose_track_manager.cpp b/filtering/ipda_pose_filtering/src/lib/ipda_pose_track_manager.cpp index e69de29bb..f32fe85b9 100644 --- a/filtering/ipda_pose_filtering/src/lib/ipda_pose_track_manager.cpp +++ b/filtering/ipda_pose_filtering/src/lib/ipda_pose_track_manager.cpp @@ -0,0 +1,18 @@ +#include "ipda_pose_filtering/lib/ipda_pose_track_manager.hpp" + +namespace vortex::filtering { + +IPDAPoseTrackManager::IPDAPoseTrackManager(const TrackManagerConfig& config) + : track_id_counter_(0), + dyn_mod_(config.dyn_mod.std_pos, config.dyn_mod.std_orient), + sensor_mod_(config.sensor_mod.std_pos, + config.sensor_mod.std_orient, + config.sensor_mod.max_angle_gate_threshold) { + ipda_config_ = config.ipda; + existence_config_ = config.existence; +} + +void IPDAPoseTrackManager::predict_tracks(const Measurements& measurements, + double dt) {} + +} // namespace vortex::filtering diff --git a/filtering/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros.cpp b/filtering/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros.cpp index 12fa32b05..4d5a71d65 100644 --- a/filtering/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros.cpp +++ b/filtering/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros.cpp @@ -1,9 +1,9 @@ #include "ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp" +#include #include -#include "vortex/utils/ros_conversions.hpp" -#include #include -#include +#include +#include "vortex/utils/ros_conversions.hpp" using std::placeholders::_1; using std::placeholders::_2; @@ -13,6 +13,7 @@ namespace vortex::filtering { IPDAPoseFilteringNode::IPDAPoseFilteringNode(const rclcpp::NodeOptions& options) : rclcpp::Node("ipda_pose_filtering_node", options) { setup_publishers_and_subscribers(); + setup_track_manager(); } void IPDAPoseFilteringNode::setup_publishers_and_subscribers() { @@ -24,6 +25,13 @@ void IPDAPoseFilteringNode::setup_publishers_and_subscribers() { pose_array_pub_ = this->create_publisher( pub_topic_name, qos_sensor_data_pub); + int publish_interval_ms = + this->declare_parameter("publish_interval_ms"); + + pub_timer_ = this->create_wall_timer( + std::chrono::milliseconds(publish_interval_ms), + std::bind(&IPDAPoseFilteringNode::timer_callback, this)); + target_frame_ = this->declare_parameter("target_frame"); std::chrono::duration buffer_timeout(1); @@ -47,33 +55,69 @@ void IPDAPoseFilteringNode::setup_publishers_and_subscribers() { if (msg_type == "PoseStamped") { create_pose_subscription( pose_sub_topic, qos_sensor_data_sub.get_rmw_qos_profile()); - } - else if (msg_type == "PoseArray") { + } else if (msg_type == "PoseArray") { create_pose_subscription( pose_sub_topic, qos_sensor_data_sub.get_rmw_qos_profile()); - } - else if (msg_type == "PoseWithCovarianceStamped") { + } else if (msg_type == "PoseWithCovarianceStamped") { create_pose_subscription( pose_sub_topic, qos_sensor_data_sub.get_rmw_qos_profile()); - } - else { + } else { spdlog::error( "IPDAPoseFilteringNode: Unsupported pose message type: {} \n" - "Supported types are: Pose, PoseStamped, PoseArray, PoseWithCovarianceStamped", + "Supported types are: Pose, PoseStamped, PoseArray, " + "PoseWithCovarianceStamped", msg_type); } } +void IPDAPoseFilteringNode::setup_track_manager() { + TrackManagerConfig config; + + config.ipda.ipda.prob_of_survival = + this->declare_parameter("ipda.prob_of_survival"); + config.ipda.pdaf.prob_of_detection = + this->declare_parameter("ipda.prob_of_detection"); + config.ipda.pdaf.mahalanobis_threshold = + this->declare_parameter("ipda.mahalanobis_gate_threshold"); + config.ipda.pdaf.min_gate_threshold = + this->declare_parameter("ipda.min_gate_threshold"); + config.ipda.pdaf.max_gate_threshold = + this->declare_parameter("ipda.max_gate_threshold"); + config.ipda.pdaf.clutter_intensity = + this->declare_parameter("ipda.clutter_intensity"); + + config.dyn_mod.std_pos = this->declare_parameter("dyn_mod.std_pos"); + config.dyn_mod.std_orient = + this->declare_parameter("dyn_mod.std_orient"); + + config.sensor_mod.std_pos = + this->declare_parameter("sensor_mod.std_pos"); + config.sensor_mod.std_orient = + this->declare_parameter("sensor_mod.std_orient"); + config.sensor_mod.max_angle_gate_threshold = + this->declare_parameter("sensor_mod.max_angle_gate_threshold"); + + config.existence.confirmation_threshold = + this->declare_parameter("existence.confirmation_threshold"); + config.existence.deletion_threshold = + this->declare_parameter("existence.deletion_threshold"); + config.existence.initial_existence_probability = + this->declare_parameter( + "existence.initial_existence_probability"); + + track_manager_ = std::make_unique(config); +} + template void IPDAPoseFilteringNode::create_pose_subscription( - const std::string& topic_name, const rmw_qos_profile_t& qos_profile) { + 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_, 100, - this->get_node_logging_interface(), - this->get_node_clock_interface()); + this->get_node_logging_interface(), this->get_node_clock_interface()); filter->registerCallback([this](const typename MsgT::ConstSharedPtr msg) { this->pose_callback(msg); @@ -84,19 +128,24 @@ void IPDAPoseFilteringNode::create_pose_subscription( } template -void IPDAPoseFilteringNode::pose_callback(const typename MsgT::ConstSharedPtr& msg) { +void IPDAPoseFilteringNode::pose_callback( + const typename MsgT::ConstSharedPtr& msg) { Eigen::Matrix measurements = vortex::utils::ros_conversions::ros_to_eigen6d(*msg); -} +}; +void IPDAPoseFilteringNode::timer_callback() {} -template void IPDAPoseFilteringNode::pose_callback( +template void +IPDAPoseFilteringNode::pose_callback( const geometry_msgs::msg::PoseStamped::ConstSharedPtr& msg); -template void IPDAPoseFilteringNode::pose_callback( +template void +IPDAPoseFilteringNode::pose_callback( const geometry_msgs::msg::PoseArray::ConstSharedPtr& msg); -template void IPDAPoseFilteringNode::pose_callback( +template void IPDAPoseFilteringNode::pose_callback< + geometry_msgs::msg::PoseWithCovarianceStamped>( const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr& msg); RCLCPP_COMPONENTS_REGISTER_NODE(IPDAPoseFilteringNode); From 5d7b9b7a2dd93847d0de18ba061ac0781362ee9b Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Sun, 26 Oct 2025 23:32:57 +0100 Subject: [PATCH 06/26] track manager impl --- .../config/ipda_pose_filtering.yaml | 3 +- .../lib/ipda_pose_track_manager.hpp | 17 +++-- .../ipda_pose_filtering/lib/typedefs.hpp | 20 ++---- .../ros/ipda_pose_filtering_ros.hpp | 16 ++--- .../src/lib/ipda_pose_track_manager.cpp | 62 ++++++++++++++++++- .../src/ros/ipda_pose_filtering_ros.cpp | 30 +++++++-- 6 files changed, 111 insertions(+), 37 deletions(-) diff --git a/filtering/ipda_pose_filtering/config/ipda_pose_filtering.yaml b/filtering/ipda_pose_filtering/config/ipda_pose_filtering.yaml index 380ed0009..845bc435b 100644 --- a/filtering/ipda_pose_filtering/config/ipda_pose_filtering.yaml +++ b/filtering/ipda_pose_filtering/config/ipda_pose_filtering.yaml @@ -4,11 +4,12 @@ pose_sub_topic: "pose" pose_array_pub_topic: "filtered_pose_array" target_frame: "odom" - update_interval_ms: 100 + publish_timer: 100 # IPDA config ipda: prob_of_survival: 0.99 + estimate_clutter: false prob_of_detection: 0.7 mahalanobis_gate_threshold: 2.5 # in std deviations min_gate_threshold: 1.0 # in meters diff --git a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/ipda_pose_track_manager.hpp b/filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/ipda_pose_track_manager.hpp index b7272234d..88de1c805 100644 --- a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/ipda_pose_track_manager.hpp +++ b/filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/ipda_pose_track_manager.hpp @@ -9,17 +9,26 @@ namespace vortex::filtering { class IPDAPoseTrackManager { - public: + public: IPDAPoseTrackManager(const TrackManagerConfig& config); - void predict_tracks(const Measurements& measurements, double dt); + void predict_tracks(); - private: - int track_id_counter_; + void measurement_update(Measurements& measurements, double dt); + + const std::vector& get_tracks() { + return tracks_; + } + + private: + void create_tracks(const Measurements& measurements); + + int track_id_counter_ = 0; std::vector tracks_; DynMod dyn_mod_; + SensorMod sensor_mod_; vortex::filter::IPDA::Config ipda_config_; diff --git a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/typedefs.hpp b/filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/typedefs.hpp index 15297b892..a5733c0b5 100644 --- a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/typedefs.hpp +++ b/filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/typedefs.hpp @@ -5,6 +5,12 @@ #include #include +namespace Eigen { + +using Vector6d = Eigen::Matrix; + +} // namespace eigen + namespace vortex::filtering { using Measurements = Eigen::Matrix; @@ -42,20 +48,6 @@ struct Track { State6d state; double existence_probability; bool confirmed; - - // For sorting tracks based on existence probability and confirmed track - bool operator<(const Track& other) const { - if (confirmed != other.confirmed) { - return confirmed > other.confirmed; - } else { - return existence_probability > other.existence_probability; - } - } - - bool operator==(const Track& other) const { - return confirmed == other.confirmed && - existence_probability == other.existence_probability; - } }; } // namespace vortex::filtering diff --git a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp b/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp index e187dd1cc..85df9f831 100644 --- a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp +++ b/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp @@ -38,23 +38,19 @@ class IPDAPoseFilteringNode : public rclcpp::Node { void timer_callback(); std::variant< - std::shared_ptr< - message_filters::Subscriber>, - std::shared_ptr< - message_filters::Subscriber>, - std::shared_ptr>> + std::shared_ptr>, + std::shared_ptr>, + std::shared_ptr>> subscriber_; std::variant< - std::shared_ptr< - tf2_ros::MessageFilter>, + std::shared_ptr>, std::shared_ptr>, - std::shared_ptr>> + std::shared_ptr>> tf_filter_; std::string target_frame_; + rclcpp::Time prev_meas_stamp_{0, 0, RCL_ROS_TIME}; std::shared_ptr tf2_buffer_; std::shared_ptr tf2_listener_; diff --git a/filtering/ipda_pose_filtering/src/lib/ipda_pose_track_manager.cpp b/filtering/ipda_pose_filtering/src/lib/ipda_pose_track_manager.cpp index f32fe85b9..2f4af92c2 100644 --- a/filtering/ipda_pose_filtering/src/lib/ipda_pose_track_manager.cpp +++ b/filtering/ipda_pose_filtering/src/lib/ipda_pose_track_manager.cpp @@ -1,4 +1,6 @@ #include "ipda_pose_filtering/lib/ipda_pose_track_manager.hpp" +#include +#include namespace vortex::filtering { @@ -12,7 +14,63 @@ IPDAPoseTrackManager::IPDAPoseTrackManager(const TrackManagerConfig& config) existence_config_ = config.existence; } -void IPDAPoseTrackManager::predict_tracks(const Measurements& measurements, - double dt) {} +void IPDAPoseTrackManager::predict_tracks() { + auto new_end = std::ranges::remove_if(tracks_, [this](Track& track) { + track.existence_probability = IPDA::existence_prediction( + track.existence_probability, ipda_config_.ipda.prob_of_survival); + + return track.existence_probability < existence_config_.deletion_threshold; + }); + tracks_.erase(new_end.begin(), new_end.end()); +} + +void IPDAPoseTrackManager::measurement_update(Measurements& measurements, double dt){ + 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; + }); + + for (Track &track : tracks_){ + const IPDA::State state_est_prev{ + .x_estimate = track.state, + .existence_probability = track.existence_probability + }; + + auto output = IPDA::step(dyn_mod_, sensor_mod_, dt, + state_est_prev, measurements, ipda_config_); + + track.state = output.state.x_estimate; + track.existence_probability = output.state.existence_probability; + + std::vector outside_cols; + outside_cols.reserve(measurements.cols()); + for (Eigen::Index i = 0; i < measurements.cols(); ++i) + if (!output.gated_measurements[i]) + outside_cols.push_back(measurements.col(i)); + + if (!outside_cols.empty()) { + Measurements outside(6, static_cast(outside_cols.size())); + for (Eigen::Index i = 0; i < outside.cols(); ++i) + outside.col(i) = outside_cols[i]; + measurements = std::move(outside); + } + } + create_tracks(measurements); +} + + +void IPDAPoseTrackManager::create_tracks(const Measurements& measurements) { + tracks_.reserve(tracks_.size() + measurements.cols()); + for(auto i : std::views::iota(Eigen::Index{0}, measurements.cols())) { + tracks_.emplace_back( + Track{ + .id = track_id_counter_++, + .state = vortex::prob::Gauss6d(measurements.col(i), Eigen::Matrix::Identity()), + .existence_probability = existence_config_.initial_existence_probability, + .confirmed = false + }); + } +} } // namespace vortex::filtering diff --git a/filtering/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros.cpp b/filtering/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros.cpp index 4d5a71d65..03e306422 100644 --- a/filtering/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros.cpp +++ b/filtering/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros.cpp @@ -25,11 +25,11 @@ void IPDAPoseFilteringNode::setup_publishers_and_subscribers() { pose_array_pub_ = this->create_publisher( pub_topic_name, qos_sensor_data_pub); - int publish_interval_ms = - this->declare_parameter("publish_interval_ms"); + int publish_timer = + this->declare_parameter("publish_timer"); pub_timer_ = this->create_wall_timer( - std::chrono::milliseconds(publish_interval_ms), + std::chrono::milliseconds(publish_timer), std::bind(&IPDAPoseFilteringNode::timer_callback, this)); target_frame_ = this->declare_parameter("target_frame"); @@ -75,6 +75,8 @@ void IPDAPoseFilteringNode::setup_track_manager() { config.ipda.ipda.prob_of_survival = this->declare_parameter("ipda.prob_of_survival"); + config.ipda.ipda.estimate_clutter = + this->declare_parameter("ipda.estimate_clutter"); config.ipda.pdaf.prob_of_detection = this->declare_parameter("ipda.prob_of_detection"); config.ipda.pdaf.mahalanobis_threshold = @@ -130,11 +132,27 @@ void IPDAPoseFilteringNode::create_pose_subscription( template void IPDAPoseFilteringNode::pose_callback( const typename MsgT::ConstSharedPtr& msg) { - Eigen::Matrix measurements = - vortex::utils::ros_conversions::ros_to_eigen6d(*msg); + Measurements measurements = vortex::utils::ros_conversions::ros_to_eigen6d(*msg); + rclcpp::Time current_time(msg->header.stamp); + double dt = (current_time - prev_meas_stamp_).seconds(); + prev_meas_stamp_ = current_time; + track_manager_->measurement_update(measurements, dt); }; -void IPDAPoseFilteringNode::timer_callback() {} +void IPDAPoseFilteringNode::timer_callback() { + + track_manager_->predict_tracks(); + + geometry_msgs::msg::PoseArray pose_array; + pose_array.header.frame_id = target_frame_; + pose_array.header.stamp = prev_meas_stamp_; + for (auto track : track_manager_->get_tracks()) { + if (!track.confirmed) continue; + pose_array.poses.push_back( + vortex::utils::ros_conversions::eigen6d_to_pose(track.state.mean())); + } + pose_array_pub_->publish(pose_array); +} template void IPDAPoseFilteringNode::pose_callback( From 4ceeeb9665784a24a64f430b96a4ea574fdf9818 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Wed, 10 Dec 2025 21:23:28 +0100 Subject: [PATCH 07/26] ValidPoseMsg concept --- .../lib/ipda_pose_track_manager.hpp | 8 +- .../ipda_pose_filtering/lib/typedefs.hpp | 2 +- .../ros/ipda_pose_filtering_ros.hpp | 28 +++++-- .../src/lib/ipda_pose_track_manager.cpp | 73 ++++++++++--------- .../src/ros/ipda_pose_filtering_ros.cpp | 19 ++--- 5 files changed, 72 insertions(+), 58 deletions(-) diff --git a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/ipda_pose_track_manager.hpp b/filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/ipda_pose_track_manager.hpp index 88de1c805..22ed0c0b1 100644 --- a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/ipda_pose_track_manager.hpp +++ b/filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/ipda_pose_track_manager.hpp @@ -9,18 +9,16 @@ namespace vortex::filtering { class IPDAPoseTrackManager { - public: + public: IPDAPoseTrackManager(const TrackManagerConfig& config); void predict_tracks(); void measurement_update(Measurements& measurements, double dt); - const std::vector& get_tracks() { - return tracks_; - } + const std::vector& get_tracks() { return tracks_; } - private: + private: void create_tracks(const Measurements& measurements); int track_id_counter_ = 0; diff --git a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/typedefs.hpp b/filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/typedefs.hpp index a5733c0b5..c259e1fe8 100644 --- a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/typedefs.hpp +++ b/filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/typedefs.hpp @@ -9,7 +9,7 @@ namespace Eigen { using Vector6d = Eigen::Matrix; -} // namespace eigen +} // namespace Eigen namespace vortex::filtering { diff --git a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp b/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp index 85df9f831..548dfcf61 100644 --- a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp +++ b/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp @@ -15,8 +15,17 @@ #include "ipda_pose_filtering/lib/ipda_pose_track_manager.hpp" +#include +#include + namespace vortex::filtering { +template +concept ValidPoseMsg = + std::same_as || + std::same_as || + std::same_as; + class IPDAPoseFilteringNode : public rclcpp::Node { public: IPDAPoseFilteringNode(const rclcpp::NodeOptions& options); @@ -28,25 +37,30 @@ class IPDAPoseFilteringNode : public rclcpp::Node { void setup_track_manager(); - template + template void create_pose_subscription(const std::string& topic_name, const rmw_qos_profile_t& qos_profile); - template + template void pose_callback(const typename MsgT::ConstSharedPtr& msg); void timer_callback(); std::variant< - std::shared_ptr>, - std::shared_ptr>, - std::shared_ptr>> + std::shared_ptr< + message_filters::Subscriber>, + std::shared_ptr< + message_filters::Subscriber>, + std::shared_ptr>> subscriber_; std::variant< - std::shared_ptr>, + std::shared_ptr< + tf2_ros::MessageFilter>, std::shared_ptr>, - std::shared_ptr>> + std::shared_ptr>> tf_filter_; std::string target_frame_; diff --git a/filtering/ipda_pose_filtering/src/lib/ipda_pose_track_manager.cpp b/filtering/ipda_pose_filtering/src/lib/ipda_pose_track_manager.cpp index 2f4af92c2..955b0e68b 100644 --- a/filtering/ipda_pose_filtering/src/lib/ipda_pose_track_manager.cpp +++ b/filtering/ipda_pose_filtering/src/lib/ipda_pose_track_manager.cpp @@ -16,61 +16,62 @@ IPDAPoseTrackManager::IPDAPoseTrackManager(const TrackManagerConfig& config) void IPDAPoseTrackManager::predict_tracks() { auto new_end = std::ranges::remove_if(tracks_, [this](Track& track) { - track.existence_probability = IPDA::existence_prediction( - track.existence_probability, ipda_config_.ipda.prob_of_survival); + track.existence_probability = IPDA::existence_prediction( + track.existence_probability, ipda_config_.ipda.prob_of_survival); - return track.existence_probability < existence_config_.deletion_threshold; + return track.existence_probability < + existence_config_.deletion_threshold; }); tracks_.erase(new_end.begin(), new_end.end()); } -void IPDAPoseTrackManager::measurement_update(Measurements& measurements, double dt){ - std::ranges::sort(tracks_, [](const Track& a, const Track& b) { +void IPDAPoseTrackManager::measurement_update(Measurements& measurements, + double dt) { + 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; }); - for (Track &track : tracks_){ - const IPDA::State state_est_prev{ - .x_estimate = track.state, - .existence_probability = track.existence_probability - }; + for (Track& track : tracks_) { + const IPDA::State state_est_prev{ + .x_estimate = track.state, + .existence_probability = track.existence_probability}; - auto output = IPDA::step(dyn_mod_, sensor_mod_, dt, - state_est_prev, measurements, ipda_config_); + auto output = IPDA::step(dyn_mod_, sensor_mod_, dt, state_est_prev, + measurements, ipda_config_); - track.state = output.state.x_estimate; - track.existence_probability = output.state.existence_probability; + track.state = output.state.x_estimate; + track.existence_probability = output.state.existence_probability; - std::vector outside_cols; - outside_cols.reserve(measurements.cols()); - for (Eigen::Index i = 0; i < measurements.cols(); ++i) - if (!output.gated_measurements[i]) - outside_cols.push_back(measurements.col(i)); + std::vector outside_cols; + outside_cols.reserve(measurements.cols()); + for (Eigen::Index i = 0; i < measurements.cols(); ++i) + if (!output.gated_measurements[i]) + outside_cols.push_back(measurements.col(i)); - if (!outside_cols.empty()) { - Measurements outside(6, static_cast(outside_cols.size())); - for (Eigen::Index i = 0; i < outside.cols(); ++i) - outside.col(i) = outside_cols[i]; - measurements = std::move(outside); - } + if (!outside_cols.empty()) { + Measurements outside( + 6, static_cast(outside_cols.size())); + for (Eigen::Index i = 0; i < outside.cols(); ++i) + outside.col(i) = outside_cols[i]; + measurements = std::move(outside); + } } create_tracks(measurements); } - void IPDAPoseTrackManager::create_tracks(const Measurements& measurements) { - tracks_.reserve(tracks_.size() + measurements.cols()); - for(auto i : std::views::iota(Eigen::Index{0}, measurements.cols())) { - tracks_.emplace_back( - Track{ - .id = track_id_counter_++, - .state = vortex::prob::Gauss6d(measurements.col(i), Eigen::Matrix::Identity()), - .existence_probability = existence_config_.initial_existence_probability, - .confirmed = false - }); - } + tracks_.reserve(tracks_.size() + measurements.cols()); + for (auto i : std::views::iota(Eigen::Index{0}, measurements.cols())) { + tracks_.emplace_back(Track{ + .id = track_id_counter_++, + .state = vortex::prob::Gauss6d( + measurements.col(i), Eigen::Matrix::Identity()), + .existence_probability = + existence_config_.initial_existence_probability, + .confirmed = false}); + } } } // namespace vortex::filtering diff --git a/filtering/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros.cpp b/filtering/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros.cpp index 03e306422..3af1f1d3e 100644 --- a/filtering/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros.cpp +++ b/filtering/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros.cpp @@ -25,8 +25,7 @@ void IPDAPoseFilteringNode::setup_publishers_and_subscribers() { pose_array_pub_ = this->create_publisher( pub_topic_name, qos_sensor_data_pub); - int publish_timer = - this->declare_parameter("publish_timer"); + int publish_timer = this->declare_parameter("publish_timer"); pub_timer_ = this->create_wall_timer( std::chrono::milliseconds(publish_timer), @@ -110,7 +109,7 @@ void IPDAPoseFilteringNode::setup_track_manager() { track_manager_ = std::make_unique(config); } -template +template void IPDAPoseFilteringNode::create_pose_subscription( const std::string& topic_name, const rmw_qos_profile_t& qos_profile) { @@ -129,10 +128,11 @@ void IPDAPoseFilteringNode::create_pose_subscription( tf_filter_ = filter; } -template +template void IPDAPoseFilteringNode::pose_callback( const typename MsgT::ConstSharedPtr& msg) { - Measurements measurements = vortex::utils::ros_conversions::ros_to_eigen6d(*msg); + Measurements measurements = + vortex::utils::ros_conversions::ros_to_eigen6d(*msg); rclcpp::Time current_time(msg->header.stamp); double dt = (current_time - prev_meas_stamp_).seconds(); prev_meas_stamp_ = current_time; @@ -140,16 +140,17 @@ void IPDAPoseFilteringNode::pose_callback( }; void IPDAPoseFilteringNode::timer_callback() { - track_manager_->predict_tracks(); - + geometry_msgs::msg::PoseArray pose_array; pose_array.header.frame_id = target_frame_; pose_array.header.stamp = prev_meas_stamp_; for (auto track : track_manager_->get_tracks()) { - if (!track.confirmed) continue; + if (!track.confirmed) + continue; pose_array.poses.push_back( - vortex::utils::ros_conversions::eigen6d_to_pose(track.state.mean())); + vortex::utils::ros_conversions::pose_like_to_pose_msg( + track.state.mean())); } pose_array_pub_->publish(pose_array); } From 1c9c7f9fe1689b42ece72b7b6ee79850c4ef4501 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Mon, 15 Dec 2025 13:40:49 +0100 Subject: [PATCH 08/26] Variant alias --- .../ros/ipda_pose_filtering_ros.hpp | 32 +++++++++---------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp b/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp index 548dfcf61..62ccb5a8a 100644 --- a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp +++ b/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp @@ -26,6 +26,19 @@ concept ValidPoseMsg = std::same_as || std::same_as; +using PoseSubscriberVariant = std::variant< + std::shared_ptr< + message_filters::Subscriber>, + std::shared_ptr>, + std::shared_ptr>>; + +using PoseTFVariant = std::variant< + std::shared_ptr>, + std::shared_ptr>, + std::shared_ptr< + tf2_ros::MessageFilter>>; + class IPDAPoseFilteringNode : public rclcpp::Node { public: IPDAPoseFilteringNode(const rclcpp::NodeOptions& options); @@ -46,22 +59,9 @@ class IPDAPoseFilteringNode : public rclcpp::Node { void timer_callback(); - std::variant< - std::shared_ptr< - message_filters::Subscriber>, - std::shared_ptr< - message_filters::Subscriber>, - std::shared_ptr>> - subscriber_; - - std::variant< - std::shared_ptr< - tf2_ros::MessageFilter>, - std::shared_ptr>, - std::shared_ptr>> - tf_filter_; + PoseSubscriberVariant subscriber_; + + PoseTFVariant tf_filter_; std::string target_frame_; rclcpp::Time prev_meas_stamp_{0, 0, RCL_ROS_TIME}; From b9f1891f3893b8e3751b10f45f9ffdd701ca265b Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Mon, 22 Dec 2025 10:56:42 +0100 Subject: [PATCH 09/26] refactor eluer to quat --- filtering/ipda_pose_filtering/README.md | 0 .../config/ipda_pose_filtering.yaml | 14 +- .../lib/ipda_pose_track_manager.hpp | 33 ++- .../ipda_pose_filtering/lib/typedefs.hpp | 48 +++-- .../ros/ipda_pose_filtering_ros.hpp | 39 ++-- .../src/lib/ipda_pose_track_manager.cpp | 189 +++++++++++++----- .../src/ros/ipda_pose_filtering_ros.cpp | 117 ++++------- .../src/pose_action_server_ros.cpp | 2 +- .../ros_node_tests/lqr_autopilot_node_test.sh | 0 9 files changed, 259 insertions(+), 183 deletions(-) create mode 100644 filtering/ipda_pose_filtering/README.md mode change 100644 => 100755 tests/ros_node_tests/lqr_autopilot_node_test.sh diff --git a/filtering/ipda_pose_filtering/README.md b/filtering/ipda_pose_filtering/README.md new file mode 100644 index 000000000..e69de29bb diff --git a/filtering/ipda_pose_filtering/config/ipda_pose_filtering.yaml b/filtering/ipda_pose_filtering/config/ipda_pose_filtering.yaml index 845bc435b..602f99c53 100644 --- a/filtering/ipda_pose_filtering/config/ipda_pose_filtering.yaml +++ b/filtering/ipda_pose_filtering/config/ipda_pose_filtering.yaml @@ -1,10 +1,11 @@ /**: ros__parameters: - pose_message_type: "PoseStamped" - pose_sub_topic: "pose" + pose_sub_topic: "/aruco_detector/board" pose_array_pub_topic: "filtered_pose_array" target_frame: "odom" - publish_timer: 100 + timer_rate_ms: 100 + + max_angle_gate_threshold: 0.4 # in radians # IPDA config ipda: @@ -18,14 +19,11 @@ # Dyn model config dyn_mod: - std_pos: 0.2 - std_orient: 0.2 + std_dev: 0.2 # Sensor model config sensor_mod: - std_pos: 0.1 - std_orient: 0.1 - max_angle_gate_threshold: 0.4 # in radians + std_dev: 0.2 # Existence management config existence: diff --git a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/ipda_pose_track_manager.hpp b/filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/ipda_pose_track_manager.hpp index 22ed0c0b1..5d3a39b40 100644 --- a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/ipda_pose_track_manager.hpp +++ b/filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/ipda_pose_track_manager.hpp @@ -12,14 +12,37 @@ class IPDAPoseTrackManager { public: IPDAPoseTrackManager(const TrackManagerConfig& config); - void predict_tracks(); - - void measurement_update(Measurements& measurements, double dt); + void step(std::vector& measurements, double dt); const std::vector& get_tracks() { return tracks_; } private: - void create_tracks(const Measurements& measurements); + std::vector angular_gate_measurements( + const Track& track, + const std::vector& measurements) const; + + void delete_tracks(); + + void create_tracks(const std::vector& measurements); + + void sort_tracks_by_priority(); + + Eigen::Matrix build_position_matrix( + const std::vector& measurements, + const std::vector& indices) const; + + std::vector collect_used_quaternions( + const std::vector& measurements, + const std::vector& angular_gate_indices, + const Eigen::Array& gated_measurements, + std::vector& consumed_indices_out) const; + + void update_track_orientation( + Track& track, + const std::vector& quaternions); + + void erase_gated_measurements(std::vector& measurements, + std::vector& indices); int track_id_counter_ = 0; @@ -32,6 +55,8 @@ class IPDAPoseTrackManager { vortex::filter::IPDA::Config ipda_config_; ExistenceManagementConfig existence_config_; + + double max_angle_gate_threshold_{}; }; } // namespace vortex::filtering diff --git a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/typedefs.hpp b/filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/typedefs.hpp index c259e1fe8..bd7e74de4 100644 --- a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/typedefs.hpp +++ b/filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/typedefs.hpp @@ -2,38 +2,30 @@ #define TYPEDEFS_HPP #include +#include #include #include -namespace Eigen { - -using Vector6d = Eigen::Matrix; - -} // namespace Eigen - namespace vortex::filtering { -using Measurements = Eigen::Matrix; -using State6d = vortex::prob::Gauss6d; -using DynMod = vortex::models::ConstantPose; -using SensorMod = vortex::models::IdentityPoseSensor<6, 6>; +using State3d = vortex::prob::Gauss3d; +using DynMod = vortex::models::ConstantDynamicModel<3>; +using SensorMod = vortex::models::IdentitySensorModel<3, 3>; using IPDA = vortex::filter::IPDA; +using Pose = vortex::utils::types::Pose; struct DynModConfig { - double std_pos; - double std_orient; + double std_dev{1.0}; }; struct SensorModConfig { - double std_pos; - double std_orient; - double max_angle_gate_threshold; + double std_dev{1.0}; }; struct ExistenceManagementConfig { - double confirmation_threshold; - double deletion_threshold; - double initial_existence_probability; + double confirmation_threshold{0.4}; + double deletion_threshold{0.2}; + double initial_existence_probability{0.3}; }; struct TrackManagerConfig { @@ -41,13 +33,25 @@ struct TrackManagerConfig { DynModConfig dyn_mod; SensorModConfig sensor_mod; ExistenceManagementConfig existence; + double max_angle_gate_threshold{}; }; struct Track { - int id; - State6d state; - double existence_probability; - bool confirmed; + int id{}; + State3d state; + Eigen::Quaterniond current_orientation; + Eigen::Quaterniond prev_orientation; + double existence_probability{0.0}; + bool confirmed{false}; + + Pose to_pose() const { + return vortex::utils::types::Pose::from_eigen(state.mean(), + current_orientation); + } + + double angular_distance(const Pose& z) const { + return current_orientation.angularDistance(z.ori_quaternion()); + } }; } // namespace vortex::filtering diff --git a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp b/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp index 62ccb5a8a..52f832da8 100644 --- a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp +++ b/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp @@ -16,28 +16,19 @@ #include "ipda_pose_filtering/lib/ipda_pose_track_manager.hpp" #include -#include namespace vortex::filtering { -template +using PoseMsgT = geometry_msgs::msg::PoseStamped; + +template concept ValidPoseMsg = - std::same_as || - std::same_as || - std::same_as; - -using PoseSubscriberVariant = std::variant< - std::shared_ptr< - message_filters::Subscriber>, - std::shared_ptr>, - std::shared_ptr>>; - -using PoseTFVariant = std::variant< - std::shared_ptr>, - std::shared_ptr>, - std::shared_ptr< - tf2_ros::MessageFilter>>; + std::same_as || + std::same_as || + std::same_as; + +static_assert(ValidPoseMsg, + "PoseMsgT must be a supported pose message type"); class IPDAPoseFilteringNode : public rclcpp::Node { public: @@ -50,29 +41,27 @@ class IPDAPoseFilteringNode : public rclcpp::Node { void setup_track_manager(); - template void create_pose_subscription(const std::string& topic_name, const rmw_qos_profile_t& qos_profile); - template - void pose_callback(const typename MsgT::ConstSharedPtr& msg); - void timer_callback(); - PoseSubscriberVariant subscriber_; + std::shared_ptr> subscriber_; - PoseTFVariant tf_filter_; + std::shared_ptr> tf_filter_; std::string target_frame_; - rclcpp::Time prev_meas_stamp_{0, 0, RCL_ROS_TIME}; std::shared_ptr tf2_buffer_; std::shared_ptr tf2_listener_; rclcpp::Publisher::SharedPtr pose_array_pub_; rclcpp::TimerBase::SharedPtr pub_timer_; + double filter_dt_seconds_{0.0}; std::unique_ptr track_manager_; + + std::vector measurements_; }; } // namespace vortex::filtering diff --git a/filtering/ipda_pose_filtering/src/lib/ipda_pose_track_manager.cpp b/filtering/ipda_pose_filtering/src/lib/ipda_pose_track_manager.cpp index 955b0e68b..26e25782a 100644 --- a/filtering/ipda_pose_filtering/src/lib/ipda_pose_track_manager.cpp +++ b/filtering/ipda_pose_filtering/src/lib/ipda_pose_track_manager.cpp @@ -1,77 +1,172 @@ #include "ipda_pose_filtering/lib/ipda_pose_track_manager.hpp" #include #include +#include namespace vortex::filtering { IPDAPoseTrackManager::IPDAPoseTrackManager(const TrackManagerConfig& config) : track_id_counter_(0), - dyn_mod_(config.dyn_mod.std_pos, config.dyn_mod.std_orient), - sensor_mod_(config.sensor_mod.std_pos, - config.sensor_mod.std_orient, - config.sensor_mod.max_angle_gate_threshold) { + dyn_mod_(config.dyn_mod.std_dev), + sensor_mod_(config.sensor_mod.std_dev) { ipda_config_ = config.ipda; existence_config_ = config.existence; + max_angle_gate_threshold_ = config.max_angle_gate_threshold; } -void IPDAPoseTrackManager::predict_tracks() { - auto new_end = std::ranges::remove_if(tracks_, [this](Track& track) { - track.existence_probability = IPDA::existence_prediction( - track.existence_probability, ipda_config_.ipda.prob_of_survival); +void IPDAPoseTrackManager::step(std::vector& measurements, double dt) { + sort_tracks_by_priority(); - return track.existence_probability < - existence_config_.deletion_threshold; - }); - tracks_.erase(new_end.begin(), new_end.end()); -} + for (Track& track : tracks_) { + auto angular_gate_indices = + angular_gate_measurements(track, measurements); -void IPDAPoseTrackManager::measurement_update(Measurements& measurements, - double dt) { - 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; - }); + if (angular_gate_indices.empty()) { + continue; + } + + Eigen::Matrix Z = + build_position_matrix(measurements, angular_gate_indices); - for (Track& track : tracks_) { const IPDA::State state_est_prev{ .x_estimate = track.state, .existence_probability = track.existence_probability}; - auto output = IPDA::step(dyn_mod_, sensor_mod_, dt, state_est_prev, - measurements, ipda_config_); + auto ipda_output = IPDA::step(dyn_mod_, sensor_mod_, dt, state_est_prev, + Z, ipda_config_); - track.state = output.state.x_estimate; - track.existence_probability = output.state.existence_probability; + track.state = ipda_output.state.x_estimate; + track.existence_probability = ipda_output.state.existence_probability; - std::vector outside_cols; - outside_cols.reserve(measurements.cols()); - for (Eigen::Index i = 0; i < measurements.cols(); ++i) - if (!output.gated_measurements[i]) - outside_cols.push_back(measurements.col(i)); + std::vector consumed_indices; + auto used_quaternions = collect_used_quaternions( + measurements, angular_gate_indices, ipda_output.gated_measurements, + consumed_indices); - if (!outside_cols.empty()) { - Measurements outside( - 6, static_cast(outside_cols.size())); - for (Eigen::Index i = 0; i < outside.cols(); ++i) - outside.col(i) = outside_cols[i]; - measurements = std::move(outside); - } + update_track_orientation(track, used_quaternions); + + erase_gated_measurements(measurements, consumed_indices); } + + delete_tracks(); create_tracks(measurements); } -void IPDAPoseTrackManager::create_tracks(const Measurements& measurements) { - tracks_.reserve(tracks_.size() + measurements.cols()); - for (auto i : std::views::iota(Eigen::Index{0}, measurements.cols())) { - tracks_.emplace_back(Track{ - .id = track_id_counter_++, - .state = vortex::prob::Gauss6d( - measurements.col(i), Eigen::Matrix::Identity()), - .existence_probability = - existence_config_.initial_existence_probability, - .confirmed = false}); +std::vector IPDAPoseTrackManager::angular_gate_measurements( + const Track& track, + const std::vector& measurements) const { + std::vector indices; + indices.reserve(measurements.size()); + + for (Eigen::Index i = 0; i < static_cast(measurements.size()); + ++i) { + if (track.angular_distance(measurements[i]) < + max_angle_gate_threshold_) { + indices.push_back(i); + } + } + return indices; +} + +Eigen::Matrix +IPDAPoseTrackManager::build_position_matrix( + const std::vector& measurements, + const std::vector& indices) const { + Eigen::Matrix Z(3, indices.size()); + for (Eigen::Index k = 0; k < static_cast(indices.size()); + ++k) { + Z.col(k) = measurements[indices[k]].pos_vector(); + } + return Z; +} + +std::vector IPDAPoseTrackManager::collect_used_quaternions( + const std::vector& measurements, + const std::vector& angular_gate_indices, + const Eigen::Array& gated_measurements, + std::vector& consumed_indices_out) const { + std::vector quats; + quats.reserve(gated_measurements.size()); + + for (Eigen::Index k = 0; + k < static_cast(angular_gate_indices.size()); ++k) { + if (gated_measurements[k]) { + Eigen::Index original_idx = angular_gate_indices[k]; + quats.push_back(measurements[original_idx].ori_quaternion()); + consumed_indices_out.push_back(original_idx); + } + } + return quats; +} + +void IPDAPoseTrackManager::update_track_orientation( + Track& track, + const std::vector& quaternions) { + if (quaternions.empty()) { + return; + } + + Eigen::Quaterniond avg_q = + vortex::utils::math::average_quaternions(quaternions); + + double d_prev = track.prev_orientation.angularDistance(avg_q); + + double d_curr = track.current_orientation.angularDistance(avg_q); + + constexpr double eps = 1e-6; + + double alpha = d_prev / (d_curr + eps); + alpha = std::clamp(alpha, 0.05, 1.0); + + track.prev_orientation = track.current_orientation; + + track.current_orientation = track.current_orientation.slerp(alpha, avg_q); +} + +void IPDAPoseTrackManager::erase_gated_measurements( + std::vector& measurements, + std::vector& indices) { + std::ranges::sort(indices, std::greater<>()); + for (Eigen::Index idx : indices) { + measurements.erase(measurements.begin() + idx); + } +} + +void IPDAPoseTrackManager::create_tracks( + const std::vector& measurements) { + tracks_.reserve(tracks_.size() + measurements.size()); + + auto make_track = [this](const Pose& measurement) { + return Track{.id = track_id_counter_++, + .state = vortex::prob::Gauss3d( + measurement.pos_vector(), + Eigen::Matrix::Identity()), + .current_orientation = measurement.ori_quaternion(), + .prev_orientation = measurement.ori_quaternion(), + .existence_probability = + existence_config_.initial_existence_probability, + .confirmed = false}; + }; + + for (const Pose& m : measurements) { + tracks_.push_back(make_track(m)); } } +void IPDAPoseTrackManager::delete_tracks() { + auto new_end = std::ranges::remove_if(tracks_, [this](Track& track) { + return track.existence_probability < + existence_config_.deletion_threshold; + }); + tracks_.erase(new_end.begin(), new_end.end()); +} + +void IPDAPoseTrackManager::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/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros.cpp b/filtering/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros.cpp index 3af1f1d3e..40d412403 100644 --- a/filtering/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros.cpp +++ b/filtering/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros.cpp @@ -25,10 +25,12 @@ void IPDAPoseFilteringNode::setup_publishers_and_subscribers() { pose_array_pub_ = this->create_publisher( pub_topic_name, qos_sensor_data_pub); - int publish_timer = this->declare_parameter("publish_timer"); + int timer_rate_ms = this->declare_parameter("timer_rate_ms"); + + filter_dt_seconds_ = static_cast(timer_rate_ms) / 1000; pub_timer_ = this->create_wall_timer( - std::chrono::milliseconds(publish_timer), + std::chrono::milliseconds(timer_rate_ms), std::bind(&IPDAPoseFilteringNode::timer_callback, this)); target_frame_ = this->declare_parameter("target_frame"); @@ -43,30 +45,34 @@ void IPDAPoseFilteringNode::setup_publishers_and_subscribers() { tf2_buffer_->setCreateTimerInterface(timer_interface); tf2_listener_ = std::make_shared(*tf2_buffer_); - const std::string msg_type = - this->declare_parameter("pose_message_type"); 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)}; - if (msg_type == "PoseStamped") { - create_pose_subscription( - pose_sub_topic, qos_sensor_data_sub.get_rmw_qos_profile()); - } else if (msg_type == "PoseArray") { - create_pose_subscription( - pose_sub_topic, qos_sensor_data_sub.get_rmw_qos_profile()); - } else if (msg_type == "PoseWithCovarianceStamped") { - create_pose_subscription( - pose_sub_topic, qos_sensor_data_sub.get_rmw_qos_profile()); - } else { - spdlog::error( - "IPDAPoseFilteringNode: Unsupported pose message type: {} \n" - "Supported types are: Pose, PoseStamped, PoseArray, " - "PoseWithCovarianceStamped", - msg_type); - } + create_pose_subscription(pose_sub_topic, + qos_sensor_data_sub.get_rmw_qos_profile()); +} + +void IPDAPoseFilteringNode::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) { + this->measurements_ = + vortex::utils::ros_conversions::ros_to_pose_vec(*msg); + }); + + subscriber_ = sub; + tf_filter_ = filter; } void IPDAPoseFilteringNode::setup_track_manager() { @@ -87,16 +93,13 @@ void IPDAPoseFilteringNode::setup_track_manager() { config.ipda.pdaf.clutter_intensity = this->declare_parameter("ipda.clutter_intensity"); - config.dyn_mod.std_pos = this->declare_parameter("dyn_mod.std_pos"); - config.dyn_mod.std_orient = - this->declare_parameter("dyn_mod.std_orient"); + config.dyn_mod.std_dev = this->declare_parameter("dyn_mod.std_dev"); + + config.sensor_mod.std_dev = + this->declare_parameter("sensor_mod.std_dev"); - config.sensor_mod.std_pos = - this->declare_parameter("sensor_mod.std_pos"); - config.sensor_mod.std_orient = - this->declare_parameter("sensor_mod.std_orient"); - config.sensor_mod.max_angle_gate_threshold = - this->declare_parameter("sensor_mod.max_angle_gate_threshold"); + config.max_angle_gate_threshold = + this->declare_parameter("max_angle_gate_threshold"); config.existence.confirmation_threshold = this->declare_parameter("existence.confirmation_threshold"); @@ -109,64 +112,26 @@ void IPDAPoseFilteringNode::setup_track_manager() { track_manager_ = std::make_unique(config); } -template -void IPDAPoseFilteringNode::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_, 100, - this->get_node_logging_interface(), this->get_node_clock_interface()); - - filter->registerCallback([this](const typename MsgT::ConstSharedPtr msg) { - this->pose_callback(msg); - }); - - subscriber_ = sub; - tf_filter_ = filter; -} - -template -void IPDAPoseFilteringNode::pose_callback( - const typename MsgT::ConstSharedPtr& msg) { - Measurements measurements = - vortex::utils::ros_conversions::ros_to_eigen6d(*msg); - rclcpp::Time current_time(msg->header.stamp); - double dt = (current_time - prev_meas_stamp_).seconds(); - prev_meas_stamp_ = current_time; - track_manager_->measurement_update(measurements, dt); -}; - void IPDAPoseFilteringNode::timer_callback() { - track_manager_->predict_tracks(); + track_manager_->step(measurements_, filter_dt_seconds_); + measurements_.clear(); geometry_msgs::msg::PoseArray pose_array; pose_array.header.frame_id = target_frame_; - pose_array.header.stamp = prev_meas_stamp_; - for (auto track : track_manager_->get_tracks()) { + 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::pose_like_to_pose_msg( - track.state.mean())); + vortex::utils::ros_conversions::to_pose_msg(track.to_pose())); + } + + if (pose_array.poses.empty()) { + return; } pose_array_pub_->publish(pose_array); } -template void -IPDAPoseFilteringNode::pose_callback( - const geometry_msgs::msg::PoseStamped::ConstSharedPtr& msg); - -template void -IPDAPoseFilteringNode::pose_callback( - const geometry_msgs::msg::PoseArray::ConstSharedPtr& msg); - -template void IPDAPoseFilteringNode::pose_callback< - geometry_msgs::msg::PoseWithCovarianceStamped>( - const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr& msg); - RCLCPP_COMPONENTS_REGISTER_NODE(IPDAPoseFilteringNode); } // namespace vortex::filtering 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/tests/ros_node_tests/lqr_autopilot_node_test.sh b/tests/ros_node_tests/lqr_autopilot_node_test.sh old mode 100644 new mode 100755 From 8cc0e027f3684686ce9638f66c7a1e7010d8f72b Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Mon, 22 Dec 2025 11:41:30 +0100 Subject: [PATCH 10/26] added confirm_tracks function --- .../lib/ipda_pose_track_manager.hpp | 10 ++++++---- .../ros/ipda_pose_filtering_ros.hpp | 14 ++++++++------ .../src/lib/ipda_pose_track_manager.cpp | 10 ++++++++++ 3 files changed, 24 insertions(+), 10 deletions(-) diff --git a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/ipda_pose_track_manager.hpp b/filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/ipda_pose_track_manager.hpp index 5d3a39b40..ccdbbe540 100644 --- a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/ipda_pose_track_manager.hpp +++ b/filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/ipda_pose_track_manager.hpp @@ -1,5 +1,5 @@ -#ifndef IPDA_POSE_TRACK_MANAGER_HPP -#define IPDA_POSE_TRACK_MANAGER_HPP +#ifndef IPDA_POSE_FILTERING__LIB__IPDA_POSE_TRACK_MANAGER_HPP_ +#define IPDA_POSE_FILTERING__LIB__IPDA_POSE_TRACK_MANAGER_HPP_ #include #include @@ -10,7 +10,7 @@ namespace vortex::filtering { class IPDAPoseTrackManager { public: - IPDAPoseTrackManager(const TrackManagerConfig& config); + explicit IPDAPoseTrackManager(const TrackManagerConfig& config); void step(std::vector& measurements, double dt); @@ -21,6 +21,8 @@ class IPDAPoseTrackManager { const Track& track, const std::vector& measurements) const; + void confirm_tracks(); + void delete_tracks(); void create_tracks(const std::vector& measurements); @@ -61,4 +63,4 @@ class IPDAPoseTrackManager { } // namespace vortex::filtering -#endif // IPDA_POSE_TRACK_MANAGER_HPP +#endif // IPDA_POSE_FILTERING__LIB__IPDA_POSE_TRACK_MANAGER_HPP_ diff --git a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp b/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp index 52f832da8..a9e02eea3 100644 --- a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp +++ b/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp @@ -1,5 +1,5 @@ -#ifndef IPDA_POSE_FILTERING_ROS_HPP -#define IPDA_POSE_FILTERING_ROS_HPP +#ifndef IPDA_POSE_FILTERING__ROS__IPDA_POSE_FILTERING_ROS_HPP_ +#define IPDA_POSE_FILTERING__ROS__IPDA_POSE_FILTERING_ROS_HPP_ #include #include @@ -10,9 +10,11 @@ #include #include #include +#include #include +#include #include - +#include #include "ipda_pose_filtering/lib/ipda_pose_track_manager.hpp" #include @@ -32,9 +34,9 @@ static_assert(ValidPoseMsg, class IPDAPoseFilteringNode : public rclcpp::Node { public: - IPDAPoseFilteringNode(const rclcpp::NodeOptions& options); + explicit IPDAPoseFilteringNode(const rclcpp::NodeOptions& options); - ~IPDAPoseFilteringNode() {}; + ~IPDAPoseFilteringNode() {} private: void setup_publishers_and_subscribers(); @@ -66,4 +68,4 @@ class IPDAPoseFilteringNode : public rclcpp::Node { } // namespace vortex::filtering -#endif // IPDA_POSE_FILTERING_ROS_HPP +#endif // IPDA_POSE_FILTERING__ROS__IPDA_POSE_FILTERING_ROS_HPP_ diff --git a/filtering/ipda_pose_filtering/src/lib/ipda_pose_track_manager.cpp b/filtering/ipda_pose_filtering/src/lib/ipda_pose_track_manager.cpp index 26e25782a..a40dd39f7 100644 --- a/filtering/ipda_pose_filtering/src/lib/ipda_pose_track_manager.cpp +++ b/filtering/ipda_pose_filtering/src/lib/ipda_pose_track_manager.cpp @@ -48,6 +48,7 @@ void IPDAPoseTrackManager::step(std::vector& measurements, double dt) { erase_gated_measurements(measurements, consumed_indices); } + confirm_tracks(); delete_tracks(); create_tracks(measurements); } @@ -153,6 +154,15 @@ void IPDAPoseTrackManager::create_tracks( } } +void IPDAPoseTrackManager::confirm_tracks() { + for (Track& track : tracks_) { + if (!track.confirmed && track.existence_probability >= + existence_config_.confirmation_threshold) { + track.confirmed = true; + } + } +} + void IPDAPoseTrackManager::delete_tracks() { auto new_end = std::ranges::remove_if(tracks_, [this](Track& track) { return track.existence_probability < From 9be9e2ab670c2778c7334794683653f6108e2a18 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Wed, 24 Dec 2025 14:46:57 +0100 Subject: [PATCH 11/26] landmark support --- filtering/ipda_pose_filtering/CMakeLists.txt | 8 ++++++ .../ros/ipda_pose_filtering_ros.hpp | 6 +++-- filtering/ipda_pose_filtering/package.xml | 3 +++ .../src/ros/ipda_pose_filtering_ros.cpp | 26 ++++++++++++++----- 4 files changed, 34 insertions(+), 9 deletions(-) diff --git a/filtering/ipda_pose_filtering/CMakeLists.txt b/filtering/ipda_pose_filtering/CMakeLists.txt index d5ef8ee4c..e5b496a1d 100644 --- a/filtering/ipda_pose_filtering/CMakeLists.txt +++ b/filtering/ipda_pose_filtering/CMakeLists.txt @@ -18,7 +18,10 @@ 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) @@ -37,6 +40,9 @@ target_include_directories(${CORE_LIB_NAME} 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") @@ -55,6 +61,8 @@ ament_target_dependencies(${COMPONENT_LIB_NAME} message_filters vortex_filtering vortex_utils + vortex_utils_ros + vortex_utils_ros_tf ) target_link_libraries(${COMPONENT_LIB_NAME} ${PROJECT_NAME}) diff --git a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp b/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp index a9e02eea3..35e0cab15 100644 --- a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp +++ b/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp @@ -15,19 +15,21 @@ #include #include #include +#include #include "ipda_pose_filtering/lib/ipda_pose_track_manager.hpp" #include namespace vortex::filtering { -using PoseMsgT = geometry_msgs::msg::PoseStamped; +using PoseMsgT = vortex_msgs::msg::LandmarkArray; template concept ValidPoseMsg = std::same_as || std::same_as || - std::same_as; + std::same_as || + std::same_as; static_assert(ValidPoseMsg, "PoseMsgT must be a supported pose message type"); diff --git a/filtering/ipda_pose_filtering/package.xml b/filtering/ipda_pose_filtering/package.xml index 637ed099c..f2bd62b1f 100644 --- a/filtering/ipda_pose_filtering/package.xml +++ b/filtering/ipda_pose_filtering/package.xml @@ -18,7 +18,10 @@ tf2_geometry_msgs message_filters vortex_utils + vortex_utils_ros + vortex_utils_ros_tf vortex_filtering + vortex_msgs ament_cmake diff --git a/filtering/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros.cpp b/filtering/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros.cpp index 40d412403..8e7a514f1 100644 --- a/filtering/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros.cpp +++ b/filtering/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros.cpp @@ -2,8 +2,9 @@ #include #include #include -#include -#include "vortex/utils/ros_conversions.hpp" +#include +#include "vortex/utils/ros/ros_conversions.hpp" +#include "vortex/utils/ros/ros_transforms.hpp" using std::placeholders::_1; using std::placeholders::_2; @@ -65,11 +66,22 @@ void IPDAPoseFilteringNode::create_pose_subscription( *sub, *tf2_buffer_, target_frame_, 10, this->get_node_logging_interface(), this->get_node_clock_interface()); - filter->registerCallback( - [this](const typename PoseMsgT::ConstSharedPtr msg) { - this->measurements_ = - vortex::utils::ros_conversions::ros_to_pose_vec(*msg); - }); + 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) { + RCLCPP_WARN( + this->get_logger(), "TF transform failed from '%s' to '%s': %s", + msg->header.frame_id.c_str(), target_frame_.c_str(), ex.what()); + return; + } + + this->measurements_ = + vortex::utils::ros_conversions::ros_to_pose_vec(pose_tf); + }); subscriber_ = sub; tf_filter_ = filter; From 7acc72ca3ce4c7064a03865619b33ce2bf54119c Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Thu, 25 Dec 2025 11:30:29 +0100 Subject: [PATCH 12/26] added debug pose publisher --- filtering/ipda_pose_filtering/CMakeLists.txt | 1 + .../config/ipda_pose_filtering.yaml | 5 ++ .../ros/ipda_pose_filtering_ros.hpp | 10 +++ .../src/ros/ipda_pose_filtering_ros.cpp | 11 +++ .../src/ros/ipda_pose_filtering_ros_debug.cpp | 70 +++++++++++++++++++ 5 files changed, 97 insertions(+) create mode 100644 filtering/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros_debug.cpp diff --git a/filtering/ipda_pose_filtering/CMakeLists.txt b/filtering/ipda_pose_filtering/CMakeLists.txt index e5b496a1d..64887b117 100644 --- a/filtering/ipda_pose_filtering/CMakeLists.txt +++ b/filtering/ipda_pose_filtering/CMakeLists.txt @@ -49,6 +49,7 @@ 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 ) ament_target_dependencies(${COMPONENT_LIB_NAME} diff --git a/filtering/ipda_pose_filtering/config/ipda_pose_filtering.yaml b/filtering/ipda_pose_filtering/config/ipda_pose_filtering.yaml index 602f99c53..3ff8de4ec 100644 --- a/filtering/ipda_pose_filtering/config/ipda_pose_filtering.yaml +++ b/filtering/ipda_pose_filtering/config/ipda_pose_filtering.yaml @@ -30,3 +30,8 @@ confirmation_threshold: 0.6 deletion_threshold: 0.2 initial_existence_probability: 0.4 + + debug: + enable: true + topic_name_meas: "/pose_debug_meas" + topic_name_state: "/pose_debug_state" diff --git a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp b/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp index 35e0cab15..85b961f66 100644 --- a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp +++ b/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp @@ -16,6 +16,7 @@ #include #include #include +#include #include "ipda_pose_filtering/lib/ipda_pose_track_manager.hpp" #include @@ -66,6 +67,15 @@ class IPDAPoseFilteringNode : public rclcpp::Node { std::unique_ptr track_manager_; std::vector measurements_; + + bool debug_{false}; + rclcpp::Publisher::SharedPtr + pose_meas_debug_pub_; + rclcpp::Publisher::SharedPtr + pose_state_debug_pub_; + void setup_debug_publishers(); + void publish_meas_debug(); + void publish_state_debug(); }; } // namespace vortex::filtering diff --git a/filtering/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros.cpp b/filtering/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros.cpp index 8e7a514f1..8e010b409 100644 --- a/filtering/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros.cpp +++ b/filtering/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros.cpp @@ -54,6 +54,11 @@ void IPDAPoseFilteringNode::setup_publishers_and_subscribers() { 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 IPDAPoseFilteringNode::create_pose_subscription( @@ -125,8 +130,14 @@ void IPDAPoseFilteringNode::setup_track_manager() { } void IPDAPoseFilteringNode::timer_callback() { + if (debug_) { + publish_meas_debug(); + } track_manager_->step(measurements_, filter_dt_seconds_); measurements_.clear(); + if (debug_) { + publish_state_debug(); + } geometry_msgs::msg::PoseArray pose_array; pose_array.header.frame_id = target_frame_; diff --git a/filtering/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros_debug.cpp b/filtering/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros_debug.cpp new file mode 100644 index 000000000..541d1b543 --- /dev/null +++ b/filtering/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros_debug.cpp @@ -0,0 +1,70 @@ +#include +#include +#include "ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp" + +namespace vortex::filtering { + +void IPDAPoseFilteringNode::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 IPDAPoseFilteringNode::publish_meas_debug() { + if (measurements_.empty()) { + return; + } + Pose 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.x; + msg.y = meas.y; + msg.z = meas.z; + + Eigen::Vector3d euler = + vortex::utils::math::quat_to_euler(meas.ori_quaternion()); + + msg.roll = euler(0); + msg.pitch = euler(1); + msg.yaw = euler(2); + + pose_meas_debug_pub_->publish(msg); +} + +void IPDAPoseFilteringNode::publish_state_debug() { + if (track_manager_->get_tracks().empty()) { + return; + } + Track track = track_manager_->get_tracks().at(0); + Eigen::Vector3d pos = track.state.mean(); + Eigen::Quaterniond quat = track.current_orientation; + + 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 From 5396c1ed714679ad19750918aaac420618f68475 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Thu, 25 Dec 2025 16:04:35 +0100 Subject: [PATCH 13/26] renamed pkg: ipda_pose -> pose --- .../CMakeLists.txt | 6 ++--- .../README.md | 0 .../config/pose_filtering.yaml} | 0 .../lib/pose_track_manager.hpp} | 10 ++++---- .../include/pose_filtering}/lib/typedefs.hpp | 6 ++--- .../ros/pose_filtering_ros.hpp} | 16 ++++++------ .../launch/pose_filtering.launch.py} | 10 ++++---- .../package.xml | 4 +-- .../src/lib/pose_track_manager.cpp} | 25 +++++++++---------- .../src/ros/pose_filtering_ros.cpp} | 20 +++++++-------- .../src/ros/pose_filtering_ros_debug.cpp} | 8 +++--- 11 files changed, 52 insertions(+), 53 deletions(-) rename filtering/{ipda_pose_filtering => pose_filtering}/CMakeLists.txt (95%) rename filtering/{ipda_pose_filtering => pose_filtering}/README.md (100%) rename filtering/{ipda_pose_filtering/config/ipda_pose_filtering.yaml => pose_filtering/config/pose_filtering.yaml} (100%) rename filtering/{ipda_pose_filtering/include/ipda_pose_filtering/lib/ipda_pose_track_manager.hpp => pose_filtering/include/pose_filtering/lib/pose_track_manager.hpp} (84%) rename filtering/{ipda_pose_filtering/include/ipda_pose_filtering => pose_filtering/include/pose_filtering}/lib/typedefs.hpp (91%) rename filtering/{ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp => pose_filtering/include/pose_filtering/ros/pose_filtering_ros.hpp} (82%) rename filtering/{ipda_pose_filtering/launch/ipda_pose_filtering.launch.py => pose_filtering/launch/pose_filtering.launch.py} (63%) rename filtering/{ipda_pose_filtering => pose_filtering}/package.xml (83%) rename filtering/{ipda_pose_filtering/src/lib/ipda_pose_track_manager.cpp => pose_filtering/src/lib/pose_track_manager.cpp} (87%) rename filtering/{ipda_pose_filtering/src/ros/ipda_pose_filtering_ros.cpp => pose_filtering/src/ros/pose_filtering_ros.cpp} (89%) rename filtering/{ipda_pose_filtering/src/ros/ipda_pose_filtering_ros_debug.cpp => pose_filtering/src/ros/pose_filtering_ros_debug.cpp} (89%) diff --git a/filtering/ipda_pose_filtering/CMakeLists.txt b/filtering/pose_filtering/CMakeLists.txt similarity index 95% rename from filtering/ipda_pose_filtering/CMakeLists.txt rename to filtering/pose_filtering/CMakeLists.txt index 64887b117..e3a20ab9d 100644 --- a/filtering/ipda_pose_filtering/CMakeLists.txt +++ b/filtering/pose_filtering/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(ipda_pose_filtering) +project(pose_filtering) if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 20) @@ -28,7 +28,7 @@ include_directories(include) set(CORE_LIB_NAME "${PROJECT_NAME}") add_library(${CORE_LIB_NAME} SHARED - src/lib/ipda_pose_track_manager.cpp + src/lib/pose_track_manager.cpp ) target_include_directories(${CORE_LIB_NAME} @@ -70,7 +70,7 @@ target_link_libraries(${COMPONENT_LIB_NAME} ${PROJECT_NAME}) rclcpp_components_register_node( ${COMPONENT_LIB_NAME} - PLUGIN "IPDAPoseFilteringNode" + PLUGIN "PoseFilteringNode" EXECUTABLE ${PROJECT_NAME}_node ) diff --git a/filtering/ipda_pose_filtering/README.md b/filtering/pose_filtering/README.md similarity index 100% rename from filtering/ipda_pose_filtering/README.md rename to filtering/pose_filtering/README.md diff --git a/filtering/ipda_pose_filtering/config/ipda_pose_filtering.yaml b/filtering/pose_filtering/config/pose_filtering.yaml similarity index 100% rename from filtering/ipda_pose_filtering/config/ipda_pose_filtering.yaml rename to filtering/pose_filtering/config/pose_filtering.yaml diff --git a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/ipda_pose_track_manager.hpp b/filtering/pose_filtering/include/pose_filtering/lib/pose_track_manager.hpp similarity index 84% rename from filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/ipda_pose_track_manager.hpp rename to filtering/pose_filtering/include/pose_filtering/lib/pose_track_manager.hpp index ccdbbe540..84f09b263 100644 --- a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/ipda_pose_track_manager.hpp +++ b/filtering/pose_filtering/include/pose_filtering/lib/pose_track_manager.hpp @@ -1,5 +1,5 @@ -#ifndef IPDA_POSE_FILTERING__LIB__IPDA_POSE_TRACK_MANAGER_HPP_ -#define IPDA_POSE_FILTERING__LIB__IPDA_POSE_TRACK_MANAGER_HPP_ +#ifndef POSE_FILTERING__LIB__POSE_TRACK_MANAGER_HPP_ +#define POSE_FILTERING__LIB__POSE_TRACK_MANAGER_HPP_ #include #include @@ -8,9 +8,9 @@ namespace vortex::filtering { -class IPDAPoseTrackManager { +class PoseTrackManager { public: - explicit IPDAPoseTrackManager(const TrackManagerConfig& config); + explicit PoseTrackManager(const TrackManagerConfig& config); void step(std::vector& measurements, double dt); @@ -63,4 +63,4 @@ class IPDAPoseTrackManager { } // namespace vortex::filtering -#endif // IPDA_POSE_FILTERING__LIB__IPDA_POSE_TRACK_MANAGER_HPP_ +#endif // POSE_FILTERING__LIB__POSE_TRACK_MANAGER_HPP_ diff --git a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/typedefs.hpp b/filtering/pose_filtering/include/pose_filtering/lib/typedefs.hpp similarity index 91% rename from filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/typedefs.hpp rename to filtering/pose_filtering/include/pose_filtering/lib/typedefs.hpp index bd7e74de4..30331942c 100644 --- a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/lib/typedefs.hpp +++ b/filtering/pose_filtering/include/pose_filtering/lib/typedefs.hpp @@ -1,5 +1,5 @@ -#ifndef TYPEDEFS_HPP -#define TYPEDEFS_HPP +#ifndef POSE_FILTERING__LIB__TYPEDEFS_HPP_ +#define POSE_FILTERING__LIB__TYPEDEFS_HPP_ #include #include @@ -56,4 +56,4 @@ struct Track { } // namespace vortex::filtering -#endif // TYPEDEFS_HPP +#endif // POSE_FILTERING__LIB__TYPEDEFS_HPP_ diff --git a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp b/filtering/pose_filtering/include/pose_filtering/ros/pose_filtering_ros.hpp similarity index 82% rename from filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp rename to filtering/pose_filtering/include/pose_filtering/ros/pose_filtering_ros.hpp index 85b961f66..5159149b8 100644 --- a/filtering/ipda_pose_filtering/include/ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp +++ b/filtering/pose_filtering/include/pose_filtering/ros/pose_filtering_ros.hpp @@ -1,5 +1,5 @@ -#ifndef IPDA_POSE_FILTERING__ROS__IPDA_POSE_FILTERING_ROS_HPP_ -#define IPDA_POSE_FILTERING__ROS__IPDA_POSE_FILTERING_ROS_HPP_ +#ifndef POSE_FILTERING__ROS__POSE_FILTERING_ROS_HPP_ +#define POSE_FILTERING__ROS__POSE_FILTERING_ROS_HPP_ #include #include @@ -17,7 +17,7 @@ #include #include #include -#include "ipda_pose_filtering/lib/ipda_pose_track_manager.hpp" +#include "pose_filtering/lib/pose_track_manager.hpp" #include @@ -35,11 +35,11 @@ concept ValidPoseMsg = static_assert(ValidPoseMsg, "PoseMsgT must be a supported pose message type"); -class IPDAPoseFilteringNode : public rclcpp::Node { +class PoseFilteringNode : public rclcpp::Node { public: - explicit IPDAPoseFilteringNode(const rclcpp::NodeOptions& options); + explicit PoseFilteringNode(const rclcpp::NodeOptions& options); - ~IPDAPoseFilteringNode() {} + ~PoseFilteringNode() {} private: void setup_publishers_and_subscribers(); @@ -64,7 +64,7 @@ class IPDAPoseFilteringNode : public rclcpp::Node { rclcpp::TimerBase::SharedPtr pub_timer_; double filter_dt_seconds_{0.0}; - std::unique_ptr track_manager_; + std::unique_ptr track_manager_; std::vector measurements_; @@ -80,4 +80,4 @@ class IPDAPoseFilteringNode : public rclcpp::Node { } // namespace vortex::filtering -#endif // IPDA_POSE_FILTERING__ROS__IPDA_POSE_FILTERING_ROS_HPP_ +#endif // POSE_FILTERING__ROS__POSE_FILTERING_ROS_HPP_ diff --git a/filtering/ipda_pose_filtering/launch/ipda_pose_filtering.launch.py b/filtering/pose_filtering/launch/pose_filtering.launch.py similarity index 63% rename from filtering/ipda_pose_filtering/launch/ipda_pose_filtering.launch.py rename to filtering/pose_filtering/launch/pose_filtering.launch.py index f347a1472..85c2adb2f 100644 --- a/filtering/ipda_pose_filtering/launch/ipda_pose_filtering.launch.py +++ b/filtering/pose_filtering/launch/pose_filtering.launch.py @@ -7,17 +7,17 @@ def generate_launch_description(): config = os.path.join( - get_package_share_directory('ipda_pose_filtering'), + get_package_share_directory('pose_filtering'), 'config', - 'ipda_pose_filtering.yaml', + 'pose_filtering.yaml', ) return LaunchDescription( [ Node( - package='ipda_pose_filtering', - executable='ipda_pose_filtering_node', - name='ipda_pose_filtering', + package='pose_filtering', + executable='pose_filtering_node', + name='pose_filtering', output='screen', parameters=[config, {'use_sim_time': False}], ), diff --git a/filtering/ipda_pose_filtering/package.xml b/filtering/pose_filtering/package.xml similarity index 83% rename from filtering/ipda_pose_filtering/package.xml rename to filtering/pose_filtering/package.xml index f2bd62b1f..bbd9167e8 100644 --- a/filtering/ipda_pose_filtering/package.xml +++ b/filtering/pose_filtering/package.xml @@ -1,9 +1,9 @@ - ipda_pose_filtering + pose_filtering 0.0.0 - IPDA pose filtering and tracking package, with a C++ track manager and ROS 2 interface node. + Pose filtering and tracking package, with a C++ track manager and ROS 2 interface node. jorgen MIT diff --git a/filtering/ipda_pose_filtering/src/lib/ipda_pose_track_manager.cpp b/filtering/pose_filtering/src/lib/pose_track_manager.cpp similarity index 87% rename from filtering/ipda_pose_filtering/src/lib/ipda_pose_track_manager.cpp rename to filtering/pose_filtering/src/lib/pose_track_manager.cpp index a40dd39f7..f7beb3ff6 100644 --- a/filtering/ipda_pose_filtering/src/lib/ipda_pose_track_manager.cpp +++ b/filtering/pose_filtering/src/lib/pose_track_manager.cpp @@ -1,11 +1,11 @@ -#include "ipda_pose_filtering/lib/ipda_pose_track_manager.hpp" +#include "pose_filtering/lib/pose_track_manager.hpp" #include #include #include namespace vortex::filtering { -IPDAPoseTrackManager::IPDAPoseTrackManager(const TrackManagerConfig& config) +PoseTrackManager::PoseTrackManager(const TrackManagerConfig& config) : track_id_counter_(0), dyn_mod_(config.dyn_mod.std_dev), sensor_mod_(config.sensor_mod.std_dev) { @@ -14,7 +14,7 @@ IPDAPoseTrackManager::IPDAPoseTrackManager(const TrackManagerConfig& config) max_angle_gate_threshold_ = config.max_angle_gate_threshold; } -void IPDAPoseTrackManager::step(std::vector& measurements, double dt) { +void PoseTrackManager::step(std::vector& measurements, double dt) { sort_tracks_by_priority(); for (Track& track : tracks_) { @@ -53,7 +53,7 @@ void IPDAPoseTrackManager::step(std::vector& measurements, double dt) { create_tracks(measurements); } -std::vector IPDAPoseTrackManager::angular_gate_measurements( +std::vector PoseTrackManager::angular_gate_measurements( const Track& track, const std::vector& measurements) const { std::vector indices; @@ -70,7 +70,7 @@ std::vector IPDAPoseTrackManager::angular_gate_measurements( } Eigen::Matrix -IPDAPoseTrackManager::build_position_matrix( +PoseTrackManager::build_position_matrix( const std::vector& measurements, const std::vector& indices) const { Eigen::Matrix Z(3, indices.size()); @@ -81,7 +81,7 @@ IPDAPoseTrackManager::build_position_matrix( return Z; } -std::vector IPDAPoseTrackManager::collect_used_quaternions( +std::vector PoseTrackManager::collect_used_quaternions( const std::vector& measurements, const std::vector& angular_gate_indices, const Eigen::Array& gated_measurements, @@ -100,7 +100,7 @@ std::vector IPDAPoseTrackManager::collect_used_quaternions( return quats; } -void IPDAPoseTrackManager::update_track_orientation( +void PoseTrackManager::update_track_orientation( Track& track, const std::vector& quaternions) { if (quaternions.empty()) { @@ -124,7 +124,7 @@ void IPDAPoseTrackManager::update_track_orientation( track.current_orientation = track.current_orientation.slerp(alpha, avg_q); } -void IPDAPoseTrackManager::erase_gated_measurements( +void PoseTrackManager::erase_gated_measurements( std::vector& measurements, std::vector& indices) { std::ranges::sort(indices, std::greater<>()); @@ -133,8 +133,7 @@ void IPDAPoseTrackManager::erase_gated_measurements( } } -void IPDAPoseTrackManager::create_tracks( - const std::vector& measurements) { +void PoseTrackManager::create_tracks(const std::vector& measurements) { tracks_.reserve(tracks_.size() + measurements.size()); auto make_track = [this](const Pose& measurement) { @@ -154,7 +153,7 @@ void IPDAPoseTrackManager::create_tracks( } } -void IPDAPoseTrackManager::confirm_tracks() { +void PoseTrackManager::confirm_tracks() { for (Track& track : tracks_) { if (!track.confirmed && track.existence_probability >= existence_config_.confirmation_threshold) { @@ -163,7 +162,7 @@ void IPDAPoseTrackManager::confirm_tracks() { } } -void IPDAPoseTrackManager::delete_tracks() { +void PoseTrackManager::delete_tracks() { auto new_end = std::ranges::remove_if(tracks_, [this](Track& track) { return track.existence_probability < existence_config_.deletion_threshold; @@ -171,7 +170,7 @@ void IPDAPoseTrackManager::delete_tracks() { tracks_.erase(new_end.begin(), new_end.end()); } -void IPDAPoseTrackManager::sort_tracks_by_priority() { +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; diff --git a/filtering/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros.cpp b/filtering/pose_filtering/src/ros/pose_filtering_ros.cpp similarity index 89% rename from filtering/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros.cpp rename to filtering/pose_filtering/src/ros/pose_filtering_ros.cpp index 8e010b409..ef6925324 100644 --- a/filtering/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros.cpp +++ b/filtering/pose_filtering/src/ros/pose_filtering_ros.cpp @@ -1,4 +1,4 @@ -#include "ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp" +#include "pose_filtering/ros/pose_filtering_ros.hpp" #include #include #include @@ -11,13 +11,13 @@ using std::placeholders::_2; namespace vortex::filtering { -IPDAPoseFilteringNode::IPDAPoseFilteringNode(const rclcpp::NodeOptions& options) - : rclcpp::Node("ipda_pose_filtering_node", options) { +PoseFilteringNode::PoseFilteringNode(const rclcpp::NodeOptions& options) + : rclcpp::Node("pose_filtering_node", options) { setup_publishers_and_subscribers(); setup_track_manager(); } -void IPDAPoseFilteringNode::setup_publishers_and_subscribers() { +void PoseFilteringNode::setup_publishers_and_subscribers() { const auto qos_sensor_data_pub{ vortex::utils::qos_profiles::sensor_data_profile(1)}; std::string pub_topic_name = @@ -32,7 +32,7 @@ void IPDAPoseFilteringNode::setup_publishers_and_subscribers() { pub_timer_ = this->create_wall_timer( std::chrono::milliseconds(timer_rate_ms), - std::bind(&IPDAPoseFilteringNode::timer_callback, this)); + std::bind(&PoseFilteringNode::timer_callback, this)); target_frame_ = this->declare_parameter("target_frame"); @@ -61,7 +61,7 @@ void IPDAPoseFilteringNode::setup_publishers_and_subscribers() { } } -void IPDAPoseFilteringNode::create_pose_subscription( +void PoseFilteringNode::create_pose_subscription( const std::string& topic_name, const rmw_qos_profile_t& qos_profile) { auto sub = std::make_shared>( @@ -92,7 +92,7 @@ void IPDAPoseFilteringNode::create_pose_subscription( tf_filter_ = filter; } -void IPDAPoseFilteringNode::setup_track_manager() { +void PoseFilteringNode::setup_track_manager() { TrackManagerConfig config; config.ipda.ipda.prob_of_survival = @@ -126,10 +126,10 @@ void IPDAPoseFilteringNode::setup_track_manager() { this->declare_parameter( "existence.initial_existence_probability"); - track_manager_ = std::make_unique(config); + track_manager_ = std::make_unique(config); } -void IPDAPoseFilteringNode::timer_callback() { +void PoseFilteringNode::timer_callback() { if (debug_) { publish_meas_debug(); } @@ -155,6 +155,6 @@ void IPDAPoseFilteringNode::timer_callback() { pose_array_pub_->publish(pose_array); } -RCLCPP_COMPONENTS_REGISTER_NODE(IPDAPoseFilteringNode); +RCLCPP_COMPONENTS_REGISTER_NODE(PoseFilteringNode); } // namespace vortex::filtering diff --git a/filtering/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros_debug.cpp b/filtering/pose_filtering/src/ros/pose_filtering_ros_debug.cpp similarity index 89% rename from filtering/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros_debug.cpp rename to filtering/pose_filtering/src/ros/pose_filtering_ros_debug.cpp index 541d1b543..ea5773d93 100644 --- a/filtering/ipda_pose_filtering/src/ros/ipda_pose_filtering_ros_debug.cpp +++ b/filtering/pose_filtering/src/ros/pose_filtering_ros_debug.cpp @@ -1,10 +1,10 @@ #include #include -#include "ipda_pose_filtering/ros/ipda_pose_filtering_ros.hpp" +#include "pose_filtering/ros/pose_filtering_ros.hpp" namespace vortex::filtering { -void IPDAPoseFilteringNode::setup_debug_publishers() { +void PoseFilteringNode::setup_debug_publishers() { std::string meas_topic_name = this->declare_parameter("debug.topic_name_meas"); std::string state_topic_name = @@ -19,7 +19,7 @@ void IPDAPoseFilteringNode::setup_debug_publishers() { state_topic_name, qos_sensor_data); } -void IPDAPoseFilteringNode::publish_meas_debug() { +void PoseFilteringNode::publish_meas_debug() { if (measurements_.empty()) { return; } @@ -42,7 +42,7 @@ void IPDAPoseFilteringNode::publish_meas_debug() { pose_meas_debug_pub_->publish(msg); } -void IPDAPoseFilteringNode::publish_state_debug() { +void PoseFilteringNode::publish_state_debug() { if (track_manager_->get_tracks().empty()) { return; } From 10bf57c242004990108ad124458651d55af0304c Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Tue, 30 Dec 2025 14:42:07 +0100 Subject: [PATCH 14/26] tests and doxygen comments --- filtering/pose_filtering/CMakeLists.txt | 4 + .../pose_filtering/config/pose_filtering.yaml | 6 +- .../pose_filtering/lib/pose_track_manager.hpp | 81 +++++++++ .../include/pose_filtering/lib/typedefs.hpp | 78 ++++++++- .../src/lib/pose_track_manager.cpp | 4 - filtering/pose_filtering/test/CMakeLists.txt | 32 ++++ .../test/test_pose_filtering.cpp | 161 ++++++++++++++++++ 7 files changed, 357 insertions(+), 9 deletions(-) create mode 100644 filtering/pose_filtering/test/CMakeLists.txt create mode 100644 filtering/pose_filtering/test/test_pose_filtering.cpp diff --git a/filtering/pose_filtering/CMakeLists.txt b/filtering/pose_filtering/CMakeLists.txt index e3a20ab9d..cda09a550 100644 --- a/filtering/pose_filtering/CMakeLists.txt +++ b/filtering/pose_filtering/CMakeLists.txt @@ -103,4 +103,8 @@ ament_export_dependencies( 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/config/pose_filtering.yaml b/filtering/pose_filtering/config/pose_filtering.yaml index 3ff8de4ec..85ccc3d8a 100644 --- a/filtering/pose_filtering/config/pose_filtering.yaml +++ b/filtering/pose_filtering/config/pose_filtering.yaml @@ -9,13 +9,13 @@ # IPDA config ipda: - prob_of_survival: 0.99 + prob_of_survival: 0.95 estimate_clutter: false - prob_of_detection: 0.7 + 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.001 + clutter_intensity: 0.01 # Dyn model config dyn_mod: 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 index 84f09b263..1f635d008 100644 --- a/filtering/pose_filtering/include/pose_filtering/lib/pose_track_manager.hpp +++ b/filtering/pose_filtering/include/pose_filtering/lib/pose_track_manager.hpp @@ -8,44 +8,125 @@ 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 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() { return tracks_; } private: + /** + * @brief Compute angular gating for a given track against measurements. + * @param track Track to gate + * @param measurements Candidate measurements + * @return Indices of measurements passing the angular gate + */ std::vector angular_gate_measurements( const Track& track, const std::vector& measurements) 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 Build a matrix of 3D positions for a subset of measurements. + * @param measurements All measurements + * @param indices Indices selecting which measurements to include + * @return 3xN Eigen matrix with positions in columns + */ Eigen::Matrix build_position_matrix( const std::vector& measurements, const std::vector& indices) const; + /** + * @brief Collect quaternions from gated measurements and mark consumed + * indices. + * @param measurements All measurements + * @param angular_gate_indices Candidate indices that passed angular gate + * @param gated_measurements Boolean mask of gated measurements + * @param consumed_indices_out Output vector filled with indices consumed + * by this collection + * @return Vector of quaternions corresponding to consumed measurements + */ std::vector collect_used_quaternions( const std::vector& measurements, const std::vector& angular_gate_indices, const Eigen::Array& gated_measurements, std::vector& consumed_indices_out) const; + /** + * @brief Update a track's orientation using the collected quaternions. + * @param track Track to update + * @param quaternions Vector of quaternions to fuse / apply + */ void update_track_orientation( Track& track, const std::vector& quaternions); + /** + * @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, std::vector& indices); + // Internal bookkeeping int track_id_counter_ = 0; std::vector tracks_; diff --git a/filtering/pose_filtering/include/pose_filtering/lib/typedefs.hpp b/filtering/pose_filtering/include/pose_filtering/lib/typedefs.hpp index 30331942c..a072fb4ed 100644 --- a/filtering/pose_filtering/include/pose_filtering/lib/typedefs.hpp +++ b/filtering/pose_filtering/include/pose_filtering/lib/typedefs.hpp @@ -8,26 +8,72 @@ 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 state representation for 3D pose (mean + covariance). + */ using State3d = vortex::prob::Gauss3d; + +/** + * @brief Discrete-time constant dynamic model of order 3. + */ using DynMod = vortex::models::ConstantDynamicModel<3>; + +/** + * @brief Identity sensor model mapping state to measurement space. + */ using SensorMod = vortex::models::IdentitySensorModel<3, 3>; + +/** + * @brief IPDA filter type specialized for the chosen dynamic and sensor models. + */ using IPDA = vortex::filter::IPDA; + +/** + * @brief Pose type (position + quaternion) from vortex utils. + */ using Pose = vortex::utils::types::Pose; +/** + * @brief Configuration for the dynamic model (process noise standard + * deviation). + */ struct DynModConfig { double std_dev{1.0}; }; +/** + * @brief Configuration for the sensor model (measurement noise standard + * deviation). + */ struct SensorModConfig { double std_dev{1.0}; }; +/** + * @brief Parameters for existence management (track confirmation / deletion). + */ struct ExistenceManagementConfig { - double confirmation_threshold{0.4}; + double confirmation_threshold{0.6}; double deletion_threshold{0.2}; - double initial_existence_probability{0.3}; + double initial_existence_probability{0.5}; }; +/** + * @brief High-level track manager configuration struct. + * + * Contains IPDA-specific configuration and parameters for dynamics, + * sensor model, existence management and gating thresholds. + */ struct TrackManagerConfig { vortex::filter::IPDA::Config ipda; DynModConfig dyn_mod; @@ -36,19 +82,47 @@ struct TrackManagerConfig { double max_angle_gate_threshold{}; }; +/** + * @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{}; + + /// Filter state (mean + covariance) State3d state; + + /// Current orientation as quaternion (w, x, y, z) Eigen::Quaterniond current_orientation; + + /// Previous orientation (useful for angular gating / smoothing) Eigen::Quaterniond prev_orientation; + + /// 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 filter state + 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(state.mean(), current_orientation); } + /** + * @brief Compute angular distance between the track orientation and a + * measurement pose orientation. + * @param z Measurement pose to compare against + * @return Angular distance in radians (double) + */ double angular_distance(const Pose& z) const { return current_orientation.angularDistance(z.ori_quaternion()); } diff --git a/filtering/pose_filtering/src/lib/pose_track_manager.cpp b/filtering/pose_filtering/src/lib/pose_track_manager.cpp index f7beb3ff6..b7b575008 100644 --- a/filtering/pose_filtering/src/lib/pose_track_manager.cpp +++ b/filtering/pose_filtering/src/lib/pose_track_manager.cpp @@ -21,10 +21,6 @@ void PoseTrackManager::step(std::vector& measurements, double dt) { auto angular_gate_indices = angular_gate_measurements(track, measurements); - if (angular_gate_indices.empty()) { - continue; - } - Eigen::Matrix Z = build_position_matrix(measurements, angular_gate_indices); 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..423a930b8 --- /dev/null +++ b/filtering/pose_filtering/test/test_pose_filtering.cpp @@ -0,0 +1,161 @@ +#include + +#include + +#include "pose_filtering/lib/pose_track_manager.hpp" +#include "vortex/utils/types.hpp" + +namespace vortex::filtering { +using vortex::utils::types::Pose; + +class PoseTrackManagerTests : public ::testing::Test { + protected: + TrackManagerConfig make_default_config() { + TrackManagerConfig cfg{}; + cfg.dyn_mod.std_dev = 0.1; + cfg.sensor_mod.std_dev = 0.1; + + cfg.existence.initial_existence_probability = 0.5; + cfg.existence.confirmation_threshold = 0.6; + cfg.existence.deletion_threshold = 0.2; + + cfg.max_angle_gate_threshold = 0.5; // radians + + cfg.ipda.pdaf.prob_of_detection = 0.5; + cfg.ipda.pdaf.clutter_intensity = 0.01; + cfg.ipda.pdaf.mahalanobis_threshold = 1.0; + cfg.ipda.pdaf.min_gate_threshold = 0.0; + cfg.ipda.pdaf.max_gate_threshold = 1.0; + cfg.ipda.ipda.estimate_clutter = false; + + return cfg; + } + + Pose make_pose( + const Eigen::Vector3d& pos, + const Eigen::Quaterniond& q = Eigen::Quaterniond::Identity()) { + return Pose::from_eigen(pos, q); + } +}; + +TEST_F(PoseTrackManagerTests, creates_tracks_from_measurements) { + PoseTrackManager mgr(make_default_config()); + + std::vector measurements{ + make_pose({0.0, 0.0, 0.0}), + make_pose({1.0, 0.0, 0.0}), + make_pose({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.current_orientation.w(), 1.0, 1e-12); + } +} + +TEST_F(PoseTrackManagerTests, angular_gate_filters_measurements) { + auto cfg = make_default_config(); + cfg.max_angle_gate_threshold = 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_pose({0, 0, 1}, q_ref), + make_pose({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_pose({0, 0.5, 1}, q_ref), + make_pose({0, 0.5, 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_pose({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_pose({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_pose({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_pose({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 From 882f985c86c4ea057c1dd4e26c8cae493114fb1b Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Wed, 31 Dec 2025 00:00:31 +0100 Subject: [PATCH 15/26] changed alpha calculation. Added README --- filtering/pose_filtering/README.md | 171 ++++++++++++++++++ .../src/lib/pose_track_manager.cpp | 13 +- 2 files changed, 179 insertions(+), 5 deletions(-) diff --git a/filtering/pose_filtering/README.md b/filtering/pose_filtering/README.md index e69de29bb..84303f256 100644 --- a/filtering/pose_filtering/README.md +++ b/filtering/pose_filtering/README.md @@ -0,0 +1,171 @@ +# 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 (independent logic): + +- Orientation is handled separately from the IPDA position filter. + Instead of embedding orientation inside the Gaussian state, the + track stores a quaternion orientation and updates it with measurement + quaternions when appropriate. +- Angular gating: for each track the manager performs an angular gate + test that compares the current track quaternion to a measurement's + quaternion. Measurements with an angular distance above the configured + `max_angle_gate_threshold` are not associated with that track for + orientation updates. This prevents spurious orientation jumps from + outlier measurements. + +Quaternion averaging and Slerp: + +- When multiple measurements pass gating and are associated with a + track, the manager extracts the corresponding quaternions and computes + a (robust) mean orientation. The implementation collects the + measurement quaternions and derives a representative quaternion — + e.g. via a mean in quaternion space (the + implementation collects and reduces the quaternions used for that + update). +- To smoothly update the stored track orientation towards the measured + mean, the node performs a spherical linear interpolation (slerp) + between the current track quaternion q_current and the measurement + mean quaternion q_meas_mean: + + q_new = slerp(q_current, q_meas_mean, alpha) + + where `slerp(q0, q1, t)` returns the interpolation at fraction `t` + along the shortest rotation between `q0` and `q1`. + +- The `alpha` parameter controls how aggressively the orientation is + updated in a single tracking step: + - `alpha = 0.0` : keep the current orientation (no change) + - `alpha = 1.0` : instantly jump to the measurement mean + - `0.0 < alpha < 1.0` : smooth interpolation; the closer alpha is to + 1.0 the faster the track follows measurements + +### Adaptive alpha computation + +The package computes the slerp `alpha` parameter adaptively using both the current and previous quaternion stored on the track and the mean quaternion built +from the current associated measurements. + +Let +- q_k be the current track orientation +- q_{k-1} be the previous track orientation +- q̄_z be the mean quaternion of the associated measurements + +Define angular discrepancies on SO(3): + +- θ_k = d(q_k, q̄_z) +- θ_{k-1} = d(q_{k-1}, q̄_z) + +where d(·,·) is the quaternion geodesic distance (Eigen::Quaterniond::angularDistance). + +Effective measurement disagreement: + +- θ = max(θ_k, θ_{k-1}) + +Adaptive SLERP gain: + +- α = clamp( exp( -θ / σ_ori ), α_min, 1 ) + with σ_ori = 0.1 + +Orientation update: + +- q_{k+1} = slerp(q_k, q̄_z; α) + +## 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/src/lib/pose_track_manager.cpp b/filtering/pose_filtering/src/lib/pose_track_manager.cpp index b7b575008..f19a269dc 100644 --- a/filtering/pose_filtering/src/lib/pose_track_manager.cpp +++ b/filtering/pose_filtering/src/lib/pose_track_manager.cpp @@ -106,14 +106,17 @@ void PoseTrackManager::update_track_orientation( Eigen::Quaterniond avg_q = vortex::utils::math::average_quaternions(quaternions); - double d_prev = track.prev_orientation.angularDistance(avg_q); + double d_curr_meas = track.current_orientation.angularDistance(avg_q); - double d_curr = track.current_orientation.angularDistance(avg_q); + double d_prev_meas = track.prev_orientation.angularDistance(avg_q); - constexpr double eps = 1e-6; + double w_meas = std::max(d_curr_meas, d_prev_meas); - double alpha = d_prev / (d_curr + eps); - alpha = std::clamp(alpha, 0.05, 1.0); + constexpr double sigma_ori = 0.1; + + double alpha = std::exp(-w_meas / sigma_ori); + + alpha = std::clamp(alpha, 0.001, 1.0); track.prev_orientation = track.current_orientation; From b73fec7e75583446cd182ec4b621dd3e9163e11c Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Thu, 1 Jan 2026 20:40:00 +0100 Subject: [PATCH 16/26] pdaf orientation error-state --- filtering/pose_filtering/CMakeLists.txt | 1 + filtering/pose_filtering/README.md | 76 +++---------------- .../pose_filtering/config/pose_filtering.yaml | 59 ++++++++------ .../pose_filtering/lib/orientation_filter.hpp | 64 ++++++++++++++++ .../pose_filtering/lib/pose_track_manager.hpp | 15 ++-- .../include/pose_filtering/lib/typedefs.hpp | 45 ++++++++--- .../src/lib/orientation_filter.cpp | 75 ++++++++++++++++++ .../src/lib/pose_track_manager.cpp | 56 +++++--------- .../src/ros/pose_filtering_ros.cpp | 48 ++++++++---- .../src/ros/pose_filtering_ros_debug.cpp | 4 +- .../test/test_pose_filtering.cpp | 2 +- 11 files changed, 284 insertions(+), 161 deletions(-) create mode 100644 filtering/pose_filtering/include/pose_filtering/lib/orientation_filter.hpp create mode 100644 filtering/pose_filtering/src/lib/orientation_filter.cpp diff --git a/filtering/pose_filtering/CMakeLists.txt b/filtering/pose_filtering/CMakeLists.txt index cda09a550..b24eaad0e 100644 --- a/filtering/pose_filtering/CMakeLists.txt +++ b/filtering/pose_filtering/CMakeLists.txt @@ -29,6 +29,7 @@ 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} diff --git a/filtering/pose_filtering/README.md b/filtering/pose_filtering/README.md index 84303f256..63abc463f 100644 --- a/filtering/pose_filtering/README.md +++ b/filtering/pose_filtering/README.md @@ -58,74 +58,22 @@ Position (IPDA): provides robust data association for cluttered measurements and handles tracks appearing/disappearing over time. -Orientation (independent logic): +Orientation (SO(3) PDAF): -- Orientation is handled separately from the IPDA position filter. - Instead of embedding orientation inside the Gaussian state, the - track stores a quaternion orientation and updates it with measurement - quaternions when appropriate. -- Angular gating: for each track the manager performs an angular gate - test that compares the current track quaternion to a measurement's - quaternion. Measurements with an angular distance above the configured - `max_angle_gate_threshold` are not associated with that track for - orientation updates. This prevents spurious orientation jumps from - outlier measurements. +Orientation is handled by a dedicated PDAF-style filter operating on +the SO(3) manifold. Each track +maintains: -Quaternion averaging and Slerp: +- a quaternion mean q (the track orientation), and +- a small Gaussian error state in the 3D tangent space (so(3)) around + that mean. -- When multiple measurements pass gating and are associated with a - track, the manager extracts the corresponding quaternions and computes - a (robust) mean orientation. The implementation collects the - measurement quaternions and derives a representative quaternion — - e.g. via a mean in quaternion space (the - implementation collects and reduces the quaternions used for that - update). -- To smoothly update the stored track orientation towards the measured - mean, the node performs a spherical linear interpolation (slerp) - between the current track quaternion q_current and the measurement - mean quaternion q_meas_mean: - q_new = slerp(q_current, q_meas_mean, alpha) - - where `slerp(q0, q1, t)` returns the interpolation at fraction `t` - along the shortest rotation between `q0` and `q1`. - -- The `alpha` parameter controls how aggressively the orientation is - updated in a single tracking step: - - `alpha = 0.0` : keep the current orientation (no change) - - `alpha = 1.0` : instantly jump to the measurement mean - - `0.0 < alpha < 1.0` : smooth interpolation; the closer alpha is to - 1.0 the faster the track follows measurements - -### Adaptive alpha computation - -The package computes the slerp `alpha` parameter adaptively using both the current and previous quaternion stored on the track and the mean quaternion built -from the current associated measurements. - -Let -- q_k be the current track orientation -- q_{k-1} be the previous track orientation -- q̄_z be the mean quaternion of the associated measurements - -Define angular discrepancies on SO(3): - -- θ_k = d(q_k, q̄_z) -- θ_{k-1} = d(q_{k-1}, q̄_z) - -where d(·,·) is the quaternion geodesic distance (Eigen::Quaterniond::angularDistance). - -Effective measurement disagreement: - -- θ = max(θ_k, θ_{k-1}) - -Adaptive SLERP gain: - -- α = clamp( exp( -θ / σ_ori ), α_min, 1 ) - with σ_ori = 0.1 - -Orientation update: - -- q_{k+1} = slerp(q_k, q̄_z; α) +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 diff --git a/filtering/pose_filtering/config/pose_filtering.yaml b/filtering/pose_filtering/config/pose_filtering.yaml index 85ccc3d8a..9c70bc054 100644 --- a/filtering/pose_filtering/config/pose_filtering.yaml +++ b/filtering/pose_filtering/config/pose_filtering.yaml @@ -8,28 +8,43 @@ max_angle_gate_threshold: 0.4 # in radians # IPDA config - 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 - - # 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 + 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 diff --git a/filtering/pose_filtering/include/pose_filtering/lib/orientation_filter.hpp b/filtering/pose_filtering/include/pose_filtering/lib/orientation_filter.hpp new file mode 100644 index 000000000..d9b013360 --- /dev/null +++ b/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 +#include +#include +#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& 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::Config pdaf_config_; +}; + +} // namespace vortex::filtering + +#endif // POSE_FILTERING__LIB__ORIENTATION_FILTER_HPP_ 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 index 1f635d008..520530861 100644 --- a/filtering/pose_filtering/include/pose_filtering/lib/pose_track_manager.hpp +++ b/filtering/pose_filtering/include/pose_filtering/lib/pose_track_manager.hpp @@ -4,6 +4,7 @@ #include #include #include +#include "orientation_filter.hpp" #include "typedefs.hpp" namespace vortex::filtering { @@ -109,15 +110,6 @@ class PoseTrackManager { const Eigen::Array& gated_measurements, std::vector& consumed_indices_out) const; - /** - * @brief Update a track's orientation using the collected quaternions. - * @param track Track to update - * @param quaternions Vector of quaternions to fuse / apply - */ - void update_track_orientation( - Track& track, - const std::vector& quaternions); - /** * @brief Erase measurements which have been gated / consumed. * @param measurements Measurements vector (modified in place) @@ -140,6 +132,11 @@ class PoseTrackManager { ExistenceManagementConfig existence_config_; double max_angle_gate_threshold_{}; + + double initial_position_std_{}; + double initial_orientation_std_{}; + + OrientationFilter orientation_filter_; }; } // namespace vortex::filtering diff --git a/filtering/pose_filtering/include/pose_filtering/lib/typedefs.hpp b/filtering/pose_filtering/include/pose_filtering/lib/typedefs.hpp index a072fb4ed..e4ca49fa0 100644 --- a/filtering/pose_filtering/include/pose_filtering/lib/typedefs.hpp +++ b/filtering/pose_filtering/include/pose_filtering/lib/typedefs.hpp @@ -19,7 +19,7 @@ namespace vortex::filtering { */ /** - * @brief Gaussian state representation for 3D pose (mean + covariance). + * @brief Gaussian 3d state representation (mean + covariance). */ using State3d = vortex::prob::Gauss3d; @@ -38,6 +38,11 @@ using SensorMod = vortex::models::IdentitySensorModel<3, 3>; */ 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. */ @@ -68,6 +73,15 @@ struct ExistenceManagementConfig { double initial_existence_probability{0.5}; }; +/** + * @brief Configuration struct for the orientation filter. + */ +struct OrientationFilterConfig { + vortex::filter::PDAF::Config pdaf; + DynModConfig dyn_mod; + SensorModConfig sensor_mod; +}; + /** * @brief High-level track manager configuration struct. * @@ -76,10 +90,21 @@ struct ExistenceManagementConfig { */ struct TrackManagerConfig { vortex::filter::IPDA::Config ipda; + double initial_position_std{1.0}; + double initial_orientation_std{1.0}; DynModConfig dyn_mod; SensorModConfig sensor_mod; ExistenceManagementConfig existence; double max_angle_gate_threshold{}; + OrientationFilterConfig ori; +}; + +/** + * @brief Orientation filter state representation. + */ +struct OrientationState { + Eigen::Quaterniond q; + State3d error_state; }; /** @@ -92,14 +117,10 @@ struct Track { /// Unique track identifier int id{}; - /// Filter state (mean + covariance) - State3d state; - - /// Current orientation as quaternion (w, x, y, z) - Eigen::Quaterniond current_orientation; + /// Filter position state (mean + covariance) + State3d state_pos; - /// Previous orientation (useful for angular gating / smoothing) - Eigen::Quaterniond prev_orientation; + OrientationState orientation_filter; /// Probability that the track exists (0..1) double existence_probability{0.0}; @@ -108,13 +129,13 @@ struct Track { bool confirmed{false}; /** - * @brief Convert the filter state + orientation to a Pose object. + * @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(state.mean(), - current_orientation); + return vortex::utils::types::Pose::from_eigen(state_pos.mean(), + orientation_filter.q); } /** @@ -124,7 +145,7 @@ struct Track { * @return Angular distance in radians (double) */ double angular_distance(const Pose& z) const { - return current_orientation.angularDistance(z.ori_quaternion()); + return orientation_filter.q.angularDistance(z.ori_quaternion()); } }; diff --git a/filtering/pose_filtering/src/lib/orientation_filter.cpp b/filtering/pose_filtering/src/lib/orientation_filter.cpp new file mode 100644 index 000000000..7bfd3ac25 --- /dev/null +++ b/filtering/pose_filtering/src/lib/orientation_filter.cpp @@ -0,0 +1,75 @@ +#include "pose_filtering/lib/orientation_filter.hpp" +#include +#include + +namespace vortex::filtering { + +OrientationFilter::OrientationFilter(const OrientationFilterConfig& cfg) + : dyn_mod_(cfg.dyn_mod.std_dev), + sensor_mod_(cfg.sensor_mod.std_dev), + pdaf_config_(cfg.pdaf) {} + +void OrientationFilter::step( + const std::vector& measurements, + double dt, + OrientationState& state) { + if (measurements.empty()) { + auto prediction_result = + PDAF::predict(dyn_mod_, sensor_mod_, dt, state.error_state); + state.error_state = prediction_result.x_pred; + return; + } + + Eigen::Matrix Z(3, measurements.size()); + for (size_t i = 0; i < measurements.size(); ++i) { + Z.col(i) = so3_log_quat(measurements.at(i) * state.q.conjugate()); + } + + auto result = PDAF::step(dyn_mod_, sensor_mod_, dt, state.error_state, Z, + pdaf_config_); + + Eigen::Vector3d delta = result.x_post.mean(); + state.q = so3_exp_quat(delta) * state.q; + state.q.normalize(); + + state.error_state.mean().setZero(); + state.error_state.cov() = result.x_post.cov(); +} + +Eigen::Vector3d OrientationFilter::so3_log_quat( + const Eigen::Quaterniond& q_in) { + Eigen::Quaterniond q = q_in.normalized(); + + if (q.w() < 0.0) { + q.coeffs() *= -1.0; + } + + double norm_v = q.vec().norm(); + + if (norm_v < 1e-6) { + return 2.0 * q.vec(); + } + + double theta = 2.0 * std::atan2(norm_v, q.w()); + return theta * q.vec() / norm_v; +} + +Eigen::Quaterniond OrientationFilter::so3_exp_quat( + const Eigen::Vector3d& rvec) { + 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)); +} + +} // namespace vortex::filtering diff --git a/filtering/pose_filtering/src/lib/pose_track_manager.cpp b/filtering/pose_filtering/src/lib/pose_track_manager.cpp index f19a269dc..1fbfb4ed9 100644 --- a/filtering/pose_filtering/src/lib/pose_track_manager.cpp +++ b/filtering/pose_filtering/src/lib/pose_track_manager.cpp @@ -2,13 +2,17 @@ #include #include #include +#include "pose_filtering/lib/orientation_filter.hpp" namespace vortex::filtering { PoseTrackManager::PoseTrackManager(const TrackManagerConfig& config) : track_id_counter_(0), dyn_mod_(config.dyn_mod.std_dev), - sensor_mod_(config.sensor_mod.std_dev) { + sensor_mod_(config.sensor_mod.std_dev), + orientation_filter_(OrientationFilter(config.ori)) { + initial_position_std_ = config.initial_position_std; + initial_orientation_std_ = config.initial_orientation_std; ipda_config_ = config.ipda; existence_config_ = config.existence; max_angle_gate_threshold_ = config.max_angle_gate_threshold; @@ -25,13 +29,13 @@ void PoseTrackManager::step(std::vector& measurements, double dt) { build_position_matrix(measurements, angular_gate_indices); const IPDA::State state_est_prev{ - .x_estimate = track.state, + .x_estimate = track.state_pos, .existence_probability = track.existence_probability}; auto ipda_output = IPDA::step(dyn_mod_, sensor_mod_, dt, state_est_prev, Z, ipda_config_); - track.state = ipda_output.state.x_estimate; + track.state_pos = ipda_output.state.x_estimate; track.existence_probability = ipda_output.state.existence_probability; std::vector consumed_indices; @@ -39,7 +43,8 @@ void PoseTrackManager::step(std::vector& measurements, double dt) { measurements, angular_gate_indices, ipda_output.gated_measurements, consumed_indices); - update_track_orientation(track, used_quaternions); + orientation_filter_.step(used_quaternions, dt, + track.orientation_filter); erase_gated_measurements(measurements, consumed_indices); } @@ -96,33 +101,6 @@ std::vector PoseTrackManager::collect_used_quaternions( return quats; } -void PoseTrackManager::update_track_orientation( - Track& track, - const std::vector& quaternions) { - if (quaternions.empty()) { - return; - } - - Eigen::Quaterniond avg_q = - vortex::utils::math::average_quaternions(quaternions); - - double d_curr_meas = track.current_orientation.angularDistance(avg_q); - - double d_prev_meas = track.prev_orientation.angularDistance(avg_q); - - double w_meas = std::max(d_curr_meas, d_prev_meas); - - constexpr double sigma_ori = 0.1; - - double alpha = std::exp(-w_meas / sigma_ori); - - alpha = std::clamp(alpha, 0.001, 1.0); - - track.prev_orientation = track.current_orientation; - - track.current_orientation = track.current_orientation.slerp(alpha, avg_q); -} - void PoseTrackManager::erase_gated_measurements( std::vector& measurements, std::vector& indices) { @@ -137,11 +115,17 @@ void PoseTrackManager::create_tracks(const std::vector& measurements) { auto make_track = [this](const Pose& measurement) { return Track{.id = track_id_counter_++, - .state = vortex::prob::Gauss3d( - measurement.pos_vector(), - Eigen::Matrix::Identity()), - .current_orientation = measurement.ori_quaternion(), - .prev_orientation = measurement.ori_quaternion(), + .state_pos = vortex::prob::Gauss3d( + measurement.pos_vector(), Eigen::Matrix3d::Identity() * + initial_position_std_ * + initial_position_std_), + .orientation_filter = + OrientationState{.q = measurement.ori_quaternion(), + .error_state = State3d( + Eigen::Vector3d::Zero(), + Eigen::Matrix3d::Identity() * + initial_orientation_std_ * + initial_orientation_std_)}, .existence_probability = existence_config_.initial_existence_probability, .confirmed = false}; diff --git a/filtering/pose_filtering/src/ros/pose_filtering_ros.cpp b/filtering/pose_filtering/src/ros/pose_filtering_ros.cpp index ef6925324..767773d35 100644 --- a/filtering/pose_filtering/src/ros/pose_filtering_ros.cpp +++ b/filtering/pose_filtering/src/ros/pose_filtering_ros.cpp @@ -96,36 +96,54 @@ void PoseFilteringNode::setup_track_manager() { TrackManagerConfig config; config.ipda.ipda.prob_of_survival = - this->declare_parameter("ipda.prob_of_survival"); + this->declare_parameter("position.ipda.prob_of_survival"); config.ipda.ipda.estimate_clutter = - this->declare_parameter("ipda.estimate_clutter"); + this->declare_parameter("position.ipda.estimate_clutter"); config.ipda.pdaf.prob_of_detection = - this->declare_parameter("ipda.prob_of_detection"); - config.ipda.pdaf.mahalanobis_threshold = - this->declare_parameter("ipda.mahalanobis_gate_threshold"); + this->declare_parameter("position.ipda.prob_of_detection"); + config.ipda.pdaf.mahalanobis_threshold = this->declare_parameter( + "position.ipda.mahalanobis_gate_threshold"); config.ipda.pdaf.min_gate_threshold = - this->declare_parameter("ipda.min_gate_threshold"); + this->declare_parameter("position.ipda.min_gate_threshold"); config.ipda.pdaf.max_gate_threshold = - this->declare_parameter("ipda.max_gate_threshold"); + this->declare_parameter("position.ipda.max_gate_threshold"); config.ipda.pdaf.clutter_intensity = - this->declare_parameter("ipda.clutter_intensity"); + this->declare_parameter("position.ipda.clutter_intensity"); - config.dyn_mod.std_dev = this->declare_parameter("dyn_mod.std_dev"); + config.initial_position_std = + this->declare_parameter("position.initial_position_std"); + config.initial_orientation_std = + this->declare_parameter("orientation.initial_orientation_std"); + + config.dyn_mod.std_dev = + this->declare_parameter("position.dyn_mod.std_dev"); config.sensor_mod.std_dev = - this->declare_parameter("sensor_mod.std_dev"); + this->declare_parameter("position.sensor_mod.std_dev"); config.max_angle_gate_threshold = this->declare_parameter("max_angle_gate_threshold"); - config.existence.confirmation_threshold = - this->declare_parameter("existence.confirmation_threshold"); - config.existence.deletion_threshold = - this->declare_parameter("existence.deletion_threshold"); + config.existence.confirmation_threshold = this->declare_parameter( + "position.existence.confirmation_threshold"); + config.existence.deletion_threshold = this->declare_parameter( + "position.existence.deletion_threshold"); config.existence.initial_existence_probability = this->declare_parameter( - "existence.initial_existence_probability"); + "position.existence.initial_existence_probability"); + + config.ori.pdaf.pdaf.prob_of_detection = + this->declare_parameter("orientation.pdaf.prob_of_detection"); + config.ori.pdaf.pdaf.clutter_intensity = + this->declare_parameter("orientation.pdaf.clutter_intensity"); + config.ori.pdaf.pdaf.mahalanobis_threshold = + this->declare_parameter( + "orientation.pdaf.mahalanobis_gate_threshold"); + config.ori.dyn_mod.std_dev = + this->declare_parameter("orientation.dyn_mod.std_dev"); + config.ori.sensor_mod.std_dev = + this->declare_parameter("orientation.sensor_mod.std_dev"); track_manager_ = std::make_unique(config); } diff --git a/filtering/pose_filtering/src/ros/pose_filtering_ros_debug.cpp b/filtering/pose_filtering/src/ros/pose_filtering_ros_debug.cpp index ea5773d93..7f0f481d3 100644 --- a/filtering/pose_filtering/src/ros/pose_filtering_ros_debug.cpp +++ b/filtering/pose_filtering/src/ros/pose_filtering_ros_debug.cpp @@ -47,8 +47,8 @@ void PoseFilteringNode::publish_state_debug() { return; } Track track = track_manager_->get_tracks().at(0); - Eigen::Vector3d pos = track.state.mean(); - Eigen::Quaterniond quat = track.current_orientation; + Eigen::Vector3d pos = track.state_pos.mean(); + Eigen::Quaterniond quat = track.orientation_filter.q; Eigen::Vector3d euler = vortex::utils::math::quat_to_euler(quat); diff --git a/filtering/pose_filtering/test/test_pose_filtering.cpp b/filtering/pose_filtering/test/test_pose_filtering.cpp index 423a930b8..df5e25369 100644 --- a/filtering/pose_filtering/test/test_pose_filtering.cpp +++ b/filtering/pose_filtering/test/test_pose_filtering.cpp @@ -55,7 +55,7 @@ TEST_F(PoseTrackManagerTests, creates_tracks_from_measurements) { for (const auto& t : tracks) { EXPECT_FALSE(t.confirmed); EXPECT_NEAR(t.existence_probability, 0.5, 1e-12); - EXPECT_NEAR(t.current_orientation.w(), 1.0, 1e-12); + EXPECT_NEAR(t.orientation_filter.q.w(), 1.0, 1e-12); } } From aeac20c37c7988fdd95f0f9f4df73e7fc8133f8a Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Tue, 6 Jan 2026 10:21:20 +0100 Subject: [PATCH 17/26] added landmark array publisher --- .../pose_filtering/config/pose_filtering.yaml | 3 +- .../pose_filtering/ros/pose_filtering_ros.hpp | 5 +- .../ros/pose_filtering_ros_conversions.hpp | 36 ++++++++++ .../src/ros/pose_filtering_ros.cpp | 18 +++++ .../ros/pose_filtering_ros_conversions.cpp | 66 +++++++++++++++++++ 5 files changed, 126 insertions(+), 2 deletions(-) create mode 100644 filtering/pose_filtering/include/pose_filtering/ros/pose_filtering_ros_conversions.hpp create mode 100644 filtering/pose_filtering/src/ros/pose_filtering_ros_conversions.cpp diff --git a/filtering/pose_filtering/config/pose_filtering.yaml b/filtering/pose_filtering/config/pose_filtering.yaml index 9c70bc054..18fcbc721 100644 --- a/filtering/pose_filtering/config/pose_filtering.yaml +++ b/filtering/pose_filtering/config/pose_filtering.yaml @@ -1,7 +1,8 @@ /**: ros__parameters: pose_sub_topic: "/aruco_detector/board" - pose_array_pub_topic: "filtered_pose_array" + pose_array_pub_topic: "/filtered_pose_array" + landmark_pub_topic: "/filtered_landmarks" target_frame: "odom" timer_rate_ms: 100 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 index 5159149b8..72ffb1a07 100644 --- a/filtering/pose_filtering/include/pose_filtering/ros/pose_filtering_ros.hpp +++ b/filtering/pose_filtering/include/pose_filtering/ros/pose_filtering_ros.hpp @@ -23,7 +23,7 @@ namespace vortex::filtering { -using PoseMsgT = vortex_msgs::msg::LandmarkArray; +using PoseMsgT = geometry_msgs::msg::PoseStamped; template concept ValidPoseMsg = @@ -61,6 +61,9 @@ class PoseFilteringNode : public rclcpp::Node { rclcpp::Publisher::SharedPtr pose_array_pub_; + rclcpp::Publisher::SharedPtr + landmark_array_pub_; + rclcpp::TimerBase::SharedPtr pub_timer_; double filter_dt_seconds_{0.0}; 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..4ac2de9d1 --- /dev/null +++ b/filtering/pose_filtering/include/pose_filtering/ros/pose_filtering_ros_conversions.hpp @@ -0,0 +1,36 @@ +#ifndef POSE_FILTERING__ROS__POSE_FILTERING_ROS_CONVERSIONS_HPP_ +#define POSE_FILTERING__ROS__POSE_FILTERING_ROS_CONVERSIONS_HPP_ + +#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); + +} // namespace vortex::filtering::ros_conversions + +#endif // POSE_FILTERING__ROS__POSE_FILTERING_ROS_CONVERSIONS_HPP_ diff --git a/filtering/pose_filtering/src/ros/pose_filtering_ros.cpp b/filtering/pose_filtering/src/ros/pose_filtering_ros.cpp index 767773d35..0f58e1e14 100644 --- a/filtering/pose_filtering/src/ros/pose_filtering_ros.cpp +++ b/filtering/pose_filtering/src/ros/pose_filtering_ros.cpp @@ -3,6 +3,7 @@ #include #include #include +#include "pose_filtering_ros_conversions.hpp" #include "vortex/utils/ros/ros_conversions.hpp" #include "vortex/utils/ros/ros_transforms.hpp" @@ -23,9 +24,16 @@ void PoseFilteringNode::setup_publishers_and_subscribers() { 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); + int timer_rate_ms = this->declare_parameter("timer_rate_ms"); filter_dt_seconds_ = static_cast(timer_rate_ms) / 1000; @@ -73,6 +81,10 @@ void PoseFilteringNode::create_pose_subscription( filter->registerCallback([this]( const typename PoseMsgT::ConstSharedPtr msg) { + Eigen::Quaterniond quat( + msg->pose.orientation.w, msg->pose.orientation.x, + msg->pose.orientation.y, msg->pose.orientation.z); + Eigen::Vector3d rpy = vortex::utils::math::quat_to_euler(quat); PoseMsgT pose_tf; try { vortex::utils::ros_transforms::transform_pose( @@ -171,6 +183,12 @@ void PoseFilteringNode::timer_callback() { 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); 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..dd0eb0401 --- /dev/null +++ b/filtering/pose_filtering/src/ros/pose_filtering_ros_conversions.cpp @@ -0,0 +1,66 @@ +#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.state_pos.mean()(0); + msg.pose.position.y = track.state_pos.mean()(1); + msg.pose.position.z = track.state_pos.mean()(2); + + msg.pose.orientation.w = track.orientation_filter.q.w(); + msg.pose.orientation.x = track.orientation_filter.q.x(); + msg.pose.orientation.y = track.orientation_filter.q.y(); + msg.pose.orientation.z = track.orientation_filter.q.z(); + + msg.covariance.fill(0.0); + + const auto& Pp = track.state_pos.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 + + const auto& Po = track.orientation_filter.error_state.cov(); + msg.covariance[3 * 6 + 3] = Po(0, 0); // RR + msg.covariance[3 * 6 + 4] = Po(0, 1); // RP + msg.covariance[3 * 6 + 5] = Po(0, 2); // RY + + msg.covariance[4 * 6 + 3] = Po(1, 0); // PR + msg.covariance[4 * 6 + 4] = Po(1, 1); // PP + msg.covariance[4 * 6 + 5] = Po(1, 2); // PY + + msg.covariance[5 * 6 + 3] = Po(2, 0); // YR + msg.covariance[5 * 6 + 4] = Po(2, 1); // YP + msg.covariance[5 * 6 + 5] = Po(2, 2); // 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_array_msg.landmarks.push_back(landmark); + } + + return landmark_array_msg; +} + +} // namespace vortex::filtering::ros_conversions From 65e31a6ad2f55d9b7ac766a2252a69f3c05bfcdf Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Tue, 6 Jan 2026 11:11:21 +0100 Subject: [PATCH 18/26] added landmarkarray pub --- filtering/pose_filtering/CMakeLists.txt | 1 + filtering/pose_filtering/launch/pose_filtering.launch.py | 2 +- filtering/pose_filtering/src/ros/pose_filtering_ros.cpp | 6 +----- 3 files changed, 3 insertions(+), 6 deletions(-) diff --git a/filtering/pose_filtering/CMakeLists.txt b/filtering/pose_filtering/CMakeLists.txt index b24eaad0e..b1bcf15d1 100644 --- a/filtering/pose_filtering/CMakeLists.txt +++ b/filtering/pose_filtering/CMakeLists.txt @@ -51,6 +51,7 @@ 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} diff --git a/filtering/pose_filtering/launch/pose_filtering.launch.py b/filtering/pose_filtering/launch/pose_filtering.launch.py index 85c2adb2f..c1deccca4 100644 --- a/filtering/pose_filtering/launch/pose_filtering.launch.py +++ b/filtering/pose_filtering/launch/pose_filtering.launch.py @@ -19,7 +19,7 @@ def generate_launch_description(): executable='pose_filtering_node', name='pose_filtering', output='screen', - parameters=[config, {'use_sim_time': False}], + parameters=[config, {'use_sim_time': True}], ), ] ) diff --git a/filtering/pose_filtering/src/ros/pose_filtering_ros.cpp b/filtering/pose_filtering/src/ros/pose_filtering_ros.cpp index 0f58e1e14..bef6bec3e 100644 --- a/filtering/pose_filtering/src/ros/pose_filtering_ros.cpp +++ b/filtering/pose_filtering/src/ros/pose_filtering_ros.cpp @@ -3,7 +3,7 @@ #include #include #include -#include "pose_filtering_ros_conversions.hpp" +#include "pose_filtering/ros/pose_filtering_ros_conversions.hpp" #include "vortex/utils/ros/ros_conversions.hpp" #include "vortex/utils/ros/ros_transforms.hpp" @@ -81,10 +81,6 @@ void PoseFilteringNode::create_pose_subscription( filter->registerCallback([this]( const typename PoseMsgT::ConstSharedPtr msg) { - Eigen::Quaterniond quat( - msg->pose.orientation.w, msg->pose.orientation.x, - msg->pose.orientation.y, msg->pose.orientation.z); - Eigen::Vector3d rpy = vortex::utils::math::quat_to_euler(quat); PoseMsgT pose_tf; try { vortex::utils::ros_transforms::transform_pose( From 610210abd6c480efe8e322fa432d00980dee7831 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Tue, 6 Jan 2026 17:33:11 +0100 Subject: [PATCH 19/26] added enu-ned rotation --- filtering/pose_filtering/config/pose_filtering.yaml | 1 + .../include/pose_filtering/ros/pose_filtering_ros.hpp | 1 + filtering/pose_filtering/src/ros/pose_filtering_ros.cpp | 7 +++++++ 3 files changed, 9 insertions(+) diff --git a/filtering/pose_filtering/config/pose_filtering.yaml b/filtering/pose_filtering/config/pose_filtering.yaml index 18fcbc721..98f99a294 100644 --- a/filtering/pose_filtering/config/pose_filtering.yaml +++ b/filtering/pose_filtering/config/pose_filtering.yaml @@ -5,6 +5,7 @@ landmark_pub_topic: "/filtered_landmarks" target_frame: "odom" timer_rate_ms: 100 + enu_ned_rotation: true max_angle_gate_threshold: 0.4 # in radians 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 index 72ffb1a07..444d089ba 100644 --- a/filtering/pose_filtering/include/pose_filtering/ros/pose_filtering_ros.hpp +++ b/filtering/pose_filtering/include/pose_filtering/ros/pose_filtering_ros.hpp @@ -70,6 +70,7 @@ class PoseFilteringNode : public rclcpp::Node { std::unique_ptr track_manager_; std::vector measurements_; + bool enu_ned_rotation_{false}; bool debug_{false}; rclcpp::Publisher::SharedPtr diff --git a/filtering/pose_filtering/src/ros/pose_filtering_ros.cpp b/filtering/pose_filtering/src/ros/pose_filtering_ros.cpp index bef6bec3e..9d2bbde45 100644 --- a/filtering/pose_filtering/src/ros/pose_filtering_ros.cpp +++ b/filtering/pose_filtering/src/ros/pose_filtering_ros.cpp @@ -19,6 +19,7 @@ PoseFilteringNode::PoseFilteringNode(const rclcpp::NodeOptions& options) } 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 = @@ -94,6 +95,12 @@ void PoseFilteringNode::create_pose_subscription( this->measurements_ = vortex::utils::ros_conversions::ros_to_pose_vec(pose_tf); + if (enu_ned_rotation_) { + std::ranges::for_each(this->measurements_, [](auto& m) { + m.set_ori( + vortex::utils::math::enu_ned_rotation(m.ori_quaternion())); + }); + } }); subscriber_ = sub; From 5fee31b044284ab7a0cf6f205672b6dbfe2d236a Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Fri, 9 Jan 2026 11:13:23 +0100 Subject: [PATCH 20/26] pr fixes --- .../pose_filtering/lib/pose_track_manager.hpp | 4 ++-- .../pose_filtering/ros/pose_filtering_ros.hpp | 9 ++++++--- .../pose_filtering/launch/pose_filtering.launch.py | 5 ++++- .../pose_filtering/src/lib/orientation_filter.cpp | 14 ++++++++++---- 4 files changed, 22 insertions(+), 10 deletions(-) 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 index 520530861..18573b6b5 100644 --- a/filtering/pose_filtering/include/pose_filtering/lib/pose_track_manager.hpp +++ b/filtering/pose_filtering/include/pose_filtering/lib/pose_track_manager.hpp @@ -50,7 +50,7 @@ class PoseTrackManager { * @brief Get the list of currently maintained tracks. * @return const reference to internal track vector */ - const std::vector& get_tracks() { return tracks_; } + const std::vector& get_tracks() const { return tracks_; } private: /** @@ -119,7 +119,7 @@ class PoseTrackManager { std::vector& indices); // Internal bookkeeping - int track_id_counter_ = 0; + int track_id_counter_{0}; std::vector tracks_; 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 index 444d089ba..aa1709e5a 100644 --- a/filtering/pose_filtering/include/pose_filtering/ros/pose_filtering_ros.hpp +++ b/filtering/pose_filtering/include/pose_filtering/ros/pose_filtering_ros.hpp @@ -51,6 +51,12 @@ class PoseFilteringNode : public rclcpp::Node { void timer_callback(); + void setup_debug_publishers(); + + void publish_meas_debug(); + + void publish_state_debug(); + std::shared_ptr> subscriber_; std::shared_ptr> tf_filter_; @@ -77,9 +83,6 @@ class PoseFilteringNode : public rclcpp::Node { pose_meas_debug_pub_; rclcpp::Publisher::SharedPtr pose_state_debug_pub_; - void setup_debug_publishers(); - void publish_meas_debug(); - void publish_state_debug(); }; } // namespace vortex::filtering diff --git a/filtering/pose_filtering/launch/pose_filtering.launch.py b/filtering/pose_filtering/launch/pose_filtering.launch.py index c1deccca4..b24870a6d 100644 --- a/filtering/pose_filtering/launch/pose_filtering.launch.py +++ b/filtering/pose_filtering/launch/pose_filtering.launch.py @@ -19,7 +19,10 @@ def generate_launch_description(): executable='pose_filtering_node', name='pose_filtering', output='screen', - parameters=[config, {'use_sim_time': True}], + parameters=[ + config, + # {'use_sim_time': True} # If testing with rosbags sim_time might be preferred if bag is looped + ], ), ] ) diff --git a/filtering/pose_filtering/src/lib/orientation_filter.cpp b/filtering/pose_filtering/src/lib/orientation_filter.cpp index 7bfd3ac25..2f3d7944d 100644 --- a/filtering/pose_filtering/src/lib/orientation_filter.cpp +++ b/filtering/pose_filtering/src/lib/orientation_filter.cpp @@ -20,13 +20,15 @@ void OrientationFilter::step( return; } - Eigen::Matrix Z(3, measurements.size()); + Eigen::Matrix measurements_error_space( + 3, measurements.size()); for (size_t i = 0; i < measurements.size(); ++i) { - Z.col(i) = so3_log_quat(measurements.at(i) * state.q.conjugate()); + measurements_error_space.col(i) = + so3_log_quat(measurements.at(i) * state.q.conjugate()); } - auto result = PDAF::step(dyn_mod_, sensor_mod_, dt, state.error_state, Z, - pdaf_config_); + auto result = PDAF::step(dyn_mod_, sensor_mod_, dt, state.error_state, + measurements_error_space, pdaf_config_); Eigen::Vector3d delta = result.x_post.mean(); state.q = so3_exp_quat(delta) * state.q; @@ -41,12 +43,16 @@ Eigen::Vector3d OrientationFilter::so3_log_quat( 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(); } From 4072a26dc07313d36adabcbd4edd2b401fb5f997 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Fri, 9 Jan 2026 11:48:27 +0100 Subject: [PATCH 21/26] removed unused stuff. Praise clangd --- .../include/pose_filtering/lib/pose_track_manager.hpp | 1 - filtering/pose_filtering/src/lib/orientation_filter.cpp | 1 - filtering/pose_filtering/src/ros/pose_filtering_ros.cpp | 5 ----- 3 files changed, 7 deletions(-) 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 index 18573b6b5..ccc07d33d 100644 --- a/filtering/pose_filtering/include/pose_filtering/lib/pose_track_manager.hpp +++ b/filtering/pose_filtering/include/pose_filtering/lib/pose_track_manager.hpp @@ -1,7 +1,6 @@ #ifndef POSE_FILTERING__LIB__POSE_TRACK_MANAGER_HPP_ #define POSE_FILTERING__LIB__POSE_TRACK_MANAGER_HPP_ -#include #include #include #include "orientation_filter.hpp" diff --git a/filtering/pose_filtering/src/lib/orientation_filter.cpp b/filtering/pose_filtering/src/lib/orientation_filter.cpp index 2f3d7944d..c8119c76c 100644 --- a/filtering/pose_filtering/src/lib/orientation_filter.cpp +++ b/filtering/pose_filtering/src/lib/orientation_filter.cpp @@ -1,5 +1,4 @@ #include "pose_filtering/lib/orientation_filter.hpp" -#include #include 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 index 9d2bbde45..e12c8dd09 100644 --- a/filtering/pose_filtering/src/ros/pose_filtering_ros.cpp +++ b/filtering/pose_filtering/src/ros/pose_filtering_ros.cpp @@ -7,9 +7,6 @@ #include "vortex/utils/ros/ros_conversions.hpp" #include "vortex/utils/ros/ros_transforms.hpp" -using std::placeholders::_1; -using std::placeholders::_2; - namespace vortex::filtering { PoseFilteringNode::PoseFilteringNode(const rclcpp::NodeOptions& options) @@ -45,8 +42,6 @@ void PoseFilteringNode::setup_publishers_and_subscribers() { target_frame_ = this->declare_parameter("target_frame"); - std::chrono::duration buffer_timeout(1); - tf2_buffer_ = std::make_shared(this->get_clock()); auto timer_interface = std::make_shared( From 6877e66006de041263e8e8479a066734aaf060ca Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Sat, 14 Feb 2026 13:04:45 +0100 Subject: [PATCH 22/26] landmark filtering refactor --- filtering/pose_filtering/CMakeLists.txt | 1 - .../pose_filtering/config/pose_filtering.yaml | 51 ++-- .../pose_filtering/lib/orientation_filter.hpp | 64 ----- .../pose_filtering/lib/pose_track_manager.hpp | 202 +++++++++++---- .../include/pose_filtering/lib/typedefs.hpp | 119 +++++---- .../pose_filtering/ros/pose_filtering_ros.hpp | 7 +- .../ros/pose_filtering_ros_conversions.hpp | 27 ++ .../launch/pose_filtering.launch.py | 4 +- .../src/lib/orientation_filter.cpp | 80 ------ .../src/lib/pose_track_manager.cpp | 234 ++++++++++++------ .../src/ros/pose_filtering_ros.cpp | 136 +++++----- .../ros/pose_filtering_ros_conversions.cpp | 95 +++++-- .../src/ros/pose_filtering_ros_debug.cpp | 15 +- .../test/test_pose_filtering.cpp | 50 ++-- 14 files changed, 584 insertions(+), 501 deletions(-) delete mode 100644 filtering/pose_filtering/include/pose_filtering/lib/orientation_filter.hpp delete mode 100644 filtering/pose_filtering/src/lib/orientation_filter.cpp diff --git a/filtering/pose_filtering/CMakeLists.txt b/filtering/pose_filtering/CMakeLists.txt index b1bcf15d1..4a9a2cb87 100644 --- a/filtering/pose_filtering/CMakeLists.txt +++ b/filtering/pose_filtering/CMakeLists.txt @@ -29,7 +29,6 @@ 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} diff --git a/filtering/pose_filtering/config/pose_filtering.yaml b/filtering/pose_filtering/config/pose_filtering.yaml index 98f99a294..02993427b 100644 --- a/filtering/pose_filtering/config/pose_filtering.yaml +++ b/filtering/pose_filtering/config/pose_filtering.yaml @@ -4,49 +4,32 @@ pose_array_pub_topic: "/filtered_pose_array" landmark_pub_topic: "/filtered_landmarks" target_frame: "odom" - timer_rate_ms: 100 + timer_rate_ms: 200 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 + 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 + 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 - dyn_mod: - std_dev: 0.05 + init_pos_std_dev: 0.1 + init_ori_std_dev: 0.05 - sensor_mod: - 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 diff --git a/filtering/pose_filtering/include/pose_filtering/lib/orientation_filter.hpp b/filtering/pose_filtering/include/pose_filtering/lib/orientation_filter.hpp deleted file mode 100644 index d9b013360..000000000 --- a/filtering/pose_filtering/include/pose_filtering/lib/orientation_filter.hpp +++ /dev/null @@ -1,64 +0,0 @@ -#ifndef POSE_FILTERING__LIB__ORIENTATION_FILTER_HPP_ -#define POSE_FILTERING__LIB__ORIENTATION_FILTER_HPP_ - -#include -#include -#include -#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& 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::Config pdaf_config_; -}; - -} // namespace vortex::filtering - -#endif // POSE_FILTERING__LIB__ORIENTATION_FILTER_HPP_ 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 index ccc07d33d..415db0617 100644 --- a/filtering/pose_filtering/include/pose_filtering/lib/pose_track_manager.hpp +++ b/filtering/pose_filtering/include/pose_filtering/lib/pose_track_manager.hpp @@ -1,9 +1,9 @@ #ifndef POSE_FILTERING__LIB__POSE_TRACK_MANAGER_HPP_ #define POSE_FILTERING__LIB__POSE_TRACK_MANAGER_HPP_ +#include #include #include -#include "orientation_filter.hpp" #include "typedefs.hpp" namespace vortex::filtering { @@ -18,6 +18,40 @@ namespace vortex::filtering { * 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. @@ -43,7 +77,7 @@ class PoseTrackManager { * the manager to remove consumed measurements) * @param dt Time step in seconds */ - void step(std::vector& measurements, double dt); + void step(std::vector& measurements, double dt); /** * @brief Get the list of currently maintained tracks. @@ -51,16 +85,94 @@ class PoseTrackManager { */ 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)) { + continue; + } + out.push_back(&t); + } + return out; + } + private: /** - * @brief Compute angular gating for a given track against measurements. - * @param track Track to gate - * @param measurements Candidate measurements - * @return Indices of measurements passing the angular gate + * @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 */ - std::vector angular_gate_measurements( + Eigen::Array compute_measurement_residuals( const Track& track, - const std::vector& measurements) const; + 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. @@ -76,7 +188,7 @@ class PoseTrackManager { * @brief Create new tracks from unassociated measurements. * @param measurements Measurements to be used for new track creation */ - void create_tracks(const std::vector& measurements); + void create_tracks(const std::vector& measurements); /** * @brief Sort tracks by priority to determine processing / deletion order. @@ -84,58 +196,52 @@ class PoseTrackManager { void sort_tracks_by_priority(); /** - * @brief Build a matrix of 3D positions for a subset of measurements. - * @param measurements All measurements - * @param indices Indices selecting which measurements to include - * @return 3xN Eigen matrix with positions in columns + * @brief Erase measurements which have been gated / consumed. + * @param measurements Measurements vector (modified in place) + * @param indices Indices of measurements to erase */ - Eigen::Matrix build_position_matrix( - const std::vector& measurements, - const std::vector& indices) const; + void erase_gated_measurements( + std::vector& measurements, + const std::vector& global_indices, + const Eigen::Array& mask) const; /** - * @brief Collect quaternions from gated measurements and mark consumed - * indices. - * @param measurements All measurements - * @param angular_gate_indices Candidate indices that passed angular gate - * @param gated_measurements Boolean mask of gated measurements - * @param consumed_indices_out Output vector filled with indices consumed - * by this collection - * @return Vector of quaternions corresponding to consumed measurements + * @brief Get the configuration for a specific landmark class key. + * @param key Landmark class key + * @return Configuration for the specified class key */ - std::vector collect_used_quaternions( - const std::vector& measurements, - const std::vector& angular_gate_indices, - const Eigen::Array& gated_measurements, - std::vector& consumed_indices_out) const; + 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 Erase measurements which have been gated / consumed. - * @param measurements Measurements vector (modified in place) - * @param indices Indices of measurements to erase + * @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 */ - void erase_gated_measurements(std::vector& measurements, - std::vector& indices); + const LandmarkClassConfig& cfg_for(const Landmark& m) const { + return cfg_for(m.class_key); + } // Internal bookkeeping int track_id_counter_{0}; std::vector tracks_; - DynMod dyn_mod_; - - SensorMod sensor_mod_; - - vortex::filter::IPDA::Config ipda_config_; - - ExistenceManagementConfig existence_config_; - - double max_angle_gate_threshold_{}; - - double initial_position_std_{}; - double initial_orientation_std_{}; - - OrientationFilter orientation_filter_; + TrackManagerConfig config_; }; } // namespace vortex::filtering diff --git a/filtering/pose_filtering/include/pose_filtering/lib/typedefs.hpp b/filtering/pose_filtering/include/pose_filtering/lib/typedefs.hpp index e4ca49fa0..736f73076 100644 --- a/filtering/pose_filtering/include/pose_filtering/lib/typedefs.hpp +++ b/filtering/pose_filtering/include/pose_filtering/lib/typedefs.hpp @@ -2,6 +2,8 @@ #define POSE_FILTERING__LIB__TYPEDEFS_HPP_ #include +#include +#include #include #include #include @@ -19,19 +21,19 @@ namespace vortex::filtering { */ /** - * @brief Gaussian 3d state representation (mean + covariance). + * @brief Gaussian 6d state representation (mean + covariance). */ -using State3d = vortex::prob::Gauss3d; +using State6d = vortex::prob::Gauss6d; /** - * @brief Discrete-time constant dynamic model of order 3. + * @brief Discrete-time constant dynamic model of order 6. */ -using DynMod = vortex::models::ConstantDynamicModel<3>; +using DynMod = vortex::models::ConstantDynamicModel<6>; /** * @brief Identity sensor model mapping state to measurement space. */ -using SensorMod = vortex::models::IdentitySensorModel<3, 3>; +using SensorMod = vortex::models::IdentitySensorModel<6, 6>; /** * @brief IPDA filter type specialized for the chosen dynamic and sensor models. @@ -48,22 +50,6 @@ using PDAF = vortex::filter::PDAF; */ using Pose = vortex::utils::types::Pose; -/** - * @brief Configuration for the dynamic model (process noise standard - * deviation). - */ -struct DynModConfig { - double std_dev{1.0}; -}; - -/** - * @brief Configuration for the sensor model (measurement noise standard - * deviation). - */ -struct SensorModConfig { - double std_dev{1.0}; -}; - /** * @brief Parameters for existence management (track confirmation / deletion). */ @@ -73,13 +59,33 @@ struct ExistenceManagementConfig { double initial_existence_probability{0.5}; }; -/** - * @brief Configuration struct for the orientation filter. - */ -struct OrientationFilterConfig { - vortex::filter::PDAF::Config pdaf; - DynModConfig dyn_mod; - SensorModConfig sensor_mod; +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; }; /** @@ -89,22 +95,33 @@ struct OrientationFilterConfig { * sensor model, existence management and gating thresholds. */ struct TrackManagerConfig { - vortex::filter::IPDA::Config ipda; - double initial_position_std{1.0}; - double initial_orientation_std{1.0}; - DynModConfig dyn_mod; - SensorModConfig sensor_mod; ExistenceManagementConfig existence; - double max_angle_gate_threshold{}; - OrientationFilterConfig ori; + 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 Orientation filter state representation. + * @brief Struct representing the nominal state of a track (position and + * orientation). */ -struct OrientationState { - Eigen::Quaterniond q; - State3d error_state; +struct NominalState { + Eigen::Vector3d pos; + Eigen::Quaterniond ori; +}; + +struct Landmark { + vortex::utils::types::Pose pose{}; + LandmarkClassKey class_key{}; }; /** @@ -117,10 +134,14 @@ struct Track { /// Unique track identifier int id{}; - /// Filter position state (mean + covariance) - State3d state_pos; + /// Landmark class (type + subtype) + LandmarkClassKey class_key{}; + + /// Nominal state representation (position and orientation) + NominalState nominal_state; - OrientationState orientation_filter; + /// Error state representation (position and orientation errors) + State6d error_state; /// Probability that the track exists (0..1) double existence_probability{0.0}; @@ -134,18 +155,8 @@ struct Track { * current_orientation */ Pose to_pose() const { - return vortex::utils::types::Pose::from_eigen(state_pos.mean(), - orientation_filter.q); - } - - /** - * @brief Compute angular distance between the track orientation and a - * measurement pose orientation. - * @param z Measurement pose to compare against - * @return Angular distance in radians (double) - */ - double angular_distance(const Pose& z) const { - return orientation_filter.q.angularDistance(z.ori_quaternion()); + return vortex::utils::types::Pose::from_eigen(nominal_state.pos, + nominal_state.ori); } }; 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 index aa1709e5a..e42f93a50 100644 --- a/filtering/pose_filtering/include/pose_filtering/ros/pose_filtering_ros.hpp +++ b/filtering/pose_filtering/include/pose_filtering/ros/pose_filtering_ros.hpp @@ -6,6 +6,7 @@ #include #include #include +#include #include #include #include @@ -19,8 +20,6 @@ #include #include "pose_filtering/lib/pose_track_manager.hpp" -#include - namespace vortex::filtering { using PoseMsgT = geometry_msgs::msg::PoseStamped; @@ -71,11 +70,11 @@ class PoseFilteringNode : public rclcpp::Node { landmark_array_pub_; rclcpp::TimerBase::SharedPtr pub_timer_; - double filter_dt_seconds_{0.0}; + std::chrono::milliseconds filter_dt_{0}; std::unique_ptr track_manager_; - std::vector measurements_; + std::vector measurements_; bool enu_ned_rotation_{false}; bool debug_{false}; 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 index 4ac2de9d1..8d3be638d 100644 --- 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 @@ -5,6 +5,7 @@ #include #include #include +#include #include #include "pose_filtering/lib/typedefs.hpp" @@ -31,6 +32,32 @@ vortex_msgs::msg::LandmarkArray tracks_to_landmark_array_msg( 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 index b24870a6d..f7e2d5cf2 100644 --- a/filtering/pose_filtering/launch/pose_filtering.launch.py +++ b/filtering/pose_filtering/launch/pose_filtering.launch.py @@ -21,7 +21,9 @@ def generate_launch_description(): output='screen', parameters=[ config, - # {'use_sim_time': True} # If testing with rosbags sim_time might be preferred if bag is looped + { + 'use_sim_time': False, + }, # If testing with rosbags sim_time might be preferred if bag is looped ], ), ] diff --git a/filtering/pose_filtering/src/lib/orientation_filter.cpp b/filtering/pose_filtering/src/lib/orientation_filter.cpp deleted file mode 100644 index c8119c76c..000000000 --- a/filtering/pose_filtering/src/lib/orientation_filter.cpp +++ /dev/null @@ -1,80 +0,0 @@ -#include "pose_filtering/lib/orientation_filter.hpp" -#include - -namespace vortex::filtering { - -OrientationFilter::OrientationFilter(const OrientationFilterConfig& cfg) - : dyn_mod_(cfg.dyn_mod.std_dev), - sensor_mod_(cfg.sensor_mod.std_dev), - pdaf_config_(cfg.pdaf) {} - -void OrientationFilter::step( - const std::vector& measurements, - double dt, - OrientationState& state) { - if (measurements.empty()) { - auto prediction_result = - PDAF::predict(dyn_mod_, sensor_mod_, dt, state.error_state); - state.error_state = prediction_result.x_pred; - return; - } - - Eigen::Matrix measurements_error_space( - 3, measurements.size()); - for (size_t i = 0; i < measurements.size(); ++i) { - measurements_error_space.col(i) = - so3_log_quat(measurements.at(i) * state.q.conjugate()); - } - - auto result = PDAF::step(dyn_mod_, sensor_mod_, dt, state.error_state, - measurements_error_space, pdaf_config_); - - Eigen::Vector3d delta = result.x_post.mean(); - state.q = so3_exp_quat(delta) * state.q; - state.q.normalize(); - - state.error_state.mean().setZero(); - state.error_state.cov() = result.x_post.cov(); -} - -Eigen::Vector3d OrientationFilter::so3_log_quat( - const Eigen::Quaterniond& q_in) { - 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 OrientationFilter::so3_exp_quat( - const Eigen::Vector3d& rvec) { - 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)); -} - -} // namespace vortex::filtering diff --git a/filtering/pose_filtering/src/lib/pose_track_manager.cpp b/filtering/pose_filtering/src/lib/pose_track_manager.cpp index 1fbfb4ed9..424c6d83e 100644 --- a/filtering/pose_filtering/src/lib/pose_track_manager.cpp +++ b/filtering/pose_filtering/src/lib/pose_track_manager.cpp @@ -1,137 +1,207 @@ #include "pose_filtering/lib/pose_track_manager.hpp" #include +#include #include #include -#include "pose_filtering/lib/orientation_filter.hpp" +#include "pose_filtering/lib/typedefs.hpp" namespace vortex::filtering { PoseTrackManager::PoseTrackManager(const TrackManagerConfig& config) - : track_id_counter_(0), - dyn_mod_(config.dyn_mod.std_dev), - sensor_mod_(config.sensor_mod.std_dev), - orientation_filter_(OrientationFilter(config.ori)) { - initial_position_std_ = config.initial_position_std; - initial_orientation_std_ = config.initial_orientation_std; - ipda_config_ = config.ipda; - existence_config_ = config.existence; - max_angle_gate_threshold_ = config.max_angle_gate_threshold; -} + : track_id_counter_(0), config_(config) {} -void PoseTrackManager::step(std::vector& measurements, double dt) { +void PoseTrackManager::step(std::vector& measurements, double dt) { sort_tracks_by_priority(); - for (Track& track : tracks_) { - auto angular_gate_indices = - angular_gate_measurements(track, measurements); - - Eigen::Matrix Z = - build_position_matrix(measurements, angular_gate_indices); + 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.state_pos, + .x_estimate = track.error_state, .existence_probability = track.existence_probability}; - auto ipda_output = IPDA::step(dyn_mod_, sensor_mod_, dt, state_est_prev, - Z, ipda_config_); + auto ipda_output = + IPDA::step(dyn_mod, sensor_mod, dt, state_est_prev, + type_matched_measurements, ipda_cfg, gate); - track.state_pos = ipda_output.state.x_estimate; + track.error_state = ipda_output.state.x_estimate; track.existence_probability = ipda_output.state.existence_probability; - - std::vector consumed_indices; - auto used_quaternions = collect_used_quaternions( - measurements, angular_gate_indices, ipda_output.gated_measurements, - consumed_indices); - - orientation_filter_.step(used_quaternions, dt, - track.orientation_filter); - - erase_gated_measurements(measurements, consumed_indices); + 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::angular_gate_measurements( +std::vector PoseTrackManager::gate_measurements_by_class( const Track& track, - const std::vector& measurements) const { - std::vector indices; - indices.reserve(measurements.size()); + const std::vector& measurements) const { + std::vector idx; + idx.reserve(measurements.size()); for (Eigen::Index i = 0; i < static_cast(measurements.size()); ++i) { - if (track.angular_distance(measurements[i]) < - max_angle_gate_threshold_) { - indices.push_back(i); + const auto& m = measurements[i]; + if (m.class_key == track.class_key) { + idx.push_back(i); } } - return indices; + return idx; } -Eigen::Matrix -PoseTrackManager::build_position_matrix( - const std::vector& measurements, +Eigen::Array +PoseTrackManager::compute_measurement_residuals( + const Track& track, + const std::vector& measurements, const std::vector& indices) const { - Eigen::Matrix Z(3, indices.size()); + IPDA::Arr_zXd Z(6, indices.size()); + for (Eigen::Index k = 0; k < static_cast(indices.size()); ++k) { - Z.col(k) = measurements[indices[k]].pos_vector(); + 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; } -std::vector PoseTrackManager::collect_used_quaternions( - const std::vector& measurements, - const std::vector& angular_gate_indices, - const Eigen::Array& gated_measurements, - std::vector& consumed_indices_out) const { - std::vector quats; - quats.reserve(gated_measurements.size()); +void PoseTrackManager::inject_and_reset(Track& track) { + const Eigen::Matrix delta = track.error_state.mean(); - for (Eigen::Index k = 0; - k < static_cast(angular_gate_indices.size()); ++k) { - if (gated_measurements[k]) { - Eigen::Index original_idx = angular_gate_indices[k]; - quats.push_back(measurements[original_idx].ori_quaternion()); - consumed_indices_out.push_back(original_idx); - } + 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; } - return quats; + + 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, - std::vector& indices) { - std::ranges::sort(indices, std::greater<>()); - for (Eigen::Index idx : indices) { + 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) { +void PoseTrackManager::create_tracks( + const std::vector& measurements) { tracks_.reserve(tracks_.size() + measurements.size()); - auto make_track = [this](const Pose& measurement) { + 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_++, - .state_pos = vortex::prob::Gauss3d( - measurement.pos_vector(), Eigen::Matrix3d::Identity() * - initial_position_std_ * - initial_position_std_), - .orientation_filter = - OrientationState{.q = measurement.ori_quaternion(), - .error_state = State3d( - Eigen::Vector3d::Zero(), - Eigen::Matrix3d::Identity() * - initial_orientation_std_ * - initial_orientation_std_)}, + .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 = - existence_config_.initial_existence_probability, + config_.existence.initial_existence_probability, .confirmed = false}; }; - for (const Pose& m : measurements) { + for (const Landmark& m : measurements) { tracks_.push_back(make_track(m)); } } @@ -139,7 +209,7 @@ void PoseTrackManager::create_tracks(const std::vector& measurements) { void PoseTrackManager::confirm_tracks() { for (Track& track : tracks_) { if (!track.confirmed && track.existence_probability >= - existence_config_.confirmation_threshold) { + config_.existence.confirmation_threshold) { track.confirmed = true; } } @@ -148,7 +218,7 @@ void PoseTrackManager::confirm_tracks() { void PoseTrackManager::delete_tracks() { auto new_end = std::ranges::remove_if(tracks_, [this](Track& track) { return track.existence_probability < - existence_config_.deletion_threshold; + config_.existence.deletion_threshold; }); tracks_.erase(new_end.begin(), new_end.end()); } diff --git a/filtering/pose_filtering/src/ros/pose_filtering_ros.cpp b/filtering/pose_filtering/src/ros/pose_filtering_ros.cpp index e12c8dd09..e156f9ba4 100644 --- a/filtering/pose_filtering/src/ros/pose_filtering_ros.cpp +++ b/filtering/pose_filtering/src/ros/pose_filtering_ros.cpp @@ -32,13 +32,11 @@ void PoseFilteringNode::setup_publishers_and_subscribers() { this->create_publisher( landmark_pub_topic, qos_sensor_data_pub); - int timer_rate_ms = this->declare_parameter("timer_rate_ms"); + filter_dt_ = std::chrono::milliseconds( + this->declare_parameter("timer_rate_ms")); - filter_dt_seconds_ = static_cast(timer_rate_ms) / 1000; - - pub_timer_ = this->create_wall_timer( - std::chrono::milliseconds(timer_rate_ms), - std::bind(&PoseFilteringNode::timer_callback, this)); + pub_timer_ = this->create_wall_timer(filter_dt_, + [this]() { this->timer_callback(); }); target_frame_ = this->declare_parameter("target_frame"); @@ -75,28 +73,28 @@ void PoseFilteringNode::create_pose_subscription( *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) { - RCLCPP_WARN( - this->get_logger(), "TF transform failed from '%s' to '%s': %s", - msg->header.frame_id.c_str(), target_frame_.c_str(), ex.what()); - return; - } - - this->measurements_ = - vortex::utils::ros_conversions::ros_to_pose_vec(pose_tf); - if (enu_ned_rotation_) { - std::ranges::for_each(this->measurements_, [](auto& m) { - m.set_ori( - vortex::utils::math::enu_ned_rotation(m.ori_quaternion())); - }); - } - }); + 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; @@ -105,55 +103,44 @@ void PoseFilteringNode::create_pose_subscription( void PoseFilteringNode::setup_track_manager() { TrackManagerConfig config; - config.ipda.ipda.prob_of_survival = - this->declare_parameter("position.ipda.prob_of_survival"); - config.ipda.ipda.estimate_clutter = - this->declare_parameter("position.ipda.estimate_clutter"); - config.ipda.pdaf.prob_of_detection = - this->declare_parameter("position.ipda.prob_of_detection"); - config.ipda.pdaf.mahalanobis_threshold = this->declare_parameter( - "position.ipda.mahalanobis_gate_threshold"); - config.ipda.pdaf.min_gate_threshold = - this->declare_parameter("position.ipda.min_gate_threshold"); - config.ipda.pdaf.max_gate_threshold = - this->declare_parameter("position.ipda.max_gate_threshold"); - config.ipda.pdaf.clutter_intensity = - this->declare_parameter("position.ipda.clutter_intensity"); - - config.initial_position_std = - this->declare_parameter("position.initial_position_std"); - config.initial_orientation_std = - this->declare_parameter("orientation.initial_orientation_std"); - - config.dyn_mod.std_dev = - this->declare_parameter("position.dyn_mod.std_dev"); - - config.sensor_mod.std_dev = - this->declare_parameter("position.sensor_mod.std_dev"); - - config.max_angle_gate_threshold = - this->declare_parameter("max_angle_gate_threshold"); - config.existence.confirmation_threshold = this->declare_parameter( - "position.existence.confirmation_threshold"); - config.existence.deletion_threshold = this->declare_parameter( - "position.existence.deletion_threshold"); + "config.existence.confirmation_threshold"); + config.existence.deletion_threshold = + this->declare_parameter("config.existence.deletion_threshold"); config.existence.initial_existence_probability = this->declare_parameter( - "position.existence.initial_existence_probability"); - - config.ori.pdaf.pdaf.prob_of_detection = - this->declare_parameter("orientation.pdaf.prob_of_detection"); - config.ori.pdaf.pdaf.clutter_intensity = - this->declare_parameter("orientation.pdaf.clutter_intensity"); - config.ori.pdaf.pdaf.mahalanobis_threshold = - this->declare_parameter( - "orientation.pdaf.mahalanobis_gate_threshold"); + "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"); - config.ori.dyn_mod.std_dev = - this->declare_parameter("orientation.dyn_mod.std_dev"); - config.ori.sensor_mod.std_dev = - this->declare_parameter("orientation.sensor_mod.std_dev"); track_manager_ = std::make_unique(config); } @@ -161,7 +148,8 @@ void PoseFilteringNode::timer_callback() { if (debug_) { publish_meas_debug(); } - track_manager_->step(measurements_, filter_dt_seconds_); + track_manager_->step(measurements_, + std::chrono::duration(filter_dt_).count()); measurements_.clear(); if (debug_) { publish_state_debug(); diff --git a/filtering/pose_filtering/src/ros/pose_filtering_ros_conversions.cpp b/filtering/pose_filtering/src/ros/pose_filtering_ros_conversions.cpp index dd0eb0401..03e7a0d52 100644 --- a/filtering/pose_filtering/src/ros/pose_filtering_ros_conversions.cpp +++ b/filtering/pose_filtering/src/ros/pose_filtering_ros_conversions.cpp @@ -6,18 +6,18 @@ geometry_msgs::msg::PoseWithCovariance track_to_pose_with_covariance( const Track& track) { geometry_msgs::msg::PoseWithCovariance msg; - msg.pose.position.x = track.state_pos.mean()(0); - msg.pose.position.y = track.state_pos.mean()(1); - msg.pose.position.z = track.state_pos.mean()(2); + 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.orientation_filter.q.w(); - msg.pose.orientation.x = track.orientation_filter.q.x(); - msg.pose.orientation.y = track.orientation_filter.q.y(); - msg.pose.orientation.z = track.orientation_filter.q.z(); + 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.state_pos.cov(); + 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 @@ -30,18 +30,17 @@ geometry_msgs::msg::PoseWithCovariance track_to_pose_with_covariance( msg.covariance[2 * 6 + 1] = Pp(2, 1); // zy msg.covariance[2 * 6 + 2] = Pp(2, 2); // zz - const auto& Po = track.orientation_filter.error_state.cov(); - msg.covariance[3 * 6 + 3] = Po(0, 0); // RR - msg.covariance[3 * 6 + 4] = Po(0, 1); // RP - msg.covariance[3 * 6 + 5] = Po(0, 2); // RY + 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] = Po(1, 0); // PR - msg.covariance[4 * 6 + 4] = Po(1, 1); // PP - msg.covariance[4 * 6 + 5] = Po(1, 2); // PY + 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] = Po(2, 0); // YR - msg.covariance[5 * 6 + 4] = Po(2, 1); // YP - msg.covariance[5 * 6 + 5] = Po(2, 2); // YY + 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; } @@ -57,10 +56,70 @@ vortex_msgs::msg::LandmarkArray tracks_to_landmark_array_msg( for (const auto& track : tracks) { vortex_msgs::msg::Landmark landmark; landmark.pose = track_to_pose_with_covariance(track); + landmark.type_class.type = track.class_key.type; + landmark.type_class.subtype = 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_class.type, lm_msg.type_class.subtype}; + 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 index 7f0f481d3..2268a0bc3 100644 --- a/filtering/pose_filtering/src/ros/pose_filtering_ros_debug.cpp +++ b/filtering/pose_filtering/src/ros/pose_filtering_ros_debug.cpp @@ -23,18 +23,17 @@ void PoseFilteringNode::publish_meas_debug() { if (measurements_.empty()) { return; } - Pose meas = measurements_.at(0); + 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.x; - msg.y = meas.y; - msg.z = meas.z; + 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.ori_quaternion()); - + vortex::utils::math::quat_to_euler(meas.pose.ori_quaternion()); msg.roll = euler(0); msg.pitch = euler(1); msg.yaw = euler(2); @@ -47,8 +46,8 @@ void PoseFilteringNode::publish_state_debug() { return; } Track track = track_manager_->get_tracks().at(0); - Eigen::Vector3d pos = track.state_pos.mean(); - Eigen::Quaterniond quat = track.orientation_filter.q; + Eigen::Vector3d pos = track.nominal_state.pos; + Eigen::Quaterniond quat = track.nominal_state.ori; Eigen::Vector3d euler = vortex::utils::math::quat_to_euler(quat); diff --git a/filtering/pose_filtering/test/test_pose_filtering.cpp b/filtering/pose_filtering/test/test_pose_filtering.cpp index df5e25369..c89fdc704 100644 --- a/filtering/pose_filtering/test/test_pose_filtering.cpp +++ b/filtering/pose_filtering/test/test_pose_filtering.cpp @@ -12,39 +12,23 @@ class PoseTrackManagerTests : public ::testing::Test { protected: TrackManagerConfig make_default_config() { TrackManagerConfig cfg{}; - cfg.dyn_mod.std_dev = 0.1; - cfg.sensor_mod.std_dev = 0.1; - - cfg.existence.initial_existence_probability = 0.5; - cfg.existence.confirmation_threshold = 0.6; - cfg.existence.deletion_threshold = 0.2; - - cfg.max_angle_gate_threshold = 0.5; // radians - - cfg.ipda.pdaf.prob_of_detection = 0.5; - cfg.ipda.pdaf.clutter_intensity = 0.01; - cfg.ipda.pdaf.mahalanobis_threshold = 1.0; - cfg.ipda.pdaf.min_gate_threshold = 0.0; - cfg.ipda.pdaf.max_gate_threshold = 1.0; - cfg.ipda.ipda.estimate_clutter = false; - return cfg; } - Pose make_pose( + Landmark make_landmark( const Eigen::Vector3d& pos, const Eigen::Quaterniond& q = Eigen::Quaterniond::Identity()) { - return Pose::from_eigen(pos, q); + return Landmark{Pose::from_eigen(pos, q)}; } }; TEST_F(PoseTrackManagerTests, creates_tracks_from_measurements) { PoseTrackManager mgr(make_default_config()); - std::vector measurements{ - make_pose({0.0, 0.0, 0.0}), - make_pose({1.0, 0.0, 0.0}), - make_pose({0.0, 1.0, 0.0}), + 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); @@ -55,22 +39,22 @@ TEST_F(PoseTrackManagerTests, creates_tracks_from_measurements) { for (const auto& t : tracks) { EXPECT_FALSE(t.confirmed); EXPECT_NEAR(t.existence_probability, 0.5, 1e-12); - EXPECT_NEAR(t.orientation_filter.q.w(), 1.0, 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.max_angle_gate_threshold = 0.2; // strict gate + 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_pose({0, 0, 1}, q_ref), - make_pose({0, 0, 1}, q_far), + std::vector measurements{ + make_landmark({0, 0, 1}, q_ref), + make_landmark({0, 0, 1}, q_far), }; mgr.step(measurements, 0.1); @@ -81,8 +65,8 @@ TEST_F(PoseTrackManagerTests, angular_gate_filters_measurements) { double t2_exist_prob_step1 = mgr.get_tracks().at(1).existence_probability; measurements = { - make_pose({0, 0.5, 1}, q_ref), - make_pose({0, 0.5, 1}, q_far), + make_landmark({0, 0.5, 1}, q_ref), + make_landmark({0, 0.5, 1}, q_far), }; mgr.step(measurements, 0.1); @@ -101,7 +85,7 @@ TEST_F(PoseTrackManagerTests, single_target_existance_increase) { Eigen::Quaterniond q = Eigen::Quaterniond::Identity(); - std::vector z = {make_pose({0, 0, 0}, q)}; + std::vector z = {make_landmark({0, 0, 0}, q)}; PoseTrackManager mgr(cfg); @@ -112,7 +96,7 @@ TEST_F(PoseTrackManagerTests, single_target_existance_increase) { ASSERT_EQ(mgr.get_tracks().size(), 1); ASSERT_EQ(exist_prob1, 0.5); - z = {make_pose({0, 0, 0}, q)}; + z = {make_landmark({0, 0, 0}, q)}; mgr.step(z, 0.1); @@ -121,7 +105,7 @@ TEST_F(PoseTrackManagerTests, single_target_existance_increase) { ASSERT_EQ(mgr.get_tracks().size(), 1); ASSERT_GT(exist_prob2, exist_prob1); - z = {make_pose({0, 0, 0}, q)}; + z = {make_landmark({0, 0, 0}, q)}; mgr.step(z, 0.1); @@ -136,7 +120,7 @@ TEST_F(PoseTrackManagerTests, existence_decrease_on_no_detection) { Eigen::Quaterniond q = Eigen::Quaterniond::Identity(); - std::vector z = {make_pose({0, 0, 0}, q)}; + std::vector z = {make_landmark({0, 0, 0}, q)}; PoseTrackManager mgr(cfg); From bb8ab351f5e26109d7e8cad8c04340262684da0f Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Thu, 19 Feb 2026 14:43:58 +0100 Subject: [PATCH 23/26] update for new landmark_msg type def --- .../src/ros/pose_filtering_ros_conversions.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/filtering/pose_filtering/src/ros/pose_filtering_ros_conversions.cpp b/filtering/pose_filtering/src/ros/pose_filtering_ros_conversions.cpp index 03e7a0d52..f1c6fdd94 100644 --- a/filtering/pose_filtering/src/ros/pose_filtering_ros_conversions.cpp +++ b/filtering/pose_filtering/src/ros/pose_filtering_ros_conversions.cpp @@ -56,8 +56,8 @@ vortex_msgs::msg::LandmarkArray tracks_to_landmark_array_msg( for (const auto& track : tracks) { vortex_msgs::msg::Landmark landmark; landmark.pose = track_to_pose_with_covariance(track); - landmark.type_class.type = track.class_key.type; - landmark.type_class.subtype = track.class_key.subtype; + 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); } @@ -116,7 +116,7 @@ std::vector ros_to_landmarks( lm.pose = vortex::utils::ros_conversions::ros_pose_to_pose(lm_msg.pose.pose); lm.class_key = - LandmarkClassKey{lm_msg.type_class.type, lm_msg.type_class.subtype}; + LandmarkClassKey{lm_msg.type.value, lm_msg.subtype.value}; out.push_back(lm); } return out; From ce07185db8837192c0a784e0e812212e30253fa8 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Thu, 19 Feb 2026 17:39:15 +0100 Subject: [PATCH 24/26] fixed tests, update default noise levels --- filtering/pose_filtering/test/test_pose_filtering.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/filtering/pose_filtering/test/test_pose_filtering.cpp b/filtering/pose_filtering/test/test_pose_filtering.cpp index c89fdc704..6451aa715 100644 --- a/filtering/pose_filtering/test/test_pose_filtering.cpp +++ b/filtering/pose_filtering/test/test_pose_filtering.cpp @@ -2,23 +2,24 @@ #include +#include #include "pose_filtering/lib/pose_track_manager.hpp" -#include "vortex/utils/types.hpp" namespace vortex::filtering { -using vortex::utils::types::Pose; 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)}; + return Landmark{Pose::from_eigen(pos, q), LandmarkClassKey{0, 0}}; } }; @@ -65,8 +66,8 @@ TEST_F(PoseTrackManagerTests, angular_gate_filters_measurements) { double t2_exist_prob_step1 = mgr.get_tracks().at(1).existence_probability; measurements = { - make_landmark({0, 0.5, 1}, q_ref), - make_landmark({0, 0.5, 1}, q_far), + make_landmark({0, 0.25, 1}, q_ref), + make_landmark({0, 0.25, 1}, q_far), }; mgr.step(measurements, 0.1); From 633dc0129f76f9a34d3f417bc979ac3ea7ac6224 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Sun, 1 Mar 2026 20:02:35 +0100 Subject: [PATCH 25/26] sleep for stable op mode in sim tests --- tests/simulator_tests/los_test/simulator_test.sh | 3 +++ tests/simulator_tests/waypoint_manager_test/simulator_test.sh | 3 +++ tests/simulator_tests/waypoint_navigation/simulator_test.sh | 3 +++ 3 files changed, 9 insertions(+) diff --git a/tests/simulator_tests/los_test/simulator_test.sh b/tests/simulator_tests/los_test/simulator_test.sh index 31641b8f3..785df15c5 100755 --- a/tests/simulator_tests/los_test/simulator_test.sh +++ b/tests/simulator_tests/los_test/simulator_test.sh @@ -77,6 +77,9 @@ if journalctl -u ros2 | grep -i "error"; then exit 1 fi +echo "Sleeping for 5 seconds to make sure operation is stable..." +sleep 5 + # Set operation mode echo "Turning off killswitch and setting operation mode to autonomous mode" ros2 service call /orca/set_killswitch vortex_msgs/srv/SetKillswitch "{killswitch_on: false}" diff --git a/tests/simulator_tests/waypoint_manager_test/simulator_test.sh b/tests/simulator_tests/waypoint_manager_test/simulator_test.sh index 3ce428785..5a0d5f3ca 100755 --- a/tests/simulator_tests/waypoint_manager_test/simulator_test.sh +++ b/tests/simulator_tests/waypoint_manager_test/simulator_test.sh @@ -90,6 +90,9 @@ if journalctl -u ros2 | grep -i "error"; then exit 1 fi +echo "Sleeping for 5 seconds to make sure operation is stable..." +sleep 5 + # Set operation mode echo "Turning off killswitch and setting operation mode to autonomous mode" ros2 service call /orca/set_killswitch vortex_msgs/srv/SetKillswitch "{killswitch_on: false}" diff --git a/tests/simulator_tests/waypoint_navigation/simulator_test.sh b/tests/simulator_tests/waypoint_navigation/simulator_test.sh index cb655736d..05cff1e14 100755 --- a/tests/simulator_tests/waypoint_navigation/simulator_test.sh +++ b/tests/simulator_tests/waypoint_navigation/simulator_test.sh @@ -81,6 +81,9 @@ if journalctl -u ros2 | grep -i "error"; then exit 1 fi +echo "Sleeping for 5 seconds to make sure operation is stable..." +sleep 5 + # Set operation mode echo "Turning off killswitch and setting operation mode to autonomous mode" ros2 service call /orca/set_killswitch vortex_msgs/srv/SetKillswitch "{killswitch_on: false}" From fa728e60cfedc43bd40a18192f9070059d406956 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Sat, 7 Mar 2026 09:54:40 +0100 Subject: [PATCH 26/26] fix bug in get_tracks_by_type --- .../include/pose_filtering/lib/pose_track_manager.hpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) 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 index 415db0617..fbb4f2b83 100644 --- a/filtering/pose_filtering/include/pose_filtering/lib/pose_track_manager.hpp +++ b/filtering/pose_filtering/include/pose_filtering/lib/pose_track_manager.hpp @@ -118,10 +118,9 @@ class PoseTrackManager { std::vector out; out.reserve(tracks_.size()); for (const auto& t : tracks_) { - if (!(t.class_key == class_key && !t.confirmed)) { - continue; + if (t.class_key == class_key && t.confirmed) { + out.push_back(&t); } - out.push_back(&t); } return out; }