Fix predict_to_current_time forward extrapolation and Omnidirectional3D jacobian

fuse_models/include/fuse_models/detail/prediction_time.hpp

@@ -0,0 +1,64 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2026, PickNik Robotics, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef FUSE_MODELS__DETAIL__PREDICTION_TIME_HPP_
+#define FUSE_MODELS__DETAIL__PREDICTION_TIME_HPP_
+
+#include <algorithm>
+
+namespace fuse_models::detail
+{
+
+/**
+ * @brief Compute the forward time delta used to extrapolate the latest optimized state up to the
+ *        current time in the odometry publishers (predict_to_current_time).
+ *
+ * The publisher predicts from the latest optimized state (at @p stamp_sec) forward to the wall-clock
+ * time it is publishing for (@p to_predict_to_sec). That delta must be clamped to be non-negative:
+ * we never want to predict *backwards* if the optimized state is momentarily ahead of the publish
+ * clock. It must NOT be clamped to be non-positive -- doing so forces dt == 0 and silently disables
+ * forward prediction, so the publisher emits a stale pose stamped as "now" (a latency bug; see the
+ * regression test). Hence std::max(..., 0.0), never std::min(..., 0.0).
+ *
+ * @param[in] to_predict_to_sec Time to predict the state to (seconds), typically wall-clock now.
+ * @param[in] stamp_sec         Timestamp of the latest optimized state (seconds).
+ * @return The non-negative forward prediction interval in seconds.
+ */
+inline double forwardPredictionDt(double const to_predict_to_sec, double const stamp_sec)
+{
+  return std::max(to_predict_to_sec - stamp_sec, 0.0);
+}
+
+}  // namespace fuse_models::detail
+
+#endif  // FUSE_MODELS__DETAIL__PREDICTION_TIME_HPP_

fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp

@@ -35,6 +35,7 @@
 #define FUSE_MODELS__OMNIDIRECTIONAL_3D_PREDICT_HPP_
 
 #include <Eigen/Core>
+#include <Eigen/QR>
 
 #include <fuse_core/util.hpp>
 #include <fuse_core/eigen.hpp>
@@ -508,9 +509,16 @@ inline void predict(fuse_core::Vector3d const& position1, Eigen::Quaterniond con
           vel_linear2.y(), vel_linear2.z(), vel_angular2.x(), vel_angular2.y(), vel_angular2.z(), acc_linear2.x(),
           acc_linear2.y(), acc_linear2.z(), jacobians.data(), jacobian_quat2rpy, velocity_decay);
 
-  // TODO(henrygerardmoore): figure out how to fix this
-  // see https://github.com/locusrobotics/fuse/pull/354#discussion_r1884288806
-  jacobian << J[0], J[1], J[2], J[3], J[4].block<15, 2>(0, 0);
+  // Convert J[1] from quaternion space (15x4) to RPY space (15x3) via the chain rule:
+  //   d(state2)/d(rpy1) = d(state2)/d(quat1) * d(quat1)/d(rpy1) = J[1] * pinv(d(rpy)/d(quat)).
+  // (see https://github.com/locusrobotics/fuse/pull/354#discussion_r1884288806)
+  // Use a rank-revealing pseudo-inverse: at gimbal lock, quaternion2rpy zeros rows of the
+  // quat->rpy Jacobian, so (A*A^T) is singular and an explicit A^T(A*A^T)^-1 would yield NaN.
+  Eigen::Map<fuse_core::Matrix<double, 3, 4>> const j_quat2rpy_map(jacobian_quat2rpy);
+  fuse_core::Matrix<double, 4, 3> const j_quat2rpy_pinv =
+      Eigen::CompleteOrthogonalDecomposition<fuse_core::Matrix<double, 3, 4>>(j_quat2rpy_map).pseudoInverse();
+  fuse_core::Matrix<double, 15, 3> const J1_rpy = J[1] * j_quat2rpy_pinv;
+  jacobian << J[0], J1_rpy, J[2], J[3], J[4];
 
   // Convert back to quaternion
   orientation2 = Eigen::AngleAxisd(rpy[2], Eigen::Vector3d::UnitZ()) *

fuse_models/src/odometry_3d_publisher.cpp

@@ -48,6 +48,7 @@
 #include <fuse_core/eigen.hpp>
 #include <fuse_core/uuid.hpp>
 #include <fuse_models/common/sensor_proc.hpp>
+#include <fuse_models/detail/prediction_time.hpp>
 #include <fuse_models/odometry_3d_publisher.hpp>
 #include <fuse_models/omnidirectional_3d_predict.hpp>
 #include <geometry_msgs/msg/accel_with_covariance_stamped.hpp>
@@ -441,7 +442,8 @@ void Odometry3DPublisher::predict(tf2::Transform& pose, nav_msgs::msg::Odometry&
                                   geometry_msgs::msg::AccelWithCovarianceStamped acceleration_output,
                                   bool latest_covariance_valid) const
 {
-  double const dt = std::min(to_predict_to.seconds() - rclcpp::Time(odom_output.header.stamp).seconds(), 0.);
+  double const dt =
+      detail::forwardPredictionDt(to_predict_to.seconds(), rclcpp::Time(odom_output.header.stamp).seconds());
   // Convert pose in Eigen representation
   fuse_core::Vector3d position;
   fuse_core::Vector3d velocity_linear;

fuse_models/test/CMakeLists.txt

@@ -9,7 +9,8 @@ set(TEST_TARGETS
     test_omnidirectional_3d
     test_omnidirectional_3d_predict
     test_omnidirectional_3d_state_cost_function
-    test_omnidirectional_3d_ignition)
+    test_omnidirectional_3d_ignition
+    test_prediction_time)
 
 foreach(test_name ${TEST_TARGETS})
   ament_add_gtest("${test_name}" "${test_name}.cpp")

fuse_models/test/test_omnidirectional_3d_predict.cpp

@@ -38,6 +38,7 @@
 #include <vector>
 
 #include <fuse_core/eigen_gtest.hpp>
+#include <fuse_core/util.hpp>
 #include <fuse_models/omnidirectional_3d_predict.hpp>
 #include <ceres/jet.h>
 
@@ -711,3 +712,63 @@ TEST(Predict, VelocityDecaysGeometricallyOverMultipleSteps)
   // AND after 1 second velocity has decayed to vel0 * exp(-k * 1.0)
   EXPECT_NEAR(vel_linear.x(), 0.5 * std::exp(-k * 1.0), 1e-6);
 }
+
+TEST(Predict, predictJacobians)
+{
+  // GIVEN a state away from gimbal lock (small angles keep the RPY parameterization well conditioned)
+  double const dt = 0.1;
+  const fuse_core::Vector3d position1(0.1, -0.2, 0.3);
+  const fuse_core::Vector3d vel_linear1(1.0, 0.2, -0.1);
+  const fuse_core::Vector3d vel_angular1(0.3, -0.2, 0.5);
+  const fuse_core::Vector3d acc_linear1(0.5, -0.4, 0.2);
+  const Eigen::Quaterniond orientation1 = Eigen::AngleAxisd(0.3, Eigen::Vector3d::UnitZ()) *
+                                          Eigen::AngleAxisd(-0.1, Eigen::Vector3d::UnitY()) *
+                                          Eigen::AngleAxisd(0.2, Eigen::Vector3d::UnitX());
+
+  // Extract RPY with the same convention predict() uses so the autodiff input matches the analytic input.
+  double const quat[4] = { orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z() };
+  double rpy[3];
+  fuse_core::quaternion2rpy(quat, rpy);
+
+  // WHEN computing the analytic 15x15 state Jacobian (orientation columns in RPY space)
+  fuse_core::Vector3d position2;
+  fuse_core::Vector3d vel_linear2;
+  fuse_core::Vector3d vel_angular2;
+  fuse_core::Vector3d acc_linear2;
+  Eigen::Quaterniond orientation2;
+  fuse_core::Matrix15d jacobian_analytic;
+  fuse_models::predict(position1, orientation1, vel_linear1, vel_angular1, acc_linear1, dt, position2, orientation2,
+                       vel_linear2, vel_angular2, acc_linear2, jacobian_analytic);
+
+  // AND the same Jacobian by autodiff through the templated (RPY-in, RPY-out) overload
+  using Jet = ceres::Jet<double, 15>;
+  const std::array<Jet, 15> x{
+    Jet(position1.x(), 0),    Jet(position1.y(), 1),    Jet(position1.z(), 2),     Jet(rpy[0], 3),
+    Jet(rpy[1], 4),           Jet(rpy[2], 5),           Jet(vel_linear1.x(), 6),   Jet(vel_linear1.y(), 7),
+    Jet(vel_linear1.z(), 8),  Jet(vel_angular1.x(), 9), Jet(vel_angular1.y(), 10), Jet(vel_angular1.z(), 11),
+    Jet(acc_linear1.x(), 12), Jet(acc_linear1.y(), 13), Jet(acc_linear1.z(), 14)
+  };
+  std::array<Jet, 3> position2_jet;
+  std::array<Jet, 3> orientation2_jet;
+  std::array<Jet, 3> vel_linear2_jet;
+  std::array<Jet, 3> vel_angular2_jet;
+  std::array<Jet, 3> acc_linear2_jet;
+  fuse_models::predict(x.data(), x.data() + 3, x.data() + 6, x.data() + 9, x.data() + 12, Jet(dt), position2_jet.data(),
+                       orientation2_jet.data(), vel_linear2_jet.data(), vel_angular2_jet.data(),
+                       acc_linear2_jet.data());
+
+  fuse_core::Matrix15d jacobian_autodiff;
+  const std::array<std::array<Jet, 3> const*, 5> outputs{ &position2_jet, &orientation2_jet, &vel_linear2_jet,
+                                                          &vel_angular2_jet, &acc_linear2_jet };
+  for (int block = 0; block < 5; ++block)
+  {
+    for (int row = 0; row < 3; ++row)
+    {
+      jacobian_autodiff.row(block * 3 + row) = (*outputs[block])[row].v.transpose();
+    }
+  }
+
+  // THEN the analytic Jacobian matches autodiff. This guards the quaternion->RPY conversion of the
+  // orientation columns: the prior truncated assembly would fail this check.
+  EXPECT_MATRIX_NEAR(jacobian_autodiff, jacobian_analytic, 1e-9);
+}

fuse_models/test/test_prediction_time.cpp

@@ -0,0 +1,69 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2026, PickNik Robotics, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+#include <gtest/gtest.h>
+
+#include <fuse_models/detail/prediction_time.hpp>
+
+using fuse_models::detail::forwardPredictionDt;
+
+// Regression test for the predict_to_current_time latency bug.
+//
+// The odometry publishers extrapolate the latest optimized state forward to the current time.
+// A prior change computed this delta as std::min(to - stamp, 0.0), which is always <= 0 in normal
+// forward operation, forcing dt == 0 and silently disabling forward prediction: the publisher then
+// emitted a stale pose stamped as "now". The correct behavior clamps the delta to be non-negative
+// (std::max(..., 0.0)) so forward prediction is preserved while backward prediction is prevented.
+
+TEST(PredictionTime, ForwardIntervalIsPreserved)
+{
+  // The whole point of predict_to_current_time: a future target must yield a positive dt so the
+  // state is actually extrapolated forward. The std::min bug returned 0.0 here.
+  // (EXPECT_NEAR, not EXPECT_DOUBLE_EQ: 10.1 - 10.0 is not exactly 0.1 in floating point.)
+  EXPECT_NEAR(0.1, forwardPredictionDt(10.1, 10.0), 1e-12);
+  EXPECT_DOUBLE_EQ(0.5, forwardPredictionDt(100.5, 100.0));  // 0.5 is exactly representable
+  EXPECT_GT(forwardPredictionDt(10.1, 10.0), 0.0);
+}
+
+TEST(PredictionTime, BackwardIntervalIsClampedToZero)
+{
+  // If the optimized state is momentarily ahead of the publish clock, do not predict backwards.
+  EXPECT_DOUBLE_EQ(0.0, forwardPredictionDt(9.9, 10.0));
+  EXPECT_DOUBLE_EQ(0.0, forwardPredictionDt(10.0, 10.0));
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}