From 201f25cc2f6ba53b4b0b1fe90ff1f87f57f53af4 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Wed, 26 Mar 2025 22:17:23 +0000 Subject: [PATCH 1/9] fix publisher when two frames are the same --- fuse_models/src/odometry_3d_publisher.cpp | 32 +++++++++++++---------- 1 file changed, 18 insertions(+), 14 deletions(-) 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; + } } } From 17dbc54992407eadf8ccf5c22e742c10db1522d9 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Mon, 31 Mar 2025 11:13:09 -0600 Subject: [PATCH 2/9] remove unnecessary frame check --- .../odometry_3d_publisher_params.hpp | 18 ------------------ 1 file changed, 18 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); From 0418d65dc7a168d82612081c04f5fa308666604f Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Mon, 31 Mar 2025 14:53:07 -0600 Subject: [PATCH 3/9] outlier filtering hack --- .../include/fuse_models/transform_sensor.hpp | 8 ++++ fuse_models/src/transform_sensor.cpp | 42 +++++++++++++++++++ 2 files changed, 50 insertions(+) diff --git a/fuse_models/include/fuse_models/transform_sensor.hpp b/fuse_models/include/fuse_models/transform_sensor.hpp index 09e25fb6e..01f163bf0 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,10 @@ class TransformSensor : public fuse_core::AsyncSensorModel ParameterType params_; + std::optional last_stamp_; + std::optional last_uuid_; + std::optional last_position_; + std::unique_ptr tf_buffer_; std::unique_ptr tf_listener_; std::set fiducial_frames_; diff --git a/fuse_models/src/transform_sensor.cpp b/fuse_models/src/transform_sensor.cpp index 7edceaabb..a40c3ee63 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,6 +49,7 @@ #include #include #include +#include #include #include #include @@ -73,6 +77,21 @@ void TransformSensor::initialize(fuse_core::node_interfaces::NodeInterfacesvariableExists(last_uuid_.value())) + { + auto const& last_position = graph->getVariable(last_uuid_.value()); + last_position_ = geometry_msgs::msg::Point(); + 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 +273,29 @@ void TransformSensor::process(MessageType const& msg) pose.pose.covariance[i * 7] = pose_covariances_[estimation_index][i]; } + // outlier filtering + if (last_position_.has_value() && last_stamp_.has_value()) + { + Eigen::Vector3d position_difference = Eigen::Vector3d::Zero(); + position_difference.x() = last_position_->x - pose.pose.pose.position.x; + position_difference.y() = last_position_->y - pose.pose.pose.position.y; + position_difference.z() = last_position_->z - pose.pose.pose.position.z; + auto const distance = position_difference.norm(); + auto const time_difference = (rclcpp::Time(transform.header.stamp) - last_stamp_.value()).seconds(); + + if (distance > 0.5 && time_difference <= 0.2) + { + // this is an outlier + RCLCPP_WARN(logger_, "Filtered outlier with distance %.3f %.3f seconds after most recent update", 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 985c655d6a197261be8cdacc62875525146891d9 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Mon, 31 Mar 2025 15:11:04 -0600 Subject: [PATCH 4/9] lower outlier threshold --- 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 a40c3ee63..c3a4b1f92 100644 --- a/fuse_models/src/transform_sensor.cpp +++ b/fuse_models/src/transform_sensor.cpp @@ -283,7 +283,7 @@ void TransformSensor::process(MessageType const& msg) auto const distance = position_difference.norm(); auto const time_difference = (rclcpp::Time(transform.header.stamp) - last_stamp_.value()).seconds(); - if (distance > 0.5 && time_difference <= 0.2) + if (distance > 0.1 && time_difference <= 0.2) { // this is an outlier RCLCPP_WARN(logger_, "Filtered outlier with distance %.3f %.3f seconds after most recent update", distance, From 92b17588ad09f874e1293459c2eac7adbe24bf0f Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Thu, 10 Apr 2025 23:32:21 +0000 Subject: [PATCH 5/9] WIP --- .../parameters/transform_sensor_params.hpp | 15 ++++- fuse_models/src/transform_sensor.cpp | 7 ++- .../config/fuse_apriltag_tutorial.yaml | 25 ++++++++- fuse_tutorials/src/apriltag_simulator.cpp | 55 +++++++++++-------- 4 files changed, 74 insertions(+), 28 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 b1dcc4ef5..a2fec693a 100644 --- a/fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp +++ b/fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp @@ -88,6 +88,15 @@ struct TransformSensorParams : public ParameterBase estimation_frames = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "estimation_frames"), estimation_frames); + filter_outliers = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "filter_outliers"), filter_outliers); + + outlier_distance = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "outlier_distance"), outlier_distance); + + outlier_time_threshold = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "outlier_time_threshold"), + outlier_time_threshold); + pose_loss = fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "pose_loss")); pose_covariance = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "pose_covariance"), std::vector{ 1, 1, 1, 1, 1, 1 }); @@ -96,7 +105,11 @@ struct TransformSensorParams : public ParameterBase bool disable_checks{ false }; bool independent{ true }; 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 tf_timeout{ 0, 0 }; //!< The maximum time to wait for a transform to become available + + bool filter_outliers{ false }; + double outlier_distance{ 0.2 }; + double outlier_time_threshold{ 0.2 }; 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 std::vector pose_covariance; //!< The diagonal elements of the tag pose covariance diff --git a/fuse_models/src/transform_sensor.cpp b/fuse_models/src/transform_sensor.cpp index c3a4b1f92..f21ed0403 100644 --- a/fuse_models/src/transform_sensor.cpp +++ b/fuse_models/src/transform_sensor.cpp @@ -273,8 +273,11 @@ void TransformSensor::process(MessageType const& msg) pose.pose.covariance[i * 7] = pose_covariances_[estimation_index][i]; } + std::stringstream s; + s << "xyz: " << pose.pose.pose.position.x << "," << pose.pose.pose.position.y << "," << pose.pose.pose.position.z; + RCLCPP_WARN(logger_, "%s", s.str().c_str()); // outlier filtering - if (last_position_.has_value() && last_stamp_.has_value()) + if (params_.filter_outliers && last_position_.has_value() && last_stamp_.has_value()) { Eigen::Vector3d position_difference = Eigen::Vector3d::Zero(); position_difference.x() = last_position_->x - pose.pose.pose.position.x; @@ -283,7 +286,7 @@ void TransformSensor::process(MessageType const& msg) auto const distance = position_difference.norm(); auto const time_difference = (rclcpp::Time(transform.header.stamp) - last_stamp_.value()).seconds(); - if (distance > 0.1 && time_difference <= 0.2) + if (distance >= params_.outlier_distance && time_difference <= params_.outlier_time_threshold) { // this is an outlier RCLCPP_WARN(logger_, "Filtered outlier with distance %.3f %.3f seconds after most recent update", distance, diff --git a/fuse_tutorials/config/fuse_apriltag_tutorial.yaml b/fuse_tutorials/config/fuse_apriltag_tutorial.yaml index 7a8dea294..2c175bda2 100644 --- a/fuse_tutorials/config/fuse_apriltag_tutorial.yaml +++ b/fuse_tutorials/config/fuse_apriltag_tutorial.yaml @@ -14,7 +14,24 @@ state_estimator: 3d_motion_model: # x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, x_acc, y_acc, z_acc # use high values for the acceleration process noise because we aren't measuring the applied forces - process_noise_diagonal: [0.000001, 0.000001, 0.000001, 0.000001, 0.000001, 0.000001, 0.000001, 0.000001, 0.000001, 0.000001, 0.000001, 0.000001, 10., 10., 10.] + process_noise_diagonal: + [ + 0.01, + 0.01, + 0.01, + 0.01, + 0.01, + 0.01, + 0.01, + 0.01, + 0.01, + 0.01, + 0.01, + 0.01, + 0.01, + 0.01, + 0.01, + ] sensor_models: initial_localization_sensor: @@ -34,11 +51,13 @@ 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 + outlier_distance: 0.5 # this publishes our estimated odometry publishers: @@ -54,4 +73,4 @@ state_estimator: world_frame_id: 'odom' publish_tf: true publish_frequency: 100.0 - predict_to_future: true + predict_to_current_time: true diff --git a/fuse_tutorials/src/apriltag_simulator.cpp b/fuse_tutorials/src/apriltag_simulator.cpp index 8ae1f4e47..753e64096 100644 --- a/fuse_tutorials/src/apriltag_simulator.cpp +++ b/fuse_tutorials/src/apriltag_simulator.cpp @@ -41,6 +41,7 @@ #include #include #include +#include #include #include #include @@ -60,6 +61,8 @@ constexpr double aprilTagOrientationSigma = 0.25; //!< the april tag orientatio constexpr size_t numAprilTags = 8; //!< the number of april tags constexpr double detectionProbability = 0.5; //!< the probability that any given april tag is detectable on a given tick of the simulation + +constexpr double outlierProbabilityPercent = 0.01; constexpr double futurePredictionTimeSeconds = 0.1; } // namespace @@ -202,10 +205,11 @@ tf2_msgs::msg::TFMessage aprilTagPoses(Robot const& robot) return msg; } -tf2_msgs::msg::TFMessage simulateAprilTag(Robot const& robot) +tf2_msgs::msg::TFMessage simulateAprilTag(Robot const& robot, rclcpp::Logger const& logger) { static std::random_device rd{}; static std::mt19937 generator{ rd() }; + static std::uniform_real_distribution<> outlier_distribution(0.0, 1.0); static std::normal_distribution<> position_noise{ 0.0, aprilTagPositionSigma }; static std::normal_distribution<> orientation_noise{ 0.0, aprilTagOrientationSigma }; static std::bernoulli_distribution april_tag_detectable(detectionProbability); @@ -241,9 +245,19 @@ tf2_msgs::msg::TFMessage simulateAprilTag(Robot const& robot) double const z_offset = z_positive ? -1. : 1.; // robot position with offset and noise - april_to_world.transform.translation.x = robot.x + x_offset + position_noise(generator); - april_to_world.transform.translation.y = robot.y + y_offset + position_noise(generator); - april_to_world.transform.translation.z = robot.z + z_offset + position_noise(generator); + if (outlier_distribution(generator) < outlierProbabilityPercent / 100.) + { + RCLCPP_WARN(logger, "Published (likely) outlier"); + april_to_world.transform.translation.x = robot.x + x_offset + 1000. * position_noise(generator); + april_to_world.transform.translation.y = robot.y + y_offset + 1000. * position_noise(generator); + april_to_world.transform.translation.z = robot.z + z_offset + 1000. * position_noise(generator); + } + else + { + april_to_world.transform.translation.x = robot.x + x_offset + position_noise(generator); + april_to_world.transform.translation.y = robot.y + y_offset + position_noise(generator); + april_to_world.transform.translation.z = robot.z + z_offset + position_noise(generator); + } if (april_tag_detectable(generator)) { @@ -275,7 +289,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. @@ -301,32 +315,32 @@ int main(int argc, char** argv) double const mod_time = std::fmod(now_d, motion_duration); // apply a harmonic force (oscillates `N_cycles` times per `motion_duration`) - double const force_magnitude = 100 * std::cos(2 * M_PI * n_cycles * mod_time / motion_duration); + double const force_magnitude = 0 * std::cos(2 * M_PI * n_cycles * mod_time / motion_duration); Eigen::Vector3d external_force = { 0, 0, 0 }; // switch oscillation axes every `motion_duration` seconds (with one 'rest period') if (std::fmod(now_d, 4 * motion_duration) < motion_duration) { - external_force.x() = force_magnitude; + // reset the robot's position and velocity, leave the external force as 0 + // we do this so the ground truth doesn't drift (due to inaccuracy from euler integration) + state.x = 0; + state.y = 0; + state.z = 0; + state.vx = 0; + state.vy = 0; + state.vz = 0; } else if (std::fmod(now_d, 4 * motion_duration) < 2 * motion_duration) { - external_force.y() = force_magnitude; + external_force.x() = force_magnitude; } else if (std::fmod(now_d, 4 * motion_duration) < 3 * motion_duration) { - external_force.z() = force_magnitude; + external_force.y() = force_magnitude; } else { - // reset the robot's position and velocity, leave the external force as 0 - // we do this so the ground truth doesn't drift (due to inaccuracy from euler integration) - state.x = 0; - state.y = 0; - state.z = 0; - state.vx = 0; - state.vy = 0; - state.vz = 0; + external_force.z() = force_magnitude; } // Simulate the robot motion @@ -336,10 +350,7 @@ int main(int argc, char** argv) ground_truth_publisher->publish(robotToOdometry(new_state)); // Generate and publish simulated measurements from the new robot state - if (now_d < 10.) - { - tf_publisher->publish(aprilTagPoses(new_state)); - } + tf_publisher->publish(aprilTagPoses(new_state)); // Wait for the next time step state = new_state; @@ -347,7 +358,7 @@ int main(int argc, char** argv) rate.sleep(); // publish simulated position after the static april tag poses since we need them to be in the tf buffer to run - tf_publisher->publish(simulateAprilTag(new_state)); + tf_publisher->publish(simulateAprilTag(new_state, node->get_logger())); } rclcpp::shutdown(); From fd08038eb89d84872203d54be561c71bacab535e Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Thu, 10 Apr 2025 23:57:45 +0000 Subject: [PATCH 6/9] WIP --- fuse_models/src/transform_sensor.cpp | 13 ++++++++-- .../config/fuse_apriltag_tutorial.yaml | 24 ++++++++++++++++--- fuse_tutorials/src/apriltag_simulator.cpp | 2 +- 3 files changed, 33 insertions(+), 6 deletions(-) diff --git a/fuse_models/src/transform_sensor.cpp b/fuse_models/src/transform_sensor.cpp index f21ed0403..b92523da2 100644 --- a/fuse_models/src/transform_sensor.cpp +++ b/fuse_models/src/transform_sensor.cpp @@ -89,6 +89,14 @@ void TransformSensor::onGraphUpdate(fuse_core::Graph::ConstSharedPtr graph) last_position_->y = last_position.data()[fuse_variables::Position3DStamped::Y]; last_position_->z = last_position.data()[fuse_variables::Position3DStamped::Z]; } + else + { + last_position_.reset(); + } + } + else + { + last_position_.reset(); } } @@ -289,8 +297,9 @@ void TransformSensor::process(MessageType const& msg) if (distance >= params_.outlier_distance && time_difference <= params_.outlier_time_threshold) { // this is an outlier - RCLCPP_WARN(logger_, "Filtered outlier with distance %.3f %.3f seconds after most recent update", distance, - time_difference); + RCLCPP_WARN(logger_, + "Filtered outlier with distance %.3f from (%.3f, %.3f, %.3f) %.3f seconds after most recent update", + distance, last_position_->x, last_position_->y, last_position_->z, time_difference); return; } } diff --git a/fuse_tutorials/config/fuse_apriltag_tutorial.yaml b/fuse_tutorials/config/fuse_apriltag_tutorial.yaml index 2c175bda2..8f24f560d 100644 --- a/fuse_tutorials/config/fuse_apriltag_tutorial.yaml +++ b/fuse_tutorials/config/fuse_apriltag_tutorial.yaml @@ -28,9 +28,9 @@ state_estimator: 0.01, 0.01, 0.01, - 0.01, - 0.01, - 0.01, + 10., + 10., + 10., ] sensor_models: @@ -74,3 +74,21 @@ state_estimator: publish_tf: true publish_frequency: 100.0 predict_to_current_time: true + process_noise_diagonal: + [ + 0.01, + 0.01, + 0.01, + 0.01, + 0.01, + 0.01, + 0.01, + 0.01, + 0.01, + 0.01, + 0.01, + 0.01, + 10., + 10., + 10., + ] diff --git a/fuse_tutorials/src/apriltag_simulator.cpp b/fuse_tutorials/src/apriltag_simulator.cpp index 753e64096..80a1a7ab1 100644 --- a/fuse_tutorials/src/apriltag_simulator.cpp +++ b/fuse_tutorials/src/apriltag_simulator.cpp @@ -315,7 +315,7 @@ int main(int argc, char** argv) double const mod_time = std::fmod(now_d, motion_duration); // apply a harmonic force (oscillates `N_cycles` times per `motion_duration`) - double const force_magnitude = 0 * std::cos(2 * M_PI * n_cycles * mod_time / motion_duration); + double const force_magnitude = 100 * std::cos(2 * M_PI * n_cycles * mod_time / motion_duration); Eigen::Vector3d external_force = { 0, 0, 0 }; // switch oscillation axes every `motion_duration` seconds (with one 'rest period') From a99f03a4f3a1101925ed3c760811349c456a3399 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Fri, 11 Apr 2025 19:59:55 +0000 Subject: [PATCH 7/9] clean up tutorial --- fuse_models/src/transform_sensor.cpp | 3 --- .../config/fuse_apriltag_tutorial.yaml | 4 +-- fuse_tutorials/src/apriltag_simulator.cpp | 26 +++++++++---------- 3 files changed, 15 insertions(+), 18 deletions(-) diff --git a/fuse_models/src/transform_sensor.cpp b/fuse_models/src/transform_sensor.cpp index b92523da2..cf94afaaa 100644 --- a/fuse_models/src/transform_sensor.cpp +++ b/fuse_models/src/transform_sensor.cpp @@ -281,9 +281,6 @@ void TransformSensor::process(MessageType const& msg) pose.pose.covariance[i * 7] = pose_covariances_[estimation_index][i]; } - std::stringstream s; - s << "xyz: " << pose.pose.pose.position.x << "," << pose.pose.pose.position.y << "," << pose.pose.pose.position.z; - RCLCPP_WARN(logger_, "%s", s.str().c_str()); // outlier filtering if (params_.filter_outliers && last_position_.has_value() && last_stamp_.has_value()) { diff --git a/fuse_tutorials/config/fuse_apriltag_tutorial.yaml b/fuse_tutorials/config/fuse_apriltag_tutorial.yaml index 8f24f560d..e0474bede 100644 --- a/fuse_tutorials/config/fuse_apriltag_tutorial.yaml +++ b/fuse_tutorials/config/fuse_apriltag_tutorial.yaml @@ -54,8 +54,8 @@ state_estimator: 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] + # these values are the real values but the orientation variance is set to nonzero to avoid numerical issues + pose_covariance: [0.01, 0.01, 0.01, 0.001, 0.001, 0.001] filter_outliers: true outlier_distance: 0.5 diff --git a/fuse_tutorials/src/apriltag_simulator.cpp b/fuse_tutorials/src/apriltag_simulator.cpp index 80a1a7ab1..abb34082c 100644 --- a/fuse_tutorials/src/apriltag_simulator.cpp +++ b/fuse_tutorials/src/apriltag_simulator.cpp @@ -52,13 +52,13 @@ namespace { -constexpr char baselinkFrame[] = "base_link"; //!< The base_link frame id used when - //!< publishing sensor data -constexpr char mapFrame[] = "map"; //!< The map frame id used when publishing ground truth - //!< data -constexpr double aprilTagPositionSigma = 0.1; //!< the april tag position std dev -constexpr double aprilTagOrientationSigma = 0.25; //!< the april tag orientation std dev -constexpr size_t numAprilTags = 8; //!< the number of april tags +constexpr char baselinkFrame[] = "base_link"; //!< The base_link frame id used when + //!< publishing sensor data +constexpr char mapFrame[] = "map"; //!< The map frame id used when publishing ground truth + //!< data +constexpr double aprilTagPositionVariance = 0.01; //!< the april tag position variance +constexpr double aprilTagOrientationVariance = 0.0; //!< the april tag orientation variance +constexpr size_t numAprilTags = 8; //!< the number of april tags constexpr double detectionProbability = 0.5; //!< the probability that any given april tag is detectable on a given tick of the simulation @@ -210,8 +210,8 @@ tf2_msgs::msg::TFMessage simulateAprilTag(Robot const& robot, rclcpp::Logger con static std::random_device rd{}; static std::mt19937 generator{ rd() }; static std::uniform_real_distribution<> outlier_distribution(0.0, 1.0); - static std::normal_distribution<> position_noise{ 0.0, aprilTagPositionSigma }; - static std::normal_distribution<> orientation_noise{ 0.0, aprilTagOrientationSigma }; + static std::normal_distribution<> position_noise{ 0.0, std::sqrt(aprilTagPositionVariance) }; + static std::normal_distribution<> orientation_noise{ 0.0, std::sqrt(aprilTagOrientationVariance) }; static std::bernoulli_distribution april_tag_detectable(detectionProbability); tf2_msgs::msg::TFMessage msg; @@ -247,10 +247,10 @@ tf2_msgs::msg::TFMessage simulateAprilTag(Robot const& robot, rclcpp::Logger con // robot position with offset and noise if (outlier_distribution(generator) < outlierProbabilityPercent / 100.) { - RCLCPP_WARN(logger, "Published (likely) outlier"); - april_to_world.transform.translation.x = robot.x + x_offset + 1000. * position_noise(generator); - april_to_world.transform.translation.y = robot.y + y_offset + 1000. * position_noise(generator); - april_to_world.transform.translation.z = robot.z + z_offset + 1000. * position_noise(generator); + RCLCPP_WARN(logger, "Published outlier"); + april_to_world.transform.translation.x = robot.x + x_offset + 10.; + april_to_world.transform.translation.y = robot.y + y_offset - 10.; + april_to_world.transform.translation.z = robot.z + z_offset + 10.; } else { From c372e303da1ed1621787aae10cb4eb44e8f07b21 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Fri, 11 Apr 2025 20:17:22 +0000 Subject: [PATCH 8/9] update documentation with outlier filtering information --- fuse_models/doc/sensor_models/transform_sensor.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/fuse_models/doc/sensor_models/transform_sensor.md b/fuse_models/doc/sensor_models/transform_sensor.md index 5c76e5416..796271b05 100644 --- a/fuse_models/doc/sensor_models/transform_sensor.md +++ b/fuse_models/doc/sensor_models/transform_sensor.md @@ -17,6 +17,8 @@ This allows the transform sensor to use any number of cameras to estimate the po A slightly confusing aspect of the sensor is the need for multiple definitions of the target frame. Every frame in `transforms` needs a corresponding `target_frame` to be published for it to be used. This is simply because `tf` uses a tree data structure, and the target frames are leaves. These should all end up being in the same global location (discounting measurement noise) but will have different names. +Finally, an optional feature of the transform sensor is outlier filtering. This can be enabled by setting `filter_outliers` to `true`. This filters outliers based on a simple distance threshold. This distance value and the associated timeout before any distance is allowed (to avoid the situation in which measurements are lost for a period and return but are ignored) can be configured with `outlier_distance` and `outlier_time_threshold`, respectively. Mahalanobis distance thresholding was explored as well, but performs poorly in most cases with external forcing (because this is generally not accounted for in the model or measured). + ## Example For example, say we have `estimation_frame` `camera`, `transforms` {`apriltag_1`, `apriltag_2`}, and `target_frame` `robot_center_of_mass`. @@ -34,3 +36,5 @@ Thus, our measurement of the actual position of `target_frame` that will be used It is not (-1.25, -0.5, 0) because the translation of `T_a_to_b` is the negation of the pose of `b` in frame `a`. Finally, this transaction is sent to the optimizer, as normal. + +To see this sensor running in an example, run `ros2 launch fuse_tutorials fuse_apriltag_tutorial.launch.py`. From 067cc70f22350c56d39738579f250055c355a756 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Tue, 15 Apr 2025 16:21:36 -0600 Subject: [PATCH 9/9] remove last position reset to improve filtering --- fuse_models/src/transform_sensor.cpp | 8 -------- 1 file changed, 8 deletions(-) diff --git a/fuse_models/src/transform_sensor.cpp b/fuse_models/src/transform_sensor.cpp index cf94afaaa..942d4b6af 100644 --- a/fuse_models/src/transform_sensor.cpp +++ b/fuse_models/src/transform_sensor.cpp @@ -89,14 +89,6 @@ void TransformSensor::onGraphUpdate(fuse_core::Graph::ConstSharedPtr graph) last_position_->y = last_position.data()[fuse_variables::Position3DStamped::Y]; last_position_->z = last_position.data()[fuse_variables::Position3DStamped::Z]; } - else - { - last_position_.reset(); - } - } - else - { - last_position_.reset(); } }