Skeleton code for GaussianFactorGraph::multiplyHessian

gtsam/linear/GaussianFactor.h

@@ -106,6 +106,9 @@ namespace gtsam {
      */
     virtual GaussianFactor::shared_ptr negate() const = 0;
 
+    /// y += alpha * A'*A*x
+    virtual void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y)=0;
+
   private:
     /** Serialization function */
     friend class boost::serialization::access;

gtsam/linear/GaussianFactorGraph.cpp

@@ -246,6 +246,14 @@ namespace gtsam {
     return e;
   }
 
+  /* ************************************************************************* */
+  VectorValues GaussianFactorGraph::multiplyHessian(const VectorValues& x) const {
+    VectorValues y;
+    BOOST_FOREACH(const GaussianFactor::shared_ptr& f, *this)
+      f->multiplyHessianAdd(1.0,x,y);
+    return y;
+  }
+
   /* ************************************************************************* */
   void GaussianFactorGraph::multiplyInPlace(const VectorValues& x, Errors& e) const {
     multiplyInPlace(x, e.begin());
@@ -275,8 +283,8 @@ namespace gtsam {
 
   /* ************************************************************************* */
   // x += alpha*A'*e
-  void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e, VectorValues& x) const
-  {
+void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e,
+    VectorValues& x) const {
   // For each factor add the gradient contribution
   Errors::const_iterator ei = e.begin();
   BOOST_FOREACH(const sharedFactor& Ai_G, *this) {

gtsam/linear/GaussianFactorGraph.h

@@ -269,6 +269,9 @@ namespace gtsam {
     ///** return A*x */
     Errors operator*(const VectorValues& x) const;
 
+    ///** return A'A*x */
+    VectorValues multiplyHessian(const VectorValues& x) const;
+
     ///** In-place version e <- A*x that overwrites e. */
     void multiplyInPlace(const VectorValues& x, Errors& e) const;
 

gtsam/linear/HessianFactor.cpp

@@ -499,6 +499,12 @@ GaussianFactor::shared_ptr HessianFactor::negate() const
   return result;
 }
 
+/* ************************************************************************* */
+void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x,
+    VectorValues& y) {
+
+}
+
 /* ************************************************************************* */
 std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<HessianFactor> >
   EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys)

gtsam/linear/HessianFactor.h

@@ -356,8 +356,8 @@ namespace gtsam {
      */
     void updateATA(const HessianFactor& update, const Scatter& scatter);
 
-    /** Return A'A*x */
-    // Vector operator*(const VectorValues& x) const;
+    /** y += alpha * A'*A*x */
+    void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y);
 
     /**
     *   Densely partially eliminate with Cholesky factorization.  JacobianFactors are

gtsam/linear/JacobianFactor.cpp

@@ -432,6 +432,12 @@ namespace gtsam {
     return model_ ? model_->whiten(Ax) : Ax;
   }
 
+  /* ************************************************************************* */
+  void JacobianFactor::multiplyHessianAdd(double alpha, const VectorValues& x,
+      VectorValues& y) {
+
+  }
+
   /* ************************************************************************* */
   void JacobianFactor::transposeMultiplyAdd(double alpha, const Vector& e,
       VectorValues& x) const

gtsam/linear/JacobianFactor.h

@@ -263,6 +263,9 @@ namespace gtsam {
     /** Return A*x */
     Vector operator*(const VectorValues& x) const;
 
+    /** y += alpha * A'*A*x */
+    void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y);
+
     /** x += A'*e.  If x is initially missing any values, they are created and assumed to start as
      *  zero vectors. */
     void transposeMultiplyAdd(double alpha, const Vector& e, VectorValues& x) const;

gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp

@@ -220,6 +220,41 @@ TEST(GaussianFactorGraph, eliminate_empty )
   EXPECT(assert_equal(*remainingGFG, expectedLF));
 }
 
+/* ************************************************************************* */
+//TEST( GaussianFactorGraph, multiplyHessian )
+//{
+//  // A is :
+//  // x1 x2 x3 x4 x5
+//  //  1  2  3  0  0
+//  //  5  6  7  0  0
+//  //  9 10  0 11 12
+//  //  0  0  0 14 15
+//  GaussianFactorGraph A = createSimpleGaussianFactorGraph();
+//
+//  VectorValues x = map_list_of
+//    (0, (Vec(2) << 1,2))
+//    (1, (Vec(2) << 3,4))
+//    (2, (Vec(1) << 5));
+//
+//  // AtA is :
+//  // x1 x2 x3 x4 x5
+//  //  107   122    38    99   108
+//  //  122   140    48   110   120
+//  //   38    48    58     0     0
+//  //   99   110     0   317   342
+//  //  108   120     0   342   369
+//
+//  // AtAx is:
+//  // 1401        1586         308        3297        3561
+//  VectorValues expected;
+//  expected.insert(0, (Vec(2) <<  1401,1586));
+//  expected.insert(1, (Vec(2) << 308,3297));
+//  expected.insert(2, (Vec(1) <<  3561));
+//
+//  VectorValues actual = A.multiplyHessian(x);
+//  EXPECT(assert_equal(expected, actual));
+//}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */