diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h index 2bcb49dba..90aeb54d1 100644 --- a/gtsam/base/ProductLieGroup.h +++ b/gtsam/base/ProductLieGroup.h @@ -23,11 +23,10 @@ namespace gtsam { -/// Template to construct the product Lie group of two other Lie groups, G and H -/// Assumes manifold structure from G and H, and binary constructor +/// Template to construct the product Lie group of two other Lie groups +/// Assumes Lie group structure for G and H template -class ProductLieGroup: public std::pair, public LieGroup< - ProductLieGroup, traits::dimension + traits::dimension> { +class ProductLieGroup: public std::pair { BOOST_CONCEPT_ASSERT((IsLieGroup)); BOOST_CONCEPT_ASSERT((IsLieGroup)); typedef std::pair Base; @@ -58,9 +57,15 @@ public: ProductLieGroup inverse() const { return ProductLieGroup(this->first.inverse(), this->second.inverse()); } + ProductLieGroup compose(const ProductLieGroup& g) const { + return (*this) * g; + } + ProductLieGroup between(const ProductLieGroup& g) const { + return this->inverse() * g; + } /// @} - /// @name Manifold (but with derivatives) + /// @name Manifold /// @{ enum {dimension = dimension1 + dimension2}; inline static size_t Dim() {return dimension;} @@ -68,26 +73,74 @@ public: typedef Eigen::Matrix TangentVector; typedef OptionalJacobian ChartJacobian; + + static ProductLieGroup Retract(const TangentVector& v) { + return ProductLieGroup::ChartAtOrigin::Retract(v); + } + 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)); + } /// @} /// @name Lie Group /// @{ - Eigen::Matrix AdjointMap() const { - Eigen::Matrix A; - A.setIdentity(); - throw std::runtime_error("ProductLieGroup::derivatives not implemented yet"); - // A.template topLeftCorner() = this->first.AdjointMap(); - // A.template bottomRightCorner() = this->second.AdjointMap(); - return A; +protected: + typedef Eigen::Matrix Jacobian; + typedef Eigen::Matrix Jacobian1; + typedef Eigen::Matrix Jacobian2; + +public: + ProductLieGroup compose(const ProductLieGroup& other, ChartJacobian H1, + ChartJacobian H2 = boost::none) const { + Jacobian1 D_g_first; Jacobian2 D_h_second; + G g = traits::Compose(this->first,other.first, H1 ? &D_g_first : 0); + H h = traits::Compose(this->second,other.second, H1 ? &D_h_second : 0); + if (H1) { + H1->setZero(); + H1->template topLeftCorner() = D_g_first; + H1->template bottomRightCorner() = D_h_second; + } + if (H2) *H2 = Jacobian::Identity(); + return ProductLieGroup(g,h); + } + ProductLieGroup between(const ProductLieGroup& other, ChartJacobian H1, + ChartJacobian H2 = boost::none) const { + Jacobian1 D_g_first; Jacobian2 D_h_second; + G g = traits::Between(this->first,other.first, H1 ? &D_g_first : 0); + H h = traits::Between(this->second,other.second, H1 ? &D_h_second : 0); + if (H1) { + H1->setZero(); + H1->template topLeftCorner() = D_g_first; + H1->template bottomRightCorner() = D_h_second; + } + if (H2) *H2 = Jacobian::Identity(); + return ProductLieGroup(g,h); + } + ProductLieGroup inverse(ChartJacobian D) const { + Jacobian1 D_g_first; Jacobian2 D_h_second; + G g = traits::Inverse(this->first, D ? &D_g_first : 0); + H h = traits::Inverse(this->second, D ? &D_h_second : 0); + if (D) { + D->setZero(); + D->template topLeftCorner() = D_g_first; + D->template bottomRightCorner() = D_h_second; + } + return ProductLieGroup(g,h); } static ProductLieGroup Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) { - if (Hv) throw std::runtime_error("ProductLieGroup::derivatives not implemented yet"); + if (Hv) throw std::runtime_error("ProductLieGroup::Expmap derivatives not implemented yet"); G g = traits::Expmap(v.template head()); H h = traits::Expmap(v.template tail()); return ProductLieGroup(g,h); } static TangentVector Logmap(const ProductLieGroup& p, ChartJacobian Hp = boost::none) { - if (Hp) throw std::runtime_error("ProductLieGroup::derivatives not implemented yet"); + if (Hp) throw std::runtime_error("ProductLieGroup::Logmap derivatives not implemented yet"); typename traits::TangentVector v1 = traits::Logmap(p.first); typename traits::TangentVector v2 = traits::Logmap(p.second); TangentVector v; @@ -102,8 +155,37 @@ public: return Expmap(v, Hv); } }; - using LieGroup::inverse; // with derivative + 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 diff --git a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp index d29af526e..2ea582292 100644 --- a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp +++ b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp @@ -120,6 +120,43 @@ TEST( testPoseRTV, dynamics_identities ) { } +/* ************************************************************************* */ +PoseRTV compose_proxy(const PoseRTV& A, const PoseRTV& B) { return A.compose(B); } +TEST( testPoseRTV, compose ) { + PoseRTV state1(pt, rot, vel), state2 = state1; + + Matrix actH1, actH2; + state1.compose(state2, actH1, actH2); + Matrix numericH1 = numericalDerivative21(compose_proxy, state1, state2); + Matrix numericH2 = numericalDerivative22(compose_proxy, state1, state2); + EXPECT(assert_equal(numericH1, actH1, tol)); + EXPECT(assert_equal(numericH2, actH2, tol)); +} + +/* ************************************************************************* */ +PoseRTV between_proxy(const PoseRTV& A, const PoseRTV& B) { return A.between(B); } +TEST( testPoseRTV, between ) { + PoseRTV state1(pt, rot, vel), state2 = state1; + + Matrix actH1, actH2; + state1.between(state2, actH1, actH2); + Matrix numericH1 = numericalDerivative21(between_proxy, state1, state2); + Matrix numericH2 = numericalDerivative22(between_proxy, state1, state2); + EXPECT(assert_equal(numericH1, actH1, tol)); + EXPECT(assert_equal(numericH2, actH2, tol)); +} + +/* ************************************************************************* */ +PoseRTV inverse_proxy(const PoseRTV& A) { return A.inverse(); } +TEST( testPoseRTV, inverse ) { + PoseRTV state1(pt, rot, vel); + + Matrix actH1; + state1.inverse(actH1); + Matrix numericH1 = numericalDerivative11(inverse_proxy, state1); + EXPECT(assert_equal(numericH1, actH1, tol)); +} + /* ************************************************************************* */ double range_proxy(const PoseRTV& A, const PoseRTV& B) { return A.range(B); } TEST( testPoseRTV, range ) {