From 9ee3f5c38f3ee28efbb1e2bd9d8ef4d3455826d1 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Mon, 7 Apr 2025 18:41:56 +0000 Subject: [PATCH 1/3] on-site fixes --- .../odometry_3d_publisher_params.hpp | 18 ------ .../parameters/transform_sensor_params.hpp | 15 ++++- .../include/fuse_models/transform_sensor.hpp | 9 +++ fuse_models/src/odometry_3d_publisher.cpp | 32 ++++++----- fuse_models/src/transform_sensor.cpp | 55 +++++++++++++++++++ 5 files changed, 96 insertions(+), 33 deletions(-) diff --git a/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp b/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp index 03a5e67fa..406955454 100644 --- a/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp +++ b/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp @@ -111,24 +111,6 @@ struct Odometry3DPublisherParams : public ParameterBase world_frame_id = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "world_frame_id"), world_frame_id); - bool const frames_valid = map_frame_id != odom_frame_id && map_frame_id != base_link_frame_id && - map_frame_id != base_link_output_frame_id && odom_frame_id != base_link_frame_id && - odom_frame_id != base_link_output_frame_id && - (world_frame_id == map_frame_id || world_frame_id == odom_frame_id); - - if (!frames_valid) - { - RCLCPP_FATAL_STREAM(interfaces.get_node_logging_interface()->get_logger(), - "Invalid frame configuration! Please note:\n" - << " - The values for map_frame_id, odom_frame_id, and base_link_frame_id must be " - << "unique\n" - << " - The values for map_frame_id, odom_frame_id, and base_link_output_frame_id must be " - << "unique\n" - << " - The world_frame_id must be the same as the map_frame_id or odom_frame_id\n"); - - assert(frames_valid); - } - topic = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "topic"), topic); acceleration_topic = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "acceleration_topic"), acceleration_topic); diff --git a/fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp b/fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp index b1dcc4ef5..28da6f371 100644 --- a/fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp +++ b/fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp @@ -72,6 +72,16 @@ struct TransformSensorParams : public ParameterBase disable_checks = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "disable_checks"), disable_checks); + + filter_outliers = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "filter_outliers"), filter_outliers); + + outlier_mahalinobis_threshold = fuse_core::getParam( + interfaces, fuse_core::joinParameterName(ns, "outlier_z_score_threshold"), outlier_mahalinobis_threshold); + + outlier_time_threshold_seconds = fuse_core::getParam( + interfaces, fuse_core::joinParameterName(ns, "outlier_time_threshold_seconds"), outlier_time_threshold_seconds); + queue_size = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "queue_size"), queue_size); fuse_core::getPositiveParam(interfaces, fuse_core::joinParameterName(ns, "tf_timeout"), tf_timeout, false); @@ -95,7 +105,10 @@ struct TransformSensorParams : public ParameterBase bool disable_checks{ false }; bool independent{ true }; - fuse_core::Matrix6d minimum_pose_relative_covariance; //!< Minimum pose relative covariance matrix + bool filter_outliers{ false }; + double outlier_mahalinobis_threshold = 4.0331422236561565; //!< sqrt of 99.9% value for 3-dof chi^2, from scipy + double outlier_time_threshold_seconds = 0.2; //!< arbitrary, set based on sensor frequency + fuse_core::Matrix6d minimum_pose_relative_covariance; //!< Minimum pose relative covariance matrix rclcpp::Duration tf_timeout{ 0, 0 }; //!< The maximum time to wait for a transform to become available rclcpp::Duration throttle_period{ 0, 0 }; //!< The throttle period duration in seconds bool throttle_use_wall_time{ false }; //!< Whether to throttle using ros::WallTime or not diff --git a/fuse_models/include/fuse_models/transform_sensor.hpp b/fuse_models/include/fuse_models/transform_sensor.hpp index 09e25fb6e..1bbcf48f5 100644 --- a/fuse_models/include/fuse_models/transform_sensor.hpp +++ b/fuse_models/include/fuse_models/transform_sensor.hpp @@ -38,6 +38,7 @@ #include #include +#include #include #include @@ -119,6 +120,9 @@ class TransformSensor : public fuse_core::AsyncSensorModel */ void onInit() override; + // only used to reject outlier measurements + void onGraphUpdate(fuse_core::Graph::ConstSharedPtr graph) override; + /** * @brief Subscribe to the input topic to start sending transactions to the optimizer */ @@ -140,6 +144,11 @@ class TransformSensor : public fuse_core::AsyncSensorModel ParameterType params_; + std::optional last_stamp_; + std::optional last_uuid_; + std::optional last_position_; + std::vector> last_covariance_; + std::unique_ptr tf_buffer_; std::unique_ptr tf_listener_; std::set fiducial_frames_; diff --git a/fuse_models/src/odometry_3d_publisher.cpp b/fuse_models/src/odometry_3d_publisher.cpp index e473e0e79..8b921c533 100644 --- a/fuse_models/src/odometry_3d_publisher.cpp +++ b/fuse_models/src/odometry_3d_publisher.cpp @@ -551,23 +551,27 @@ void Odometry3DPublisher::publishTF(nav_msgs::msg::Odometry const& odom_output, trans.transform = tf2::toMsg(pose); if (!params_.invert_tf && params_.world_frame_id == params_.map_frame_id) { - try + if (params_.base_link_frame_id != params_.odom_frame_id) { - auto base_to_odom = tf_buffer_->lookupTransform(params_.base_link_frame_id, params_.odom_frame_id, - trans.header.stamp, params_.tf_timeout); + try + { + auto base_to_odom = tf_buffer_->lookupTransform(params_.base_link_frame_id, params_.odom_frame_id, + trans.header.stamp, params_.tf_timeout); - geometry_msgs::msg::TransformStamped map_to_odom; - tf2::doTransform(base_to_odom, map_to_odom, trans); - map_to_odom.child_frame_id = params_.odom_frame_id; - trans = map_to_odom; - } - catch (std::exception const& e) - { - RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 5.0 * 1000, - "Could not lookup the " << params_.base_link_frame_id << "->" << params_.odom_frame_id - << " transform. Error: " << e.what()); + geometry_msgs::msg::TransformStamped map_to_odom; + tf2::doTransform(base_to_odom, map_to_odom, trans); + map_to_odom.child_frame_id = params_.odom_frame_id; + trans = map_to_odom; + } + catch (std::exception const& e) + { + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 5.0 * 1000, + "Could not lookup the " << params_.base_link_frame_id << "->" + << params_.odom_frame_id + << " transform. Error: " << e.what()); - return; + return; + } } } diff --git a/fuse_models/src/transform_sensor.cpp b/fuse_models/src/transform_sensor.cpp index 7edceaabb..76bca9d6f 100644 --- a/fuse_models/src/transform_sensor.cpp +++ b/fuse_models/src/transform_sensor.cpp @@ -36,6 +36,9 @@ #include #include #include +#include +#include +#include #include #include @@ -46,11 +49,13 @@ #include #include #include +#include #include #include #include #include #include +#include // Register this sensor model with ROS as a plugin. PLUGINLIB_EXPORT_CLASS(fuse_models::TransformSensor, fuse_core::SensorModel) @@ -73,6 +78,25 @@ void TransformSensor::initialize(fuse_core::node_interfaces::NodeInterfacesvariableExists(last_uuid_.value())) + { + auto const& last_position = graph->getVariable(last_uuid_.value()); + last_covariance_.clear(); + std::vector> input_uuids; + input_uuids.emplace_back(last_uuid_, last_uuid_); + graph->getCovariance(input_uuids, last_covariance_); + last_position_ = Eigen::Vector3d::Zero(); + last_position_->x() = last_position.data()[fuse_variables::Position3DStamped::X]; + last_position_->y() = last_position.data()[fuse_variables::Position3DStamped::Y]; + last_position_->z() = last_position.data()[fuse_variables::Position3DStamped::Z]; + } + } +} + void TransformSensor::onInit() { logger_ = interfaces_.get_node_logging_interface()->get_logger(); @@ -254,6 +278,37 @@ void TransformSensor::process(MessageType const& msg) pose.pose.covariance[i * 7] = pose_covariances_[estimation_index][i]; } + // outlier filtering + if (params_.filter_outliers && last_position_.has_value() && last_stamp_.has_value()) + { + // calculate the mahalinobis distance and use that for filtering outliers + Eigen::Vector3d position_difference = Eigen::Vector3d::Zero(); + position_difference.x() = pose.pose.pose.position.x - last_position_->x(); + position_difference.y() = pose.pose.pose.position.y - last_position_->y(); + position_difference.z() = pose.pose.pose.position.z - last_position_->z(); + + Eigen::Matrix3d inverse_covariance_matrix = Eigen::Matrix3d::Zero(); + inverse_covariance_matrix.diagonal() = + Eigen::Vector3d{ 1. / last_covariance_[0][0], 1. / last_covariance_[0][1], 1. / last_covariance_[0][2] }; + + auto const mahalinobis_distance = + std::sqrt(position_difference.transpose() * inverse_covariance_matrix * position_difference); + auto const time_difference = (rclcpp::Time(transform.header.stamp) - last_stamp_.value()).seconds(); + + if (mahalinobis_distance > params_.outlier_mahalinobis_threshold && + time_difference <= params_.outlier_time_threshold_seconds) + { + // this is an outlier + RCLCPP_WARN(logger_, "Filtered outlier with Mahalinobis distance %.3f %.3f seconds after most recent update", + mahalinobis_distance, time_difference); + return; + } + } + + // update outlier finding variables (must occur after outlier filtering) + last_stamp_ = transform.header.stamp; + last_uuid_ = fuse_variables::Position3DStamped(transform.header.stamp, device_id_).uuid(); + bool const validate = !params_.disable_checks; common::processAbsolutePose3DWithCovariance(name(), device_id_, pose, params_.pose_loss, "", params_.position_indices, params_.orientation_indices, *tf_buffer_, From 99915bb7177753e9ce1c68d4acca181bdd6b5b75 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Mon, 7 Apr 2025 22:14:53 +0000 Subject: [PATCH 2/3] fix compilation error --- fuse_models/src/transform_sensor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fuse_models/src/transform_sensor.cpp b/fuse_models/src/transform_sensor.cpp index 76bca9d6f..e74c4fb7a 100644 --- a/fuse_models/src/transform_sensor.cpp +++ b/fuse_models/src/transform_sensor.cpp @@ -87,7 +87,7 @@ void TransformSensor::onGraphUpdate(fuse_core::Graph::ConstSharedPtr graph) auto const& last_position = graph->getVariable(last_uuid_.value()); last_covariance_.clear(); std::vector> input_uuids; - input_uuids.emplace_back(last_uuid_, last_uuid_); + input_uuids.emplace_back(last_uuid_.value(), last_uuid_.value()); graph->getCovariance(input_uuids, last_covariance_); last_position_ = Eigen::Vector3d::Zero(); last_position_->x() = last_position.data()[fuse_variables::Position3DStamped::X]; From 72a4f5690a562e024583998d5a8b58a9dc4ffd11 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Tue, 8 Apr 2025 22:42:02 +0000 Subject: [PATCH 3/3] WIP --- .../fuse_models/parameters/transform_sensor_params.hpp | 2 +- fuse_tutorials/config/fuse_apriltag_tutorial.yaml | 4 ++-- fuse_tutorials/launch/fuse_apriltag_tutorial.launch.py | 2 +- fuse_tutorials/src/apriltag_simulator.cpp | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp b/fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp index 28da6f371..ec997d3e7 100644 --- a/fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp +++ b/fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp @@ -77,7 +77,7 @@ struct TransformSensorParams : public ParameterBase fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "filter_outliers"), filter_outliers); outlier_mahalinobis_threshold = fuse_core::getParam( - interfaces, fuse_core::joinParameterName(ns, "outlier_z_score_threshold"), outlier_mahalinobis_threshold); + interfaces, fuse_core::joinParameterName(ns, "outlier_mahalinobis_threshold"), outlier_mahalinobis_threshold); outlier_time_threshold_seconds = fuse_core::getParam( interfaces, fuse_core::joinParameterName(ns, "outlier_time_threshold_seconds"), outlier_time_threshold_seconds); diff --git a/fuse_tutorials/config/fuse_apriltag_tutorial.yaml b/fuse_tutorials/config/fuse_apriltag_tutorial.yaml index 7a8dea294..e672060a4 100644 --- a/fuse_tutorials/config/fuse_apriltag_tutorial.yaml +++ b/fuse_tutorials/config/fuse_apriltag_tutorial.yaml @@ -34,11 +34,12 @@ state_estimator: transform_sensor: transforms: ['april_1', 'april_2', 'april_3', 'april_4', 'april_5', 'april_6', 'april_7', 'april_8'] target_frame: 'base_link' - estimation_frame: 'odom' + estimation_frames: ['odom'] position_dimensions: ['x', 'y', 'z'] orientation_dimensions: ['roll', 'pitch', 'yaw'] # these are the true covariance values used by the simulator; what happens if we change these? pose_covariance: [0.1, 0.1, 0.1, 0.25, 0.25, 0.25] + filter_outliers: true # this publishes our estimated odometry publishers: @@ -54,4 +55,3 @@ state_estimator: world_frame_id: 'odom' publish_tf: true publish_frequency: 100.0 - predict_to_future: true diff --git a/fuse_tutorials/launch/fuse_apriltag_tutorial.launch.py b/fuse_tutorials/launch/fuse_apriltag_tutorial.launch.py index a4524e8e7..9abcfe43d 100644 --- a/fuse_tutorials/launch/fuse_apriltag_tutorial.launch.py +++ b/fuse_tutorials/launch/fuse_apriltag_tutorial.launch.py @@ -49,7 +49,7 @@ def generate_launch_description(): [pkg_dir, "config", "fuse_apriltag_tutorial.yaml"] ) ], - # prefix=['gdbserver localhost:3000'], + prefix=["gdbserver localhost:3000"], ), # run visualization Node( diff --git a/fuse_tutorials/src/apriltag_simulator.cpp b/fuse_tutorials/src/apriltag_simulator.cpp index 8ae1f4e47..9a71421e3 100644 --- a/fuse_tutorials/src/apriltag_simulator.cpp +++ b/fuse_tutorials/src/apriltag_simulator.cpp @@ -275,7 +275,7 @@ int main(int argc, char** argv) // you can modify the rate at which this loop runs to see the different performance of the estimator and the effect of // integration inaccuracy on the ground truth - auto rate = rclcpp::Rate(1000.0); + auto rate = rclcpp::Rate(100.0); // normally we would have to initialize the state estimation, but we included an ignition 'sensor' in our config, // which takes care of that.