diff --git a/gtsam/nonlinear/expressions.h b/gtsam/nonlinear/expressions.h index 2f46d6d74..a6e2e66b6 100644 --- a/gtsam/nonlinear/expressions.h +++ b/gtsam/nonlinear/expressions.h @@ -12,20 +12,112 @@ namespace gtsam { -// Generics -template +// Generic between, assumes existence of traits::Between +template Expression between(const Expression& t1, const Expression& t2) { return Expression(traits::Between, t1, t2); } -// Generics -template +// 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_); + } +}; + +/** + * 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 48cf03f8c..5fd1a9cb5 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -345,7 +345,6 @@ TEST(ExpressionFactor, tree) { } /* ************************************************************************* */ - TEST(ExpressionFactor, Compose1) { // Create expression @@ -600,6 +599,65 @@ TEST(Expression, testMultipleCompositions2) { EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum4_, values, fd_step, tolerance); } +/* ************************************************************************* */ +// Test multiplication with the inverse of a matrix +TEST(ExpressionFactor, MultiplyWithInverse) { + auto model = noiseModel::Isotropic::Sigma(3, 1); + + // Create expression + 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); +} + +/* ************************************************************************* */ +// 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); +} + /* ************************************************************************* */ int main() { TestResult tr;