diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 9c410ac25..bf2056cc8 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -110,7 +110,7 @@ struct LieGroup { Class retract(const TangentVector& v, // ChartJacobian H1, ChartJacobian H2 = boost::none) const { Jacobian D_g_v; - Class g = Class::ChartAtOrigin::Retract(v,D_g_v); + Class g = Class::ChartAtOrigin::Retract(v,H2 ? &D_g_v : 0); Class h = compose(g,H1,H2); if (H2) *H2 = (*H2) * D_g_v; return h; @@ -120,7 +120,7 @@ struct LieGroup { ChartJacobian H1, ChartJacobian H2 = boost::none) const { Class h = between(g,H1,H2); Jacobian D_v_h; - TangentVector v = Class::ChartAtOrigin::Local(h, D_v_h); + TangentVector v = Class::ChartAtOrigin::Local(h, (H1 || H2) ? &D_v_h : 0); if (H1) *H1 = D_v_h * (*H1); if (H2) *H2 = D_v_h * (*H2); return v;