From 4f2a62aa3a45d113660b451921a8c09a5d8e4808 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 27 Apr 2025 17:59:34 -0400 Subject: [PATCH] Update testLIEKF.cpp --- gtsam/navigation/tests/testLIEKF.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/gtsam/navigation/tests/testLIEKF.cpp b/gtsam/navigation/tests/testLIEKF.cpp index 23f812274..65325c63e 100644 --- a/gtsam/navigation/tests/testLIEKF.cpp +++ b/gtsam/navigation/tests/testLIEKF.cpp @@ -26,14 +26,14 @@ namespace example { static constexpr double k = 0.5; Vector3 dynamics(const Rot3& X, OptionalJacobian<3, 3> H = {}) { // φ = Logmap(R), Dφ = ∂φ/∂δR - Matrix3 Dφ; - Vector3 φ = Rot3::Logmap(X, Dφ); + Matrix3 D_phi; + Vector3 phi = Rot3::Logmap(X, D_phi); // zero out yaw - φ[2] = 0.0; - Dφ.row(2).setZero(); + phi[2] = 0.0; + D_phi.row(2).setZero(); - if (H) *H = -k * Dφ; // ∂(–kφ)/∂δR - return -k * φ; // xi ∈ 𝔰𝔬(3) + if (H) *H = -k * D_phi; // ∂(–kφ)/∂δR + return -k * phi; // xi ∈ 𝔰𝔬(3) } } // namespace example