From e296f320f5f29a87c9e2d07fa1647b0ce7b34287 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 24 Jul 2015 15:10:00 +0200 Subject: [PATCH] Inlined compose and between derivatives in expmap/logmap --- gtsam/base/Lie.h | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 05c7bc20f..4c26adaa9 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -110,19 +110,20 @@ struct LieGroup { Class retract(const TangentVector& v, // ChartJacobian H1, ChartJacobian H2 = boost::none) const { Jacobian 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; + Class g = Class::ChartAtOrigin::Retract(v, H2 ? &D_g_v : 0); + Class h = compose(g); // derivatives inlined below + if (H1) *H1 = g.inverse().AdjointMap(); + if (H2) *H2 = D_g_v; return h; } TangentVector localCoordinates(const Class& g, // ChartJacobian H1, ChartJacobian H2 = boost::none) const { - Class h = between(g,H1,H2); + Class h = between(g); // derivatives inlined below Jacobian 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); + if (H1) *H1 = - D_v_h * h.inverse().AdjointMap(); + if (H2) *H2 = D_v_h; return v; }