From 063e0a47ee689c823bcc319857b1745fa6155b19 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 1 Feb 2016 09:13:25 -0800 Subject: [PATCH] Moved functors to Matrix.h, without Expression sugar --- gtsam/base/Matrix.h | 73 ++++++++++++++++++++++++++++- gtsam/nonlinear/expressions.h | 84 ---------------------------------- tests/testExpressionFactor.cpp | 5 +- 3 files changed, 75 insertions(+), 87 deletions(-) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index e2b40b02b..37a0d28b8 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -21,15 +21,17 @@ // \callgraph #pragma once +#include #include #include // Configuration from CMake -#include #include #include #include #include +#include #include +#include /** @@ -532,6 +534,75 @@ GTSAM_EXPORT Matrix expm(const Matrix& A, size_t K=7); std::string formatMatrixIndented(const std::string& label, const Matrix& matrix, bool makeVectorHorizontal = false); +/** + * 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 + */ +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; + } +}; + +/** + * 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; + } + + private: + const Operator phi_; +}; + } // namespace gtsam #include diff --git a/gtsam/nonlinear/expressions.h b/gtsam/nonlinear/expressions.h index a6e2e66b6..5b591bcf0 100644 --- a/gtsam/nonlinear/expressions.h +++ b/gtsam/nonlinear/expressions.h @@ -24,90 +24,6 @@ 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_; diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index 5fd1a9cb5..bef69edb6 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -605,7 +605,7 @@ TEST(ExpressionFactor, MultiplyWithInverse) { auto model = noiseModel::Isotropic::Sigma(3, 1); // Create expression - auto f_expr = MultiplyWithInverse<3>()(Key(0), Key(1)); + Vector3_ f_expr(MultiplyWithInverse<3>(), Expression(0), Vector3_(1)); // Check derivatives Values values; @@ -638,7 +638,8 @@ TEST(ExpressionFactor, MultiplyWithInverseFunction) { auto model = noiseModel::Isotropic::Sigma(3, 1); using test_operator::f; - auto f_expr = MultiplyWithInverseFunction(f)(Key(0), Key(1)); + Vector3_ f_expr(MultiplyWithInverseFunction(f), + Expression(0), Vector3_(1)); // Check derivatives Point2 a(1, 2);