Bring back MultiplyWithInverse

Author: Gold856

gtsam/base/Matrix.h

@@ -347,6 +347,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 <int N>
+struct MultiplyWithInverse {
+  typedef Eigen::Matrix<double, N, 1> VectorN;
+  typedef Eigen::Matrix<double, N, N> MatrixN;
+
+  /// A.inverse() * b, with optional derivatives
+  VectorN operator()(const MatrixN& A, const VectorN& b,
+                     OptionalJacobian<N, N* N> H1 = {},
+                     OptionalJacobian<N, N> H2 = {}) 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>(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 <typename T, int N>
+struct MultiplyWithInverseFunction {
+  inline constexpr static auto M = traits<T>::dimension;
+  typedef Eigen::Matrix<double, N, 1> VectorN;
+  typedef Eigen::Matrix<double, N, N> MatrixN;
+
+  // The function phi should calculate f(a)*b, with derivatives in a and b.
+  // Naturally, the derivative in b is f(a).
+  typedef std::function<VectorN(
+      const T&, const VectorN&, OptionalJacobian<N, M>, OptionalJacobian<N, N>)>
+      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<N, M> H1 = {},
+                     OptionalJacobian<N, N> H2 = {}) const {
+    MatrixN A;
+    phi_(a, b, {}, A);  // get A = f(a) by calling f once
+    const MatrixN invA = A.inverse();
+    const VectorN c = invA * b;
+
+    if (H1) {
+      Eigen::Matrix<double, N, M> H;
+      phi_(a, c, H, {});  // get derivative H of forward mapping
+      *H1 = -invA* H;
+    }
+    if (H2) *H2 = invA;
+    return c;
+  }
+
+ private:
+  const Operator phi_;
+};
+
 GTSAM_EXPORT Matrix LLt(const Matrix& A);
 
 GTSAM_EXPORT Matrix RtR(const Matrix& A);

tests/testExpressionFactor.cpp

@@ -565,6 +565,26 @@ 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
+  Vector3_ f_expr(MultiplyWithInverse<3>(), Expression<Matrix3>(0), Vector3_(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<Matrix3>(0, A);
+  values.insert<Vector3>(1, b);
+  ExpressionFactor<Vector3> 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 {
@@ -580,6 +600,33 @@ Vector3 f(const Point2& a, const Vector3& b, OptionalJacobian<3, 2> H1,
 }
 }
 
+TEST(ExpressionFactor, MultiplyWithInverseFunction) {
+  auto model = noiseModel::Isotropic::Sigma(3, 1);
+
+  using test_operator::f;
+  Vector3_ f_expr(MultiplyWithInverseFunction<Point2, 3>(f),
+                  Expression<Point2>(0), Vector3_(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<Vector3, Point2>(
+        [&](const Point2& a) { return f(a, b, {}, {}); }, a),
+      H1))
+
+  Values values;
+  values.insert<Point2>(0, a);
+  values.insert<Vector3>(1, b);
+  ExpressionFactor<Vector3> factor(model, Vector3::Zero(), f_expr);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5)
+}
+
+
 /* ************************************************************************* */
 // Test N-ary variadic template
 class TestNaryFactor