Derivative of multiplying with inverse of matrix

gtsam/nonlinear/expressions.h

@@ -12,18 +12,54 @@
 
 namespace gtsam {
 
-// Generics
+// Generic between, assumes existence of traits<T>::Between
 template<typename T>
 Expression<T> between(const Expression<T>& t1, const Expression<T>& t2) {
   return Expression<T>(traits<T>::Between, t1, t2);
 }
 
-// Generics
+// Generic compose, assumes existence of traits<T>::Compose
 template<typename T>
 Expression<T> compose(const Expression<T>& t1, const Expression<T>& t2) {
   return Expression<T>(traits<T>::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<Vector3> expression = MultiplyWithInverse<3>()(Key(0), Key(1));
+ */
+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 = boost::none,
+                     OptionalJacobian<N, N> 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>(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<VectorN> operator()(const Expression<MatrixN>& A_,
+                                 const Expression<VectorN>& b_) const {
+    return Expression<VectorN>(*this, A_, b_);
+  }
+};
+
 typedef Expression<double> double_;
 typedef Expression<Vector3> Vector3_;
 

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<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);  // another way
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;