Outlier filter and changes from hardware testing (#28)

* fix publisher when two frames are the same

* remove unnecessary frame check

* outlier filtering

Author: henrygerardmoore

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`.

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);

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<double>{ 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<double> pose_covariance;       //!< The diagonal elements of the tag pose covariance

fuse_models/include/fuse_models/transform_sensor.hpp

@@ -38,6 +38,7 @@
 #include <tf2_ros/transform_listener.h>
 
 #include <memory>
+#include <optional>
 #include <string>
 
 #include <fuse_models/parameters/transform_sensor_params.hpp>
@@ -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<rclcpp::Time> last_stamp_;
+  std::optional<fuse_core::UUID> last_uuid_;
+  std::optional<geometry_msgs::msg::Point> last_position_;
+
   std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
   std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
   std::set<std::string> fiducial_frames_;

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;
+      }
     }
   }
 

fuse_models/src/transform_sensor.cpp

@@ -36,6 +36,9 @@
 #include <tf2/impl/utils.h>
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/message_filter.h>
+#include <fuse_core/graph.hpp>
+#include <fuse_variables/position_3d_stamped.hpp>
+#include <geometry_msgs/msg/detail/point__struct.hpp>
 #include <memory>
 
 #include <fuse_core/transaction.hpp>
@@ -46,6 +49,7 @@
 #include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <pluginlib/class_list_macros.hpp>
+#include <rclcpp/logging.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <stdexcept>
 #include <string>
@@ -73,6 +77,21 @@ void TransformSensor::initialize(fuse_core::node_interfaces::NodeInterfaces<ALL_
   fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback);
 }
 
+void TransformSensor::onGraphUpdate(fuse_core::Graph::ConstSharedPtr graph)
+{
+  if (last_uuid_.has_value())
+  {
+    if (graph->variableExists(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_,

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.,
+        ]