Fixes from usage with hardware

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

@@ -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_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);
+
     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

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,11 @@ class TransformSensor : public fuse_core::AsyncSensorModel
 
   ParameterType params_;
 
+  std::optional<rclcpp::Time> last_stamp_;
+  std::optional<fuse_core::UUID> last_uuid_;
+  std::optional<Eigen::Vector3d> last_position_;
+  std::vector<std::vector<double>> last_covariance_;
+
   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,11 +49,13 @@
 #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>
 #include <tf2/LinearMath/Transform.hpp>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <utility>
 
 // 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::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_covariance_.clear();
+      std::vector<std::pair<fuse_core::UUID, fuse_core::UUID>> input_uuids;
+      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];
+      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_,

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

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(

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.