Merge pull request #337 from borglab/feature/fast_hessian

Switched to in-place update of the diagonal Hessian

gtsam/config.h.in

@@ -42,6 +42,9 @@
 // Whether we are using TBB (if TBB was found and GTSAM_WITH_TBB is enabled in CMake)
 #cmakedefine GTSAM_USE_TBB
 
+// Whether we are using a TBB version higher than 2020
+#cmakedefine TBB_GREATER_EQUAL_2020
+
 // Whether we are using system-Eigen or our own patched version
 #cmakedefine GTSAM_USE_SYSTEM_EIGEN
 

gtsam/linear/GaussianFactor.cpp

@@ -0,0 +1,35 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    GaussianFactor.cpp
+ * @brief   A factor with a quadratic error function - a Gaussian
+ * @brief   GaussianFactor
+ * @author  Fan Jiang
+ */
+
+// \callgraph
+
+#pragma once
+
+#include <gtsam/linear/GaussianFactor.h>
+#include <gtsam/linear/VectorValues.h>
+
+namespace gtsam {
+
+/* ************************************************************************* */
+  VectorValues GaussianFactor::hessianDiagonal() const {
+    VectorValues d;
+    hessianDiagonalAdd(d);
+    return d;
+  }
+
+}

gtsam/linear/GaussianFactor.h

@@ -100,7 +100,10 @@ namespace gtsam {
     virtual Matrix information() const = 0;
 
     /// Return the diagonal of the Hessian for this factor
-    virtual VectorValues hessianDiagonal() const = 0;
+    VectorValues hessianDiagonal() const;
+
+    /// Add the current diagonal to a VectorValues instance
+    virtual void hessianDiagonalAdd(VectorValues& d) const = 0;
 
     /// Raw memory access version of hessianDiagonal
     virtual void hessianDiagonal(double* d) const = 0;

gtsam/linear/GaussianFactorGraph.cpp

@@ -255,8 +255,7 @@ namespace gtsam {
     VectorValues d;
     for (const sharedFactor& factor : *this) {
       if(factor){
-        VectorValues di = factor->hessianDiagonal();
-        d.addInPlace_(di);
+        factor->hessianDiagonalAdd(d);
       }
     }
     return d;

gtsam/linear/HessianFactor.cpp

@@ -302,12 +302,14 @@ Matrix HessianFactor::information() const {
 }
 
 /* ************************************************************************* */
-VectorValues HessianFactor::hessianDiagonal() const {
-  VectorValues d;
+void HessianFactor::hessianDiagonalAdd(VectorValues &d) const {
   for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) {
-    d.emplace(keys_[j], info_.diagonal(j));
+    auto result = d.emplace(keys_[j], info_.diagonal(j));
+    if(!result.second) {
+      // if emplace fails, it returns an iterator to the existing element, which we add to:
+      result.first->second += info_.diagonal(j);
+    }
   }
-  return d;
 }
 
 /* ************************************************************************* */

gtsam/linear/HessianFactor.h

@@ -293,8 +293,11 @@ namespace gtsam {
      */
     Matrix information() const override;
 
-    /// Return the diagonal of the Hessian for this factor
-    VectorValues hessianDiagonal() const override;
+    /// Add the current diagonal to a VectorValues instance
+    void hessianDiagonalAdd(VectorValues& d) const override;
+
+    /// Using the base method
+    using Base::hessianDiagonal;
 
     /// Raw memory access version of hessianDiagonal
     void hessianDiagonal(double* d) const override;

gtsam/linear/JacobianFactor.cpp

@@ -542,21 +542,31 @@ Matrix JacobianFactor::information() const {
 }
 
 /* ************************************************************************* */
-VectorValues JacobianFactor::hessianDiagonal() const {
-  VectorValues d;
+void JacobianFactor::hessianDiagonalAdd(VectorValues& d) const {
   for (size_t pos = 0; pos < size(); ++pos) {
     Key j = keys_[pos];
     size_t nj = Ab_(pos).cols();
-    Vector dj(nj);
+    auto result = d.emplace(j, nj);
+
+    Vector& dj = result.first->second;
+
     for (size_t k = 0; k < nj; ++k) {
-      Vector column_k = Ab_(pos).col(k);
-      if (model_)
-        model_->whitenInPlace(column_k);
-      dj(k) = dot(column_k, column_k);
+      Eigen::Ref<const Vector> column_k = Ab_(pos).col(k);
+      if (model_) {
+        Vector column_k_copy = column_k;
+        model_->whitenInPlace(column_k_copy);
+        if(!result.second)
+          dj(k) += dot(column_k_copy, column_k_copy);
+        else
+          dj(k) = dot(column_k_copy, column_k_copy);
+      } else {
+        if (!result.second)
+          dj(k) += dot(column_k, column_k);
+        else
+          dj(k) = dot(column_k, column_k);
+      }
     }
-    d.emplace(j, dj);
   }
-  return d;
 }
 
 /* ************************************************************************* */

gtsam/linear/JacobianFactor.h

@@ -215,8 +215,11 @@ namespace gtsam {
      */
     Matrix information() const override;
 
-    /// Return the diagonal of the Hessian for this factor
-    VectorValues hessianDiagonal() const override;
+    /// Using the base method
+    using Base::hessianDiagonal;
+
+    /// Add the current diagonal to a VectorValues instance
+    void hessianDiagonalAdd(VectorValues& d) const override;
 
     /// Raw memory access version of hessianDiagonal
     void hessianDiagonal(double* d) const override;

gtsam/linear/RegularJacobianFactor.h

@@ -102,10 +102,8 @@ public:
       DMap(y + D * keys_[pos]) += Ab_(pos).transpose() * Ax;
   }
 
-  /// Expose base class hessianDiagonal
-  virtual VectorValues hessianDiagonal() const {
-    return JacobianFactor::hessianDiagonal();
-  }
+  /// Using the base method
+  using GaussianFactor::hessianDiagonal;
 
   /// Raw memory access version of hessianDiagonal
   void hessianDiagonal(double* d) const {

gtsam/linear/VectorValues.cpp

@@ -96,20 +96,6 @@ namespace gtsam {
     return result.first;
   }
 
-  /* ************************************************************************* */
-  VectorValues::iterator VectorValues::emplace(Key j, const Vector& value) {
-#ifdef TBB_GREATER_EQUAL_2020
-    std::pair<iterator, bool> result = values_.emplace(j, value);
-#else
-    std::pair<iterator, bool> result = values_.insert(std::make_pair(j, value));
-#endif
-    if(!result.second)
-      throw std::invalid_argument(
-      "Requested to emplace variable '" + DefaultKeyFormatter(j)
-      + "' already in this VectorValues.");
-    return result.first;
-  }
-
   /* ************************************************************************* */
   void VectorValues::update(const VectorValues& values)
   {

gtsam/linear/VectorValues.h

@@ -179,7 +179,14 @@ namespace gtsam {
      *  j is already used.
      * @param value The vector to be inserted.
      * @param j The index with which the value will be associated. */
-    iterator emplace(Key j, const Vector& value);
+    template<class... Args>
+    inline std::pair<VectorValues::iterator, bool> emplace(Key j, Args&&... args) {
+#if ! defined(GTSAM_USE_TBB) || defined (TBB_GREATER_EQUAL_2020)
+      return values_.emplace(std::piecewise_construct, std::forward_as_tuple(j), std::forward_as_tuple(args...));
+#else
+      return values_.insert(std::make_pair(j, Vector(std::forward<Args>(args)...)));
+#endif
+    }
 
     /** Insert a vector \c value with key \c j.  Throws an invalid_argument exception if the key \c
      *  j is already used.
@@ -197,7 +204,7 @@ namespace gtsam {
      *  and an iterator to the existing value is returned, along with 'false'.  If the value did not
      *  exist, it is inserted and an iterator pointing to the new element, along with 'true', is
      *  returned. */
-    std::pair<iterator, bool> tryInsert(Key j, const Vector& value) {
+    inline std::pair<iterator, bool> tryInsert(Key j, const Vector& value) {
 #ifdef TBB_GREATER_EQUAL_2020
       return values_.emplace(j, value);
 #else

gtsam/linear/linearAlgorithms-inst.h

@@ -98,8 +98,13 @@ namespace gtsam
             // Insert solution into a VectorValues
             DenseIndex vectorPosition = 0;
             for(GaussianConditional::const_iterator frontal = c.beginFrontals(); frontal != c.endFrontals(); ++frontal) {
-              VectorValues::const_iterator r =
-                collectedResult.emplace(*frontal, solution.segment(vectorPosition, c.getDim(frontal)));
+              auto result = collectedResult.emplace(*frontal, solution.segment(vectorPosition, c.getDim(frontal)));
+              if(!result.second)
+                  throw std::runtime_error(
+                      "Internal error while optimizing clique. Trying to insert key '" + DefaultKeyFormatter(*frontal)
+                      + "' that exists.");
+
+              VectorValues::const_iterator r = result.first;
               myData.cliqueResults.emplace(r->first, r);
               vectorPosition += c.getDim(frontal);
             }

gtsam/slam/RegularImplicitSchurFactor.h

@@ -141,11 +141,12 @@ public:
     return augmented.block(0, 0, M, M);
   }
 
-  /// Return the diagonal of the Hessian for this factor
-  virtual VectorValues hessianDiagonal() const {
-    // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F);
-    VectorValues d;
+  /// Using the base method
+  using GaussianFactor::hessianDiagonal;
 
+  /// Add the diagonal of the Hessian for this factor to existing VectorValues
+  virtual void hessianDiagonalAdd(VectorValues &d) const override {
+    // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F);
     for (size_t k = 0; k < size(); ++k) { // for each camera
       Key j = keys_[k];
 
@@ -153,7 +154,7 @@ public:
       // D x 3 = (D x ZDim) * (ZDim x 3)
       const MatrixZD& Fj = FBlocks_[k];
       Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
-          * E_.block<ZDim, 3>(ZDim * k, 0);
+                                        * E_.block<ZDim, 3>(ZDim * k, 0);
 
       Eigen::Matrix<double, D, 1> dj;
       for (int k = 0; k < D; ++k) { // for each diagonal element of the camera hessian
@@ -163,9 +164,12 @@ public:
         // (1 x 1) = (1 x 3) * (3 * 3) * (3 x 1)
         dj(k) -= FtE.row(k) * PointCovariance_ * FtE.row(k).transpose();
       }
-      d.insert(j, dj);
+
+      auto result = d.emplace(j, dj);
+      if(!result.second) {
+        result.first->second += dj;
+      }
     }
-    return d;
   }
 
   /**