Multiplying with the inverse of a matrix function

gtsam/nonlinear/expressions.h

@@ -13,13 +13,13 @@
 namespace gtsam {
 
 // Generic between, assumes existence of traits<T>::Between
-template<typename T>
+template <typename T>
 Expression<T> between(const Expression<T>& t1, const Expression<T>& t2) {
   return Expression<T>(traits<T>::Between, t1, t2);
 }
 
 // Generic compose, assumes existence of traits<T>::Compose
-template<typename T>
+template <typename T>
 Expression<T> compose(const Expression<T>& t1, const Expression<T>& t2) {
   return Expression<T>(traits<T>::Compose, t1, t2);
 }
@@ -60,8 +60,64 @@ struct MultiplyWithInverse {
   }
 };
 
+/**
+ * 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 {
+  enum { 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 boost::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 = boost::none,
+                     OptionalJacobian<N, N> 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<double, N, M> 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<VectorN> operator()(const Expression<T>& a_,
+                                 const Expression<VectorN>& b_) const {
+    return Expression<VectorN>(*this, a_, b_);
+  }
+
+ private:
+  const Operator phi_;
+};
+
+// Some typedefs
 typedef Expression<double> double_;
+typedef Expression<Vector1> Vector1_;
+typedef Expression<Vector2> Vector2_;
 typedef Expression<Vector3> Vector3_;
+typedef Expression<Vector4> Vector4_;
+typedef Expression<Vector5> Vector5_;
+typedef Expression<Vector6> Vector6_;
+typedef Expression<Vector7> Vector7_;
+typedef Expression<Vector8> Vector8_;
+typedef Expression<Vector9> Vector9_;
 
-} // \namespace gtsam
-
+}  // \namespace gtsam

tests/testExpressionFactor.cpp

@@ -600,10 +600,11 @@ TEST(Expression, testMultipleCompositions2) {
 }
 
 /* ************************************************************************* */
-// Test multiplication with a matrix
+// Test multiplication with the inverse of a matrix
 TEST(ExpressionFactor, MultiplyWithInverse) {
-  // Create expression
   auto model = noiseModel::Isotropic::Sigma(3, 1);
+
+  // Create expression
   auto f_expr = MultiplyWithInverse<3>()(Key(0), Key(1));
 
   // Check derivatives
@@ -615,7 +616,46 @@ TEST(ExpressionFactor, MultiplyWithInverse) {
   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
+  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<Point2, 3>(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<Vector3, Point2>(
+                         boost::bind(f, _1, b, boost::none, boost::none), 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);
 }
 
 /* ************************************************************************* */