diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h index 463b5f5d9..87ead88f0 100644 --- a/gtsam/base/ProductLieGroup.h +++ b/gtsam/base/ProductLieGroup.h @@ -138,17 +138,27 @@ public: return ProductLieGroup(g,h); } static ProductLieGroup Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) { - 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()); + Jacobian1 D_g_first; Jacobian2 D_h_second; + G g = traits::Expmap(v.template head(), Hv ? &D_g_first : 0); + H h = traits::Expmap(v.template tail(), Hv ? &D_h_second : 0); + if (Hv) { + Hv->setZero(); + Hv->template topLeftCorner() = D_g_first; + Hv->template bottomRightCorner() = D_h_second; + } return ProductLieGroup(g,h); } static TangentVector Logmap(const ProductLieGroup& p, ChartJacobian Hp = boost::none) { - 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); + Jacobian1 D_g_first; Jacobian2 D_h_second; + typename traits::TangentVector v1 = traits::Logmap(p.first, Hp ? &D_g_first : 0); + typename traits::TangentVector v2 = traits::Logmap(p.second, Hp ? &D_h_second : 0); TangentVector v; v << v1, v2; + if (Hp) { + Hp->setZero(); + Hp->template topLeftCorner() = D_g_first; + Hp->template bottomRightCorner() = D_h_second; + } return v; } ProductLieGroup expmap(const TangentVector& v) const { diff --git a/tests/testLie.cpp b/tests/testLie.cpp index a134a899c..2d8a0b975 100644 --- a/tests/testLie.cpp +++ b/tests/testLie.cpp @@ -102,6 +102,33 @@ TEST( testProduct, inverse ) { EXPECT(assert_equal(numericH1, actH1, tol)); } +/* ************************************************************************* */ +Product expmap_proxy(const Vector5& vec) { + return Product::Expmap(vec); +} +TEST( testProduct, Expmap ) { + Vector5 vec; + vec << 1, 2, 0.1, 0.2, 0.3; + + Matrix actH; + Product::Expmap(vec, actH); + Matrix numericH = numericalDerivative11(expmap_proxy, vec); + EXPECT(assert_equal(numericH, actH, tol)); +} + +/* ************************************************************************* */ +Vector5 logmap_proxy(const Product& p) { + return Product::Logmap(p); +} +TEST( testProduct, Logmap ) { + Product state(Point2(1, 2), Pose2(3, 4, 5)); + + Matrix actH; + Product::Logmap(state, actH); + Matrix numericH = numericalDerivative11(logmap_proxy, state); + EXPECT(assert_equal(numericH, actH, tol)); +} + //****************************************************************************** int main() { TestResult tr;