diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h index 90aeb54d1..ceb7fa48c 100644 --- a/gtsam/base/ProductLieGroup.h +++ b/gtsam/base/ProductLieGroup.h @@ -74,17 +74,21 @@ public: typedef Eigen::Matrix TangentVector; typedef OptionalJacobian ChartJacobian; - static ProductLieGroup Retract(const TangentVector& v) { - return ProductLieGroup::ChartAtOrigin::Retract(v); + ProductLieGroup retract(const TangentVector& v, // + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) const { + if (H1||H2) throw std::runtime_error("ProductLieGroup::retract derivatives not implemented yet"); + G g = traits::Retract(this->first, v.template head()); + H h = traits::Retract(this->second, v.template tail()); + return ProductLieGroup(g,h); } - static TangentVector LocalCoordinates(const ProductLieGroup& g) { - return ProductLieGroup::ChartAtOrigin::Local(g); - } - ProductLieGroup retract(const TangentVector& v) const { - return compose(ProductLieGroup::ChartAtOrigin::Retract(v)); - } - TangentVector localCoordinates(const ProductLieGroup& g) const { - return ProductLieGroup::ChartAtOrigin::Local(between(g)); + TangentVector localCoordinates(const ProductLieGroup& g, // + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) const { + if (H1||H2) throw std::runtime_error("ProductLieGroup::localCoordinates derivatives not implemented yet"); + typename traits::TangentVector v1 = traits::Local(this->first, g.first); + typename traits::TangentVector v2 = traits::Local(this->second, g.second); + TangentVector v; + v << v1, v2; + return v; } /// @} @@ -147,51 +151,19 @@ public: v << v1, v2; return v; } - struct ChartAtOrigin { - static TangentVector Local(const ProductLieGroup& m, ChartJacobian Hm = boost::none) { - return Logmap(m, Hm); - } - static ProductLieGroup Retract(const TangentVector& v, ChartJacobian Hv = boost::none) { - return Expmap(v, Hv); - } - }; ProductLieGroup expmap(const TangentVector& v) const { return compose(ProductLieGroup::Expmap(v)); } TangentVector logmap(const ProductLieGroup& g) const { return ProductLieGroup::Logmap(between(g)); } - static ProductLieGroup Retract(const TangentVector& v, ChartJacobian H1) { - return ProductLieGroup::ChartAtOrigin::Retract(v,H1); - } - static TangentVector LocalCoordinates(const ProductLieGroup& g, ChartJacobian H1) { - return ProductLieGroup::ChartAtOrigin::Local(g,H1); - } - ProductLieGroup retract(const TangentVector& v, // - ChartJacobian H1, ChartJacobian H2 = boost::none) const { - Jacobian D_g_v; - ProductLieGroup g = ProductLieGroup::ChartAtOrigin::Retract(v,H2 ? &D_g_v : 0); - ProductLieGroup h = compose(g,H1,H2); - if (H2) *H2 = (*H2) * D_g_v; - return h; - } - TangentVector localCoordinates(const ProductLieGroup& g, // - ChartJacobian H1, ChartJacobian H2 = boost::none) const { - ProductLieGroup h = between(g,H1,H2); - Jacobian D_v_h; - TangentVector v = ProductLieGroup::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; - } /// @} }; // Define any direct product group to be a model of the multiplicative Group concept template -struct traits > : internal::LieGroupTraits< - ProductLieGroup > { -}; +struct traits > : internal::LieGroupTraits > {}; + } // namespace gtsam diff --git a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp index 2cc613d65..a8721a60d 100644 --- a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp +++ b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp @@ -86,21 +86,23 @@ TEST( testPoseRTV, Lie ) { Vector delta(9); delta << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4,-0.1,-0.2,-0.3; - Rot3 rot2 = rot.retract(repeat(3, 0.1)); - Point3 pt2 = pt + rot * Point3(0.2, 0.3, 0.4); - Velocity3 vel2 = vel + rot * Velocity3(-0.1,-0.2,-0.3); - PoseRTV state2(pt2, rot2, vel2); - EXPECT(assert_equal(state2, (PoseRTV)state1.retract(delta), 1e-1)); - EXPECT(assert_equal(delta, state1.localCoordinates(state2), 1e-1)); - EXPECT(assert_equal(delta, -state2.localCoordinates(state1), 1e-1)); + Pose3 pose2 = Pose3(rot, pt).retract(delta.head<6>()); + Velocity3 vel2 = vel + Velocity3(-0.1, -0.2, -0.3); + PoseRTV state2(pose2.translation(), pose2.rotation(), vel2); + EXPECT(assert_equal(state2, (PoseRTV)state1.retract(delta), tol)); + EXPECT(assert_equal(delta, state1.localCoordinates(state2), tol)); // roundtrip from state2 to state3 and back PoseRTV state3 = state2.retract(delta); - EXPECT(assert_equal(delta, state2.localCoordinates(state3), 1e-1)); + EXPECT(assert_equal(delta, state2.localCoordinates(state3), tol)); - // roundtrip from state3 to state4 and back, with expmap + // roundtrip from state3 to state4 and back, with expmap. PoseRTV state4 = state3.expmap(delta); - EXPECT(assert_equal(delta, state3.logmap(state4), 1e-1)); + EXPECT(assert_equal(delta, state3.logmap(state4), tol)); + + // For the expmap/logmap (not necessarily retract/local) -delta goes other way + EXPECT(assert_equal(state3, (PoseRTV)state4.expmap(-delta), tol)); + EXPECT(assert_equal(delta, -state4.logmap(state3), tol)); } /* ************************************************************************* */ diff --git a/tests/testLie.cpp b/tests/testLie.cpp index 7be629053..c153adf5f 100644 --- a/tests/testLie.cpp +++ b/tests/testLie.cpp @@ -54,9 +54,9 @@ TEST(Lie, ProductLieGroup) { Vector5 d; d << 1, 2, 0.1, 0.2, 0.3; Product expected(Point2(1, 2), Pose2::Expmap(Vector3(0.1, 0.2, 0.3))); - Product pair2 = pair1.retract(d); + Product pair2 = pair1.expmap(d); EXPECT(assert_equal(expected, pair2, 1e-9)); - EXPECT(assert_equal(d, pair1.localCoordinates(pair2), 1e-9)); + EXPECT(assert_equal(d, pair1.logmap(pair2), 1e-9)); } /* ************************************************************************* */