Merge pull request #1700 from borglab/fix-1690

Fix #1690
borglab · Jan 2, 2024 · c79543b · c79543b
2 parents 5c87f97 + b15bcb1
commit c79543b
Showing 1 changed file with 2 additions and 1 deletion.
diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp
@@ -106,7 +106,8 @@ bool NavState::equals(const NavState& other, double tol) const {
 //------------------------------------------------------------------------------
 NavState NavState::retract(const Vector9& xi, //
     OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const {
-  auto [nRb, n_t, n_v] = (*this);
+  Rot3 nRb = R_;
+  Point3 n_t = t_, n_v = v_;
   Matrix3 D_bRc_xi, D_R_nRb, D_t_nRb, D_v_nRb;
   const Rot3 bRc = Rot3::Expmap(dR(xi), H2 ? &D_bRc_xi : 0);
   const Rot3 nRc = nRb.compose(bRc, H1 ? &D_R_nRb : 0);