Updating Hessian in-place instead of computing Hessian for each Jacobian

gtsam/linear/HessianFactor.cpp

@@ -382,7 +382,6 @@ void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatte
 
   // First build an array of slots
   gttic(slots);
-  //size_t* slots = (size_t*)alloca(sizeof(size_t)*update.size()); // FIXME: alloca is bad, just ask Google.
   FastVector<DenseIndex> slots(update.size());
   DenseIndex slot = 0;
   BOOST_FOREACH(Key j, update) {
@@ -409,10 +408,63 @@ void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatte
 }
 
 /* ************************************************************************* */
-void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatter)
+void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatter) {
-{
+
-  if(update.rows() > 0) // Zero-row Jacobians are treated specially
+  // This function updates 'combined' with the information in 'update'.
-    updateATA(HessianFactor(update), scatter);
+  // 'scatter' maps variables in the update factor to slots in the combined
+  // factor.
+  
+  gttic(updateATA);
+
+  if(update.rows() > 0)
+  {
+    // First build an array of slots
+    gttic(slots);
+    FastVector<DenseIndex> slots(update.size());
+    DenseIndex slot = 0;
+    BOOST_FOREACH(Key j, update) {
+      slots[slot] = scatter.at(j).slot;
+      ++ slot;
+    }
+    gttoc(slots);
+
+    gttic(whiten);
+    // Whiten the factor if it has a noise model
+    boost::optional<JacobianFactor> _whitenedFactor;
+    const JacobianFactor* whitenedFactor;
+    if(update.get_model())
+    {
+      if(update.get_model()->isConstrained())
+        throw invalid_argument("Cannot update HessianFactor from JacobianFactor with constrained noise model");
+
+      _whitenedFactor = update.whiten();
+      whitenedFactor = &(*_whitenedFactor);
+    }
+    else
+    {
+      whitenedFactor = &update;
+    }
+    gttoc(whiten);
+
+    const VerticalBlockMatrix& updateBlocks = whitenedFactor->matrixObject();
+
+    // Apply updates to the upper triangle
+    gttic(update);
+    for(DenseIndex j2=0; j2<updateBlocks.nBlocks(); ++j2) { // Horizontal block of Hessian
+      DenseIndex slot2 = (j2 == update.size()) ? this->info_.nBlocks()-1 : slots[j2];
+      for(DenseIndex j1=0; j1<=j2; ++j1) { // Vertical block of Hessian
+        DenseIndex slot1 = (j1 == update.size()) ? this->info_.nBlocks()-1 : slots[j1];
+        DenseIndex off0 = updateBlocks.offset(0);
+        if(slot2 > slot1)
+          info_(slot1, slot2).noalias() += updateBlocks(j1).transpose() * updateBlocks(j2);
+        else if(slot1 > slot2)
+          info_(slot2, slot1).noalias() += updateBlocks(j2).transpose() * updateBlocks(j1);
+        else
+          info_(slot1, slot2).triangularView<Eigen::Upper>() += updateBlocks(j1).transpose() * updateBlocks(j2);
+      }
+    }
+    gttoc(update);
+  }
 }
 
 /* ************************************************************************* */

gtsam/linear/JacobianFactor.cpp

@@ -481,8 +481,8 @@ namespace gtsam {
   JacobianFactor JacobianFactor::whiten() const {
     JacobianFactor result(*this);
     if(model_) {
-      result.model_->WhitenInPlace(result.Ab_.matrix());
+      result.model_->WhitenInPlace(result.Ab_.full());
-      result.model_ = noiseModel::Unit::Create(result.model_->dim());
+      result.model_ = SharedDiagonal();
     }
     return result;
   }

gtsam/linear/NoiseModel.cpp

@@ -106,6 +106,11 @@ void Gaussian::WhitenInPlace(Matrix& H) const {
   H = thisR() * H;
 }
 
+/* ************************************************************************* */
+void Gaussian::WhitenInPlace(Eigen::Block<Matrix>& H) const {
+  H = thisR() * H;
+}
+
 /* ************************************************************************* */
 // General QR, see also special version in Constrained
 SharedDiagonal Gaussian::QR(Matrix& Ab) const {
@@ -232,6 +237,11 @@ void Diagonal::WhitenInPlace(Matrix& H) const {
   vector_scale_inplace(invsigmas(), H);
 }
 
+/* ************************************************************************* */
+void Diagonal::WhitenInPlace(Eigen::Block<Matrix>& H) const {
+  H = invsigmas().asDiagonal() * H;
+}
+
 /* ************************************************************************* */
 // Constrained
 /* ************************************************************************* */
@@ -304,6 +314,18 @@ void Constrained::WhitenInPlace(Matrix& H) const {
   vector_scale_inplace(invsigmas(), H, true);
 }
 
+/* ************************************************************************* */
+void Constrained::WhitenInPlace(Eigen::Block<Matrix>& H) const {
+  // selective scaling
+  // Scale row i of H by sigmas[i], basically multiplying H with diag(sigmas)
+  // Set inf_mask flag is true so that if invsigmas[i] is inf, i.e. sigmas[i] = 0,
+  // indicating a hard constraint, we leave H's row i in place.
+  const Vector& _invsigmas = invsigmas();
+  for(DenseIndex row = 0; row < _invsigmas.size(); ++row)
+    if(isfinite(_invsigmas(row)))
+      H.row(row) *= _invsigmas(row);
+}
+
 /* ************************************************************************* */
 Constrained::shared_ptr Constrained::unit(size_t augmentedDim) const {
   Vector sigmas = ones(dim()+augmentedDim);
@@ -443,6 +465,11 @@ void Isotropic::WhitenInPlace(Matrix& H) const {
   H *= invsigma_;
 }
 
+/* ************************************************************************* */
+void Isotropic::WhitenInPlace(Eigen::Block<Matrix>& H) const {
+  H *= invsigma_;
+}
+
 /* ************************************************************************* */
 // Unit
 /* ************************************************************************* */

gtsam/linear/NoiseModel.h

@@ -93,6 +93,16 @@ namespace gtsam {
         v = unwhiten(v);
       }
 
+      /** in-place whiten, override if can be done more efficiently */
+      virtual void whitenInPlace(Eigen::Block<Vector>& v) const {
+        v = whiten(v);
+      }
+
+      /** in-place unwhiten, override if can be done more efficiently */
+      virtual void unwhitenInPlace(Eigen::Block<Vector>& v) const {
+        v = unwhiten(v);
+      }
+
     private:
       /** Serialization function */
       friend class boost::serialization::access;
@@ -186,6 +196,11 @@ namespace gtsam {
        */
       virtual void WhitenInPlace(Matrix& H) const;
 
+      /**
+       * In-place version
+       */
+      virtual void WhitenInPlace(Eigen::Block<Matrix>& H) const;
+
       /**
        * Whiten a system, in place as well
        */
@@ -282,6 +297,7 @@ namespace gtsam {
       virtual Vector unwhiten(const Vector& v) const;
       virtual Matrix Whiten(const Matrix& H) const;
       virtual void WhitenInPlace(Matrix& H) const;
+      virtual void WhitenInPlace(Eigen::Block<Matrix>& H) const;
 
       /**
        * Return standard deviations (sqrt of diagonal)
@@ -431,6 +447,7 @@ namespace gtsam {
       /// with a non-zero sigma.  Other rows remain untouched.
       virtual Matrix Whiten(const Matrix& H) const;
       virtual void WhitenInPlace(Matrix& H) const;
+      virtual void WhitenInPlace(Eigen::Block<Matrix>& H) const;
 
       /**
        * Apply QR factorization to the system [A b], taking into account constraints
@@ -510,6 +527,7 @@ namespace gtsam {
       virtual Vector unwhiten(const Vector& v) const;
       virtual Matrix Whiten(const Matrix& H) const;
       virtual void WhitenInPlace(Matrix& H) const;
+      virtual void WhitenInPlace(Eigen::Block<Matrix>& H) const;
 
       /**
        * Return standard deviation
@@ -557,6 +575,7 @@ namespace gtsam {
       virtual Vector unwhiten(const Vector& v) const { return v; }
       virtual Matrix Whiten(const Matrix& H) const { return H; }
       virtual void WhitenInPlace(Matrix& H) const {}
+      virtual void WhitenInPlace(Eigen::Block<Matrix>& H) const {}
 
     private:
       /** Serialization function */