From 778fbb20885f177c365ead45eff4da5aab1b81f9 Mon Sep 17 00:00:00 2001 From: Breelyn Styler Date: Thu, 23 Jul 2026 08:08:05 -0400 Subject: [PATCH 1/2] fix(odometry_3d): restore predict_to_current_time forward extrapolation The forward prediction delta was computed as std::min(to_predict_to - stamp, 0.0), which is <= 0 in normal forward operation, forcing dt == 0 and silently disabling predict_to_current_time: the publisher emitted the stale latest-optimized pose stamped as "now". This manifested as a turn-rate-proportional yaw lag (~150 ms; up to ~4.4 deg at 30 deg/s on a mecanum base) that no configuration could fix. Regressed in #24. Clamp the delta to be non-negative (std::max, matching the 2D publisher) so forward prediction is preserved while backward prediction is still prevented. Extract the delta into detail::forwardPredictionDt() and add a regression test that would have caught this. Co-Authored-By: Claude Opus 4.8 --- .../fuse_models/detail/prediction_time.hpp | 64 +++++++++++++++++ fuse_models/src/odometry_3d_publisher.cpp | 4 +- fuse_models/test/CMakeLists.txt | 3 +- fuse_models/test/test_prediction_time.cpp | 69 +++++++++++++++++++ 4 files changed, 138 insertions(+), 2 deletions(-) create mode 100644 fuse_models/include/fuse_models/detail/prediction_time.hpp create mode 100644 fuse_models/test/test_prediction_time.cpp diff --git a/fuse_models/include/fuse_models/detail/prediction_time.hpp b/fuse_models/include/fuse_models/detail/prediction_time.hpp new file mode 100644 index 000000000..918b41db9 --- /dev/null +++ b/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 + +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_ diff --git a/fuse_models/src/odometry_3d_publisher.cpp b/fuse_models/src/odometry_3d_publisher.cpp index 8b921c533..ab44fd4d9 100644 --- a/fuse_models/src/odometry_3d_publisher.cpp +++ b/fuse_models/src/odometry_3d_publisher.cpp @@ -48,6 +48,7 @@ #include #include #include +#include #include #include #include @@ -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; diff --git a/fuse_models/test/CMakeLists.txt b/fuse_models/test/CMakeLists.txt index 71b7009e6..4dd3f5f0f 100644 --- a/fuse_models/test/CMakeLists.txt +++ b/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") diff --git a/fuse_models/test/test_prediction_time.cpp b/fuse_models/test/test_prediction_time.cpp new file mode 100644 index 000000000..5c5d6b290 --- /dev/null +++ b/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 + +#include + +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(); +} From aa35b421cb5119d9a53f125ef42afd5c94af7a69 Mon Sep 17 00:00:00 2001 From: Breelyn Styler Date: Thu, 23 Jul 2026 08:08:05 -0400 Subject: [PATCH 2/2] fix(omnidirectional_3d): compute prediction jacobian in RPY space The assembled state jacobian placed J[1] (d(state)/d(quaternion), 15x4) directly into the orientation columns and truncated the acceleration block (J[4].block<15,2>), leaving Ceres an incorrect orientation gradient during 3D rotation. Convert J[1] to RPY space (15x3) via the pseudo-inverse of the quat->rpy jacobian (chain rule). A rank-revealing CompleteOrthogonalDecomposition is used so it degrades gracefully at gimbal lock, where quaternion2rpy zeros rows of the quat->rpy jacobian and an explicit (A*A^T)^-1 would NaN. Add a predictJacobians test that compares the analytic 15x15 jacobian against ceres::Jet autodiff; it fails against the prior truncated assembly and passes with this fix. Co-Authored-By: Claude Opus 4.8 --- .../omnidirectional_3d_predict.hpp | 14 ++++- .../test/test_omnidirectional_3d_predict.cpp | 61 +++++++++++++++++++ 2 files changed, 72 insertions(+), 3 deletions(-) diff --git a/fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp index 0b8a7ecba..430ade729 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp @@ -35,6 +35,7 @@ #define FUSE_MODELS__OMNIDIRECTIONAL_3D_PREDICT_HPP_ #include +#include #include #include @@ -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> const j_quat2rpy_map(jacobian_quat2rpy); + fuse_core::Matrix const j_quat2rpy_pinv = + Eigen::CompleteOrthogonalDecomposition>(j_quat2rpy_map).pseudoInverse(); + fuse_core::Matrix 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()) * diff --git a/fuse_models/test/test_omnidirectional_3d_predict.cpp b/fuse_models/test/test_omnidirectional_3d_predict.cpp index 4faadd096..ef3ec2bf8 100644 --- a/fuse_models/test/test_omnidirectional_3d_predict.cpp +++ b/fuse_models/test/test_omnidirectional_3d_predict.cpp @@ -38,6 +38,7 @@ #include #include +#include #include #include @@ -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; + const std::array 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 position2_jet; + std::array orientation2_jet; + std::array vel_linear2_jet; + std::array vel_angular2_jet; + std::array 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 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); +}