diff --git a/gtsam/nonlinear/expressions.h b/gtsam/nonlinear/expressions.h index 2f46d6d74..b4c9a244c 100644 --- a/gtsam/nonlinear/expressions.h +++ b/gtsam/nonlinear/expressions.h @@ -12,18 +12,54 @@ namespace gtsam { -// Generics +// Generic between, assumes existence of traits::Between template Expression between(const Expression& t1, const Expression& t2) { return Expression(traits::Between, t1, t2); } -// Generics +// Generic compose, assumes existence of traits::Compose template Expression compose(const Expression& t1, const Expression& t2) { return Expression(traits::Compose, t1, t2); } +/** + * Functor that implements multiplication of a vector b with the inverse of a + * matrix A. The derivatives are inspired by Mike Giles' "An extended collection + * of matrix derivative results for forward and reverse mode algorithmic + * differentiation", at https://people.maths.ox.ac.uk/gilesm/files/NA-08-01.pdf + * + * Usage example: + * Expression expression = MultiplyWithInverse<3>()(Key(0), Key(1)); + */ +template +struct MultiplyWithInverse { + typedef Eigen::Matrix VectorN; + typedef Eigen::Matrix MatrixN; + + /// A.inverse() * b, with optional derivatives + VectorN operator()(const MatrixN& A, const VectorN& b, + OptionalJacobian H1 = boost::none, + OptionalJacobian H2 = boost::none) const { + const MatrixN invA = A.inverse(); + const VectorN c = invA * b; + // The derivative in A is just -[c[0]*invA c[1]*invA ... c[N-1]*invA] + if (H1) + for (size_t j = 0; j < N; j++) + H1->template middleCols(N * j) = -c[j] * invA; + // The derivative in b is easy, as invA*b is just a linear map: + if (H2) *H2 = invA; + return c; + } + + /// Create expression + Expression operator()(const Expression& A_, + const Expression& b_) const { + return Expression(*this, A_, b_); + } +}; + typedef Expression double_; typedef Expression Vector3_; diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index 48cf03f8c..cef0d0ceb 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -345,7 +345,6 @@ TEST(ExpressionFactor, tree) { } /* ************************************************************************* */ - TEST(ExpressionFactor, Compose1) { // Create expression @@ -600,6 +599,25 @@ TEST(Expression, testMultipleCompositions2) { EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum4_, values, fd_step, tolerance); } +/* ************************************************************************* */ +// Test multiplication with a matrix +TEST(ExpressionFactor, MultiplyWithInverse) { + // Create expression + auto model = noiseModel::Isotropic::Sigma(3, 1); + auto f_expr = MultiplyWithInverse<3>()(Key(0), Key(1)); + + // Check derivatives + Values values; + Matrix3 A = Vector3(1, 2, 3).asDiagonal(); + A(0, 1) = 0.1; + A(0, 2) = 0.1; + const Vector3 b(0.1, 0.2, 0.3); + 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 +} + /* ************************************************************************* */ int main() { TestResult tr;