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`. 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..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/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/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..942d4b6af 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,30 @@ 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()) + { + 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 >= params_.outlier_distance && time_difference <= params_.outlier_time_threshold) + { + // this is an outlier + 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; + } + } + + // 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_, diff --git a/fuse_tutorials/config/fuse_apriltag_tutorial.yaml b/fuse_tutorials/config/fuse_apriltag_tutorial.yaml index 7a8dea294..e0474bede 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, + 10., + 10., + 10., + ] 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] + # 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 # this publishes our estimated odometry publishers: @@ -54,4 +73,22 @@ state_estimator: world_frame_id: 'odom' publish_tf: true publish_frequency: 100.0 - predict_to_future: true + 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 8ae1f4e47..abb34082c 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 @@ -51,15 +52,17 @@ 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 + +constexpr double outlierProbabilityPercent = 0.01; constexpr double futurePredictionTimeSeconds = 0.1; } // namespace @@ -202,12 +205,13 @@ 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::normal_distribution<> position_noise{ 0.0, aprilTagPositionSigma }; - static std::normal_distribution<> orientation_noise{ 0.0, aprilTagOrientationSigma }; + static std::uniform_real_distribution<> outlier_distribution(0.0, 1.0); + 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; @@ -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 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 + { + 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. @@ -307,26 +321,26 @@ int main(int argc, char** argv) // 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();