Added hessianDiagonal; works for Gaussian factor graphs with JacobianFactors

gtsam/linear/GaussianFactor.h

@@ -96,6 +96,9 @@ namespace gtsam {
      */
     virtual Matrix information() const = 0;
 
+    /// Return the diagonal of the Hessian for this factor
+    virtual VectorValues hessianDiagonal() const = 0;
+
     /** Clone a factor (make a deep copy) */
     virtual GaussianFactor::shared_ptr clone() const = 0;
 

gtsam/linear/GaussianFactorGraph.cpp

@@ -196,6 +196,16 @@ namespace gtsam {
         augmented.col(augmented.rows() - 1).head(augmented.rows() - 1));
   }
 
+  /* ************************************************************************* */
+  VectorValues GaussianFactorGraph::hessianDiagonal() const {
+    VectorValues d;
+    BOOST_FOREACH(const sharedFactor& factor, *this) {
+      VectorValues di = factor->hessianDiagonal();
+      d.addInPlace_(di);
+    }
+    return d;
+  }
+
   /* ************************************************************************* */
   VectorValues GaussianFactorGraph::optimize(OptionalOrdering ordering, const Eliminate& function) const
   {

gtsam/linear/GaussianFactorGraph.h

@@ -221,6 +221,9 @@ namespace gtsam {
      */
     std::pair<Matrix,Vector> hessian(boost::optional<const Ordering&> optionalOrdering = boost::none) const;
 
+    /** Return only the diagonal of the Hessian A'*A, as a VectorValues */
+    VectorValues hessianDiagonal() const;
+
     /** Solve the factor graph by performing multifrontal variable elimination in COLAMD order using
      *  the dense elimination function specified in \c function (default EliminatePreferCholesky),
      *  followed by back-substitution in the Bayes tree resulting from elimination.  Is equivalent

gtsam/linear/HessianFactor.cpp

@@ -351,6 +351,12 @@ Matrix HessianFactor::information() const
   return info_.range(0, this->size(), 0, this->size()).selfadjointView();
 }
 
+/* ************************************************************************* */
+VectorValues HessianFactor::hessianDiagonal() const {
+  VectorValues d;
+  return d;
+}
+
 /* ************************************************************************* */
 Matrix HessianFactor::augmentedJacobian() const
 {

gtsam/linear/HessianFactor.h

@@ -335,7 +335,10 @@ namespace gtsam {
      * GaussianFactor.
      */
     virtual Matrix information() const;
-        
+
+    /// Return the diagonal of the Hessian for this factor
+    virtual VectorValues hessianDiagonal() const;
+
     /**
      * Return (dense) matrix associated with factor
      * @param ordering of variables needed for matrix column order

gtsam/linear/JacobianFactor.cpp

@@ -438,6 +438,23 @@ namespace gtsam {
     }
   }
 
+  /* ************************************************************************* */
+  VectorValues JacobianFactor::hessianDiagonal() const {
+    VectorValues d;
+    for(size_t pos=0; pos<size(); ++pos)
+    {
+      Key j = keys_[pos];
+      size_t nj = Ab_(pos).cols();
+      Vector dj(nj);
+      for (size_t k = 0; k < nj; ++k) {
+        Vector column_k = Ab_(pos).col(k);
+        dj(k) = dot(column_k,column_k);
+      }
+      d.insert(j,dj);
+    }
+    return d;
+  }
+
   /* ************************************************************************* */
   Vector JacobianFactor::operator*(const VectorValues& x) const {
     Vector Ax = zero(Ab_.rows());
@@ -458,8 +475,9 @@ namespace gtsam {
     // Just iterate over all A matrices and insert Ai^e into VectorValues
     for(size_t pos=0; pos<size(); ++pos)
     {
+      Key j = keys_[pos];
       // Create the value as a zero vector if it does not exist.
-      pair<VectorValues::iterator, bool> xi = x.tryInsert(keys_[pos], Vector());
+      pair<VectorValues::iterator, bool> xi = x.tryInsert(j, Vector());
       if(xi.second)
         xi.first->second = Vector::Zero(getDim(begin() + pos));
       gtsam::transposeMultiplyAdd(Ab_(pos), E, xi.first->second);

gtsam/linear/JacobianFactor.h

@@ -181,6 +181,9 @@ namespace gtsam {
      */
     virtual Matrix information() const;
     
+    /// Return the diagonal of the Hessian for this factor
+    virtual VectorValues hessianDiagonal() const;
+
     /**
      * @brief Returns (dense) A,b pair associated with factor, bakes in the weights
      */

gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp

@@ -139,10 +139,10 @@ TEST(GaussianFactorGraph, matrices) {
   EXPECT(assert_equal(L, actualL));
   EXPECT(assert_equal(eta, actualeta));
 
-  Vector expectLdiagonal(5); // Make explicit that diagonal is sum-squares of columns
+  VectorValues expectLdiagonal; // Make explicit that diagonal is sum-squares of columns
-  expectLdiagonal << 1+25+81, 4+36+100, 9+49, 121+196, 144+225;
+  expectLdiagonal.insert(0, (Vector(3) << 1+25+81, 4+36+100, 9+49));
-  EXPECT(assert_equal(L.diagonal(), expectLdiagonal));
+  expectLdiagonal.insert(1, (Vector(2) << 121+196, 144+225));
-  // EXPECT(assert_equal(expectLdiagonal, gfg.hessianDiagonal()));
+  EXPECT(assert_equal(expectLdiagonal, gfg.hessianDiagonal()));
 }
 
 /* ************************************************************************* */