diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 036b961e8..1576aca1d 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -75,57 +75,89 @@ struct LieGroup { return derived().inverse(); } + /// expmap as required by manifold concept + /// Applies exponential map to v and composes with *this Class expmap(const TangentVector& v) const { return compose(Class::Expmap(v)); } + /// logmap as required by manifold concept + /// Applies logarithmic map to group element that takes *this to g TangentVector logmap(const Class& g) const { return Class::Logmap(between(g)); } + /// expmap with optional derivatives Class expmap(const TangentVector& v, // ChartJacobian H1, ChartJacobian H2 = boost::none) const { Jacobian D_g_v; Class g = Class::Expmap(v,H2 ? &D_g_v : 0); - Class h = compose(g,H1,H2); - if (H2) *H2 = (*H2) * D_g_v; + Class h = compose(g); // derivatives inlined below + if (H1) *H1 = g.inverse().AdjointMap(); + if (H2) *H2 = D_g_v; return h; } + /// logmap with optional derivatives TangentVector logmap(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::Logmap(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; } + /// Retract at origin: possible in Lie group because it has an identity + static Class Retract(const TangentVector& v) { + return Class::ChartAtOrigin::Retract(v); + } + + /// LocalCoordinates at origin: possible in Lie group because it has an identity + static TangentVector LocalCoordinates(const Class& g) { + return Class::ChartAtOrigin::Local(g); + } + + /// Retract at origin with optional derivative + static Class Retract(const TangentVector& v, ChartJacobian H) { + return Class::ChartAtOrigin::Retract(v,H); + } + + /// LocalCoordinates at origin with optional derivative + static TangentVector LocalCoordinates(const Class& g, ChartJacobian H) { + return Class::ChartAtOrigin::Local(g,H); + } + + /// retract as required by manifold concept: applies v at *this Class retract(const TangentVector& v) const { return compose(Class::ChartAtOrigin::Retract(v)); } + /// localCoordinates as required by manifold concept: finds tangent vector between *this and g TangentVector localCoordinates(const Class& g) const { return Class::ChartAtOrigin::Local(between(g)); } + /// retract with optional derivatives 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; } + /// localCoordinates with optional derivatives 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; } diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index fd26535e5..3f46df40d 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -157,7 +157,7 @@ Pose3 Pose3::ChartAtOrigin::Retract(const Vector6& xi, ChartJacobian H) { return Expmap(xi, H); #else Matrix3 DR; - Rot3 R = Rot3::ChartAtOrigin::Retract(xi.head<3>(), H ? &DR : 0); + Rot3 R = Rot3::Retract(xi.head<3>(), H ? &DR : 0); if (H) { *H = I_6x6; H->topLeftCorner<3,3>() = DR; @@ -172,7 +172,7 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& T, ChartJacobian H) { return Logmap(T, H); #else Matrix3 DR; - Vector3 omega = Rot3::ChartAtOrigin::Local(T.rotation(), H ? &DR : 0); + Vector3 omega = Rot3::LocalCoordinates(T.rotation(), H ? &DR : 0); if (H) { *H = I_6x6; H->topLeftCorner<3,3>() = DR; diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 50c143bb9..f6f8b7b40 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -471,6 +471,15 @@ TEST( Pose3, transformPose_to) EXPECT(assert_equal(expected, actual, 0.001)); } +/* ************************************************************************* */ +TEST(Pose3, Retract_LocalCoordinates) +{ + Vector6 d; + d << 1,2,3,4,5,6; d/=10; + R = Rot3::Retract(d.head<3>()); + Pose3 t = Pose3::Retract(d); + EXPECT(assert_equal(d, Pose3::LocalCoordinates(t))); +} /* ************************************************************************* */ TEST(Pose3, retract_localCoordinates) {