diff --git a/gtsam/nonlinear/expressions.h b/gtsam/nonlinear/expressions.h index b4c9a244c..a6e2e66b6 100644 --- a/gtsam/nonlinear/expressions.h +++ b/gtsam/nonlinear/expressions.h @@ -13,13 +13,13 @@ namespace gtsam { // Generic between, assumes existence of traits::Between -template +template Expression between(const Expression& t1, const Expression& t2) { return Expression(traits::Between, t1, t2); } // Generic compose, assumes existence of traits::Compose -template +template Expression compose(const Expression& t1, const Expression& t2) { return Expression(traits::Compose, t1, t2); } @@ -60,8 +60,64 @@ struct MultiplyWithInverse { } }; +/** + * Functor that implements multiplication with the inverse of a matrix, itself + * the result of a function f. It turn out we only need the derivatives of the + * operator phi(a): b -> f(a) * b + */ +template +struct MultiplyWithInverseFunction { + enum { M = traits::dimension }; + typedef Eigen::Matrix VectorN; + typedef Eigen::Matrix MatrixN; + + // The function phi should calculate f(a)*b, with derivatives in a and b. + // Naturally, the derivative in b is f(a). + typedef boost::function, OptionalJacobian)> + Operator; + + /// Construct with function as explained above + MultiplyWithInverseFunction(const Operator& phi) : phi_(phi) {} + + /// f(a).inverse() * b, with optional derivatives + VectorN operator()(const T& a, const VectorN& b, + OptionalJacobian H1 = boost::none, + OptionalJacobian H2 = boost::none) const { + MatrixN A; + phi_(a, b, boost::none, A); // get A = f(a) by calling f once + const MatrixN invA = A.inverse(); + const VectorN c = invA * b; + + if (H1) { + Eigen::Matrix H; + phi_(a, c, H, boost::none); // get derivative H of forward mapping + *H1 = -invA* H; + } + if (H2) *H2 = invA; + return c; + } + + /// Create expression + Expression operator()(const Expression& a_, + const Expression& b_) const { + return Expression(*this, a_, b_); + } + + private: + const Operator phi_; +}; + +// Some typedefs typedef Expression double_; +typedef Expression Vector1_; +typedef Expression Vector2_; typedef Expression Vector3_; +typedef Expression Vector4_; +typedef Expression Vector5_; +typedef Expression Vector6_; +typedef Expression Vector7_; +typedef Expression Vector8_; +typedef Expression Vector9_; -} // \namespace gtsam - +} // \namespace gtsam diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index cef0d0ceb..5fd1a9cb5 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -600,10 +600,11 @@ TEST(Expression, testMultipleCompositions2) { } /* ************************************************************************* */ -// Test multiplication with a matrix +// Test multiplication with the inverse of a matrix TEST(ExpressionFactor, MultiplyWithInverse) { - // Create expression auto model = noiseModel::Isotropic::Sigma(3, 1); + + // Create expression auto f_expr = MultiplyWithInverse<3>()(Key(0), Key(1)); // Check derivatives @@ -615,7 +616,46 @@ TEST(ExpressionFactor, MultiplyWithInverse) { values.insert(0, A); values.insert(1, b); ExpressionFactor factor(model, Vector3::Zero(), f_expr); - EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5); // another way + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5); +} + +/* ************************************************************************* */ +// Test multiplication with the inverse of a matrix function +namespace test_operator { +Vector3 f(const Point2& a, const Vector3& b, OptionalJacobian<3, 2> H1, + OptionalJacobian<3, 3> H2) { + Matrix3 A = Vector3(1, 2, 3).asDiagonal(); + A(0, 1) = a.x(); + A(0, 2) = a.y(); + A(1, 0) = a.x(); + if (H1) *H1 << b.y(), b.z(), b.x(), 0, 0, 0; + if (H2) *H2 = A; + return A * b; +}; +} + +TEST(ExpressionFactor, MultiplyWithInverseFunction) { + auto model = noiseModel::Isotropic::Sigma(3, 1); + + using test_operator::f; + auto f_expr = MultiplyWithInverseFunction(f)(Key(0), Key(1)); + + // Check derivatives + Point2 a(1, 2); + const Vector3 b(0.1, 0.2, 0.3); + Matrix32 H1; + Matrix3 A; + const Vector Ab = f(a, b, H1, A); + CHECK(assert_equal(A * b, Ab)); + CHECK(assert_equal(numericalDerivative11( + boost::bind(f, _1, b, boost::none, boost::none), a), + H1)); + + Values values; + values.insert(0, a); + values.insert(1, b); + ExpressionFactor factor(model, Vector3::Zero(), f_expr); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5); } /* ************************************************************************* */