From f451503905c896a29e06780502aa9a38c4f305a4 Mon Sep 17 00:00:00 2001 From: Griswald Brooks Date: Wed, 28 Oct 2026 13:00:27 -0400 Subject: [PATCH] fix(fuse_constraints): wrap Euler angle residuals in absolute pose prior --- ...rior_orientation_3d_euler_cost_functor.hpp | 3 + .../normal_prior_pose_3d_euler.hpp | 3 + ...ormal_prior_pose_3d_euler_cost_functor.hpp | 8 ++ .../src/normal_prior_pose_3d_euler.cpp | 5 ++ ...rientation_3d_stamped_euler_constraint.cpp | 27 +++++++ ...olute_pose_3d_stamped_euler_constraint.cpp | 76 +++++++++++++++++++ 6 files changed, 122 insertions(+) diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.hpp index 851d65a58..a96a724fc 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.hpp @@ -65,6 +65,8 @@ namespace fuse_constraints * where the matrix A and the vector b are fixed and (roll, pitch, yaw) are the components of the 3D * orientation variable. * + * Each Euler angle difference is wrapped to [-pi, pi) before multiplication by A. + * * In case the user is interested in implementing a cost function of the form * * cost(X) = (X - mu)^T S^{-1} (X - mu) @@ -129,6 +131,7 @@ class NormalPriorOrientation3DEulerCostFunctor } } residuals[i] = angle - T(b_[i]); + fuse_core::wrapAngle2D(residuals[i]); } Eigen::Map> residuals_map(residuals, A_.rows()); diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler.hpp index 723fc3fbd..689523530 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler.hpp @@ -47,6 +47,9 @@ namespace fuse_constraints * The Ceres::NormalPrior cost function only supports a single variable. This is a convenience cost * function that applies a prior constraint on both the position and orientation variables at once. * + * Each Euler angle difference is wrapped to [-pi, pi) before multiplication by A. + * The Jacobian is defined away from the pi-error cut and Euler singularities. + * * The cost function is of the form: * * || [ x - b(0)] ||^2 diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler_cost_functor.hpp index 7085b79a2..01bbe38bd 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler_cost_functor.hpp @@ -50,6 +50,9 @@ namespace fuse_constraints * function that applies a prior constraint on both the 3D position and orientation variables at * once. * + * Each Euler angle difference is wrapped to [-pi, pi) before multiplication by A. + * The Jacobian is defined away from the pi-error cut and Euler singularities. + * * The cost function is of the form: * * cost(x) = || A * [ p - b(0:2) ] ||^2 @@ -114,6 +117,11 @@ bool NormalPriorPose3DEulerCostFunctor::operator()(const T* const position, cons full_residuals[4] = orientation_rpy[1] - T(b_(4)); full_residuals[5] = orientation_rpy[2] - T(b_(5)); + // Use the shortest angular errors before mixing them with the information matrix. + fuse_core::wrapAngle2D(full_residuals[3]); + fuse_core::wrapAngle2D(full_residuals[4]); + fuse_core::wrapAngle2D(full_residuals[5]); + // Scale the residuals by the square root information matrix to account for // the measurement uncertainty. Eigen::Map> full_residuals_map(full_residuals); diff --git a/fuse_constraints/src/normal_prior_pose_3d_euler.cpp b/fuse_constraints/src/normal_prior_pose_3d_euler.cpp index 098168027..208f9466c 100644 --- a/fuse_constraints/src/normal_prior_pose_3d_euler.cpp +++ b/fuse_constraints/src/normal_prior_pose_3d_euler.cpp @@ -68,6 +68,11 @@ bool NormalPriorPose3DEuler::Evaluate(double const* const* parameters, double* r full_residuals(4) = orientation_rpy[1] - b_(4); full_residuals(5) = orientation_rpy[2] - b_(5); + // Use the shortest angular errors before mixing them with the information matrix. + fuse_core::wrapAngle2D(full_residuals(3)); + fuse_core::wrapAngle2D(full_residuals(4)); + fuse_core::wrapAngle2D(full_residuals(5)); + // Scale the residuals by the square root information matrix to account for // the measurement uncertainty. Eigen::Map> residuals_map(residuals, A_.rows()); diff --git a/fuse_constraints/test/test_absolute_orientation_3d_stamped_euler_constraint.cpp b/fuse_constraints/test/test_absolute_orientation_3d_stamped_euler_constraint.cpp index e139bfdc8..7590d277e 100644 --- a/fuse_constraints/test/test_absolute_orientation_3d_stamped_euler_constraint.cpp +++ b/fuse_constraints/test/test_absolute_orientation_3d_stamped_euler_constraint.cpp @@ -38,6 +38,8 @@ #include #include +#include +#include #include #include @@ -234,3 +236,28 @@ TEST(AbsoluteOrientation3DStampedEulerConstraint, Serialization) EXPECT_MATRIX_EQ(expected.mean(), actual.mean()); EXPECT_MATRIX_EQ(expected.sqrtInformation(), actual.sqrtInformation()); } + +TEST(AbsoluteOrientation3DStampedEulerConstraint, YawResidualAcrossPi) +{ + const Orientation3DStamped orientation(rclcpp::Time(1, 0)); + fuse_core::Matrix3d A; + A << 2.0, 0.3, -0.4, 0.0, 1.5, 0.6, 0.0, 0.0, 3.0; + const fuse_core::Matrix3d covariance = (A.transpose() * A).inverse(); + for (const double direction : { 1.0, -1.0 }) + { + SCOPED_TRACE(direction); + const fuse_core::Vector3d mean(0.2, -0.3, direction * (-M_PI + 0.01)); + const AbsoluteOrientation3DStampedEulerConstraint constraint( + "test", orientation, mean, covariance, + { Orientation3DStamped::Euler::ROLL, Orientation3DStamped::Euler::PITCH, Orientation3DStamped::Euler::YAW }); + const std::unique_ptr cost_function(constraint.costFunction()); + const Eigen::Quaterniond q = Eigen::AngleAxisd(direction * (M_PI - 0.01), Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(-0.3, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.2, Eigen::Vector3d::UnitX()); + const double quaternion[] = { q.w(), q.x(), q.y(), q.z() }; + const double* parameters[] = { quaternion }; + fuse_core::Vector3d residual; + ASSERT_TRUE(cost_function->Evaluate(parameters, residual.data(), nullptr)); + EXPECT_MATRIX_NEAR(A * fuse_core::Vector3d(0.0, 0.0, direction * -0.02), residual, 1.0e-12); + } +} diff --git a/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp index 43b92de91..43c552013 100644 --- a/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp +++ b/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp @@ -31,15 +31,20 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ +#include #include #include #include #include +#include +#include +#include #include #include #include +#include #include #include #include @@ -458,3 +463,74 @@ TEST(AbsolutePose3DStampedEulerConstraint, Serialization) EXPECT_MATRIX_EQ(expected.mean(), actual.mean()); EXPECT_MATRIX_EQ(expected.sqrtInformation(), actual.sqrtInformation()); } + +namespace +{ +// Exercise the same partial-pose constraint used for an absolute IMU orientation. +void expectAngularResiduals(const Eigen::Vector3d& state_rpy, const Eigen::Vector3d& mean_rpy, + const Eigen::Vector3d& expected_error, bool correlated) +{ + const Position3DStamped position(rclcpp::Time(1, 0)); + const Orientation3DStamped orientation(rclcpp::Time(1, 0)); + fuse_core::Vector6d mean = fuse_core::Vector6d::Zero(); + mean.tail<3>() = mean_rpy; + fuse_core::Matrix3d A = fuse_core::Matrix3d::Identity(); + if (correlated) + { + A << 2.0, 0.3, -0.4, 0.0, 1.5, 0.6, 0.0, 0.0, 3.0; + } + const fuse_core::Matrix3d covariance = (A.transpose() * A).inverse(); + const AbsolutePose3DStampedEulerConstraint constraint("test", position, orientation, mean, covariance, { 3, 4, 5 }); + const std::unique_ptr analytic(constraint.costFunction()); + ceres::AutoDiffCostFunction autodiff( + new fuse_constraints::NormalPriorPose3DEulerCostFunctor(constraint.sqrtInformation(), mean), 3); + const Eigen::Quaterniond q = Eigen::AngleAxisd(state_rpy.z(), Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(state_rpy.y(), Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(state_rpy.x(), Eigen::Vector3d::UnitX()); + const Eigen::Vector3d expected = A * expected_error; + for (const double sign : { 1.0, -1.0 }) + { + SCOPED_TRACE(sign); + const std::array quaternion{ sign * q.w(), sign * q.x(), sign * q.y(), sign * q.z() }; + const double* parameters[] = { position.data(), quaternion.data() }; + Eigen::Vector3d analytic_residual, autodiff_residual; + fuse_core::Matrix analytic_position_jacobian, autodiff_position_jacobian; + fuse_core::Matrix analytic_orientation_jacobian, autodiff_orientation_jacobian; + double* analytic_jacobians[] = { analytic_position_jacobian.data(), analytic_orientation_jacobian.data() }; + double* autodiff_jacobians[] = { autodiff_position_jacobian.data(), autodiff_orientation_jacobian.data() }; + ASSERT_TRUE(analytic->Evaluate(parameters, analytic_residual.data(), analytic_jacobians)); + ASSERT_TRUE(autodiff.Evaluate(parameters, autodiff_residual.data(), autodiff_jacobians)); + EXPECT_MATRIX_NEAR(expected, analytic_residual, 1.0e-12); + EXPECT_MATRIX_NEAR(expected, autodiff_residual, 1.0e-12); + // All cases avoid the pi-error cut and Euler pitch singularities. + EXPECT_MATRIX_NEAR(analytic_position_jacobian, autodiff_position_jacobian, 1.0e-12); + EXPECT_MATRIX_NEAR(analytic_orientation_jacobian, autodiff_orientation_jacobian, 1.0e-12); + } +} +} // namespace + +TEST(AbsolutePose3DStampedEulerConstraint, YawResidualAcrossPi) +{ + for (const bool correlated : { false, true }) + { + for (const double direction : { 1.0, -1.0 }) + { + SCOPED_TRACE(correlated); + SCOPED_TRACE(direction); + expectAngularResiduals({ 0.2, -0.3, direction * (M_PI - 0.01) }, { 0.2, -0.3, direction * (-M_PI + 0.01) }, + { 0.0, 0.0, direction * -0.02 }, correlated); + } + } +} + +TEST(AbsolutePose3DStampedEulerConstraint, OrdinaryAngularResiduals) +{ + expectAngularResiduals({ 0.3, -0.2, 0.7 }, { 0.2, -0.3, 0.4 }, { 0.1, 0.1, 0.3 }, true); +} + +TEST(AbsolutePose3DStampedEulerConstraint, WrapAllAnglesBeforeWeighting) +{ + // Pitch uses an equivalent nonprincipal prior because quaternion pitch is in [-pi/2, pi/2]. + expectAngularResiduals({ M_PI - 0.01, 0.2, M_PI - 0.01 }, { -M_PI + 0.01, 0.3 + 2.0 * M_PI, -M_PI + 0.01 }, + { -0.02, -0.1, -0.02 }, true); +}