fix(fuse_constraints): wrap Euler angle residuals in absolute 3D pose and orientation priors

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<Eigen::Matrix<T, Eigen::Dynamic, 1>> residuals_map(residuals, A_.rows());

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

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<Eigen::Vector<T, 6>> full_residuals_map(full_residuals);

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<Eigen::Vector<double, Eigen::Dynamic>> residuals_map(residuals, A_.rows());

fuse_constraints/test/test_absolute_orientation_3d_stamped_euler_constraint.cpp

@@ -38,6 +38,8 @@
 #include <Eigen/Geometry>
 #include <gtest/gtest.h>
 
+#include <cmath>
+#include <memory>
 #include <utility>
 #include <vector>
 
@@ -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<ceres::CostFunction> 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);
+  }
+}

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 <ceres/autodiff_cost_function.h>
 #include <ceres/covariance.h>
 #include <ceres/problem.h>
 #include <ceres/solver.h>
 #include <gtest/gtest.h>
 
+#include <array>
+#include <cmath>
+#include <memory>
 #include <utility>
 #include <vector>
 
 #include <fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp>
+#include <fuse_constraints/normal_prior_pose_3d_euler_cost_functor.hpp>
 #include <fuse_core/eigen.hpp>
 #include <fuse_core/eigen_gtest.hpp>
 #include <fuse_core/serialization.hpp>
@@ -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<ceres::CostFunction> analytic(constraint.costFunction());
+  ceres::AutoDiffCostFunction<fuse_constraints::NormalPriorPose3DEulerCostFunctor, ceres::DYNAMIC, 3, 4> 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<double, 4> 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<double, 3, 3> analytic_position_jacobian, autodiff_position_jacobian;
+    fuse_core::Matrix<double, 3, 4> 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);
+}