Made updateATA a virtual method for a small saving in CPU, but more importantly to allow for custom Jacobian or HessianFactors...

gtsam/linear/GaussianFactor.h

@@ -28,6 +28,8 @@ namespace gtsam {
 
   // Forward declarations
   class VectorValues;
+  class Scatter;
+  class SymmetricBlockMatrix;
 
   /**
    * An abstract virtual base class for JacobianFactor and HessianFactor. A GaussianFactor has a
@@ -119,6 +121,14 @@ namespace gtsam {
      */
     virtual GaussianFactor::shared_ptr negate() const = 0;
 
+    /** Update an information matrix by adding the information corresponding to this factor
+     * (used internally during elimination).
+     * @param scatter A mapping from variable index to slot index in this HessianFactor
+     * @param info The information matrix to be updated
+     */
+    virtual void updateATA(const Scatter& scatter,
+                           SymmetricBlockMatrix* info) const = 0;
+
     /// y += alpha * A'*A*x
     virtual void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const = 0;
 

gtsam/linear/HessianFactor.cpp

@@ -75,7 +75,7 @@ Scatter::Scatter(const GaussianFactorGraph& gfg,
         const JacobianFactor* asJacobian =
             dynamic_cast<const JacobianFactor*>(factor.get());
         if (!asJacobian || asJacobian->cols() > 1)
-          this->insert(
+          insert(
               make_pair(*variable, SlotEntry(none, factor->getDim(variable))));
       }
     }
@@ -296,16 +296,8 @@ HessianFactor::HessianFactor(const GaussianFactorGraph& factors,
   // Form A' * A
   gttic(update);
   BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors)
-  {
+    if(factor)
-    if(factor) {
+      factor->updateATA(*scatter, &info_);
-      if(const HessianFactor* hessian = dynamic_cast<const HessianFactor*>(factor.get()))
-        updateATA(*hessian, *scatter);
-      else if(const JacobianFactor* jacobian = dynamic_cast<const JacobianFactor*>(factor.get()))
-        updateATA(*jacobian, *scatter);
-      else
-        throw invalid_argument("GaussianFactor is neither Hessian nor Jacobian");
-    }
-  }
   gttoc(update);
 }
 
@@ -313,8 +305,8 @@ HessianFactor::HessianFactor(const GaussianFactorGraph& factors,
 void HessianFactor::print(const std::string& s, const KeyFormatter& formatter) const {
   cout << s << "\n";
   cout << " keys: ";
-  for(const_iterator key=this->begin(); key!=this->end(); ++key)
+  for(const_iterator key=begin(); key!=end(); ++key)
-    cout << formatter(*key) << "(" << this->getDim(key) << ") ";
+    cout << formatter(*key) << "(" << getDim(key) << ") ";
   cout << "\n";
   gtsam::print(Matrix(info_.full().selfadjointView()), "Augmented information matrix: ");
 }
@@ -326,7 +318,7 @@ bool HessianFactor::equals(const GaussianFactor& lf, double tol) const {
   else {
     if(!Factor::equals(lf, tol))
       return false;
-    Matrix thisMatrix = this->info_.full().selfadjointView();
+    Matrix thisMatrix = info_.full().selfadjointView();
     thisMatrix(thisMatrix.rows()-1, thisMatrix.cols()-1) = 0.0;
     Matrix rhsMatrix = static_cast<const HessianFactor&>(lf).info_.full().selfadjointView();
     rhsMatrix(rhsMatrix.rows()-1, rhsMatrix.cols()-1) = 0.0;
@@ -343,7 +335,7 @@ Matrix HessianFactor::augmentedInformation() const
 /* ************************************************************************* */
 Matrix HessianFactor::information() const
 {
-  return info_.range(0, this->size(), 0, this->size()).selfadjointView();
+  return info_.range(0, size(), 0, size()).selfadjointView();
 }
 
 /* ************************************************************************* */
@@ -396,97 +388,34 @@ double HessianFactor::error(const VectorValues& c) const {
   double xtg = 0, xGx = 0;
   // extract the relevant subset of the VectorValues
   // NOTE may not be as efficient
-  const Vector x = c.vector(this->keys());
+  const Vector x = c.vector(keys());
   xtg = x.dot(linearTerm());
-  xGx = x.transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView() *  x;
+  xGx = x.transpose() * info_.range(0, size(), 0, size()).selfadjointView() *  x;
   return 0.5 * (f - 2.0 * xtg +  xGx);
 }
 
 /* ************************************************************************* */
-void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatter)
+void HessianFactor::updateATA(const Scatter& scatter,
-{
+                              SymmetricBlockMatrix* info) const {
   gttic(updateATA_HessianFactor);
-  // This function updates 'combined' with the information in 'update'. 'scatter' maps variables in
+  // N is number of variables in information matrix, n in HessianFactor
-  // the update factor to slots in the combined factor.
+  DenseIndex N = info->nBlocks() - 1, n = size();
 
   // First build an array of slots
-  gttic(slots);
+  FastVector<DenseIndex> slots(n + 1);
-  FastVector<DenseIndex> slots(update.size());
   DenseIndex slot = 0;
-  BOOST_FOREACH(Key j, update) {
+  BOOST_FOREACH (Key key, *this)
-    slots[slot] = scatter.at(j).slot;
+    slots[slot++] = scatter.at(key).slot;
-    ++ slot;
+  slots[n] = N;
-  }
-  gttoc(slots);
 
   // Apply updates to the upper triangle
-  gttic(update);
+  for (DenseIndex j = 0; j <= n; ++j) {
-  size_t nrInfoBlocks = this->info_.nBlocks();
+    DenseIndex J = slots[j];
-  for(DenseIndex j2=0; j2<update.info_.nBlocks(); ++j2) {
+    for (DenseIndex i = 0; i <= j; ++i) {
-    DenseIndex slot2 = (j2 == (DenseIndex)update.size()) ? nrInfoBlocks-1 : slots[j2];
+      DenseIndex I = slots[i];
-    for(DenseIndex j1=0; j1<=j2; ++j1) {
+      (*info)(I, J) += info_(i, j);
-      DenseIndex slot1 = (j1 == (DenseIndex)update.size()) ? nrInfoBlocks-1 : slots[j1];
-      info_(slot1, slot2) += update.info_(j1, j2);
     }
   }
-  gttoc(update);
-}
-
-/* ************************************************************************* */
-void HessianFactor::updateATA(const JacobianFactor& update,
-    const Scatter& scatter) {
-
-  // This function updates 'combined' with the information in 'update'.
-  // 'scatter' maps variables in the update factor to slots in the combined
-  // factor.
-
-  gttic(updateATA_JacobianFactor);
-
-  if (update.rows() > 0) {
-    gttic(whiten);
-    // Whiten the factor if it has a noise model
-    boost::optional<JacobianFactor> _whitenedFactor;
-    const JacobianFactor* whitenedFactor = &update;
-    const SharedDiagonal& model = update.get_model();
-    if (model && !model->isUnit()) {
-      if (model->isConstrained())
-        throw invalid_argument(
-            "Cannot update HessianFactor from JacobianFactor with constrained noise model");
-      _whitenedFactor = update.whiten();
-      whitenedFactor = &(*_whitenedFactor);
-    }
-    gttoc(whiten);
-
-    // A is the whitened Jacobian matrix A, and we are going to perform I += A'*A below
-    const VerticalBlockMatrix& Ab = whitenedFactor->matrixObject();
-
-    // N is number of variables in HessianFactor, n in JacobianFactor
-    DenseIndex N = this->info_.nBlocks() - 1, n = Ab.nBlocks() - 1;
-
-    // First build an array of slots
-    gttic(slots);
-    FastVector<DenseIndex> slots(n + 1);
-    DenseIndex slot = 0;
-    BOOST_FOREACH(Key j, update)
-      slots[slot++] = scatter.at(j).slot;
-    slots[n] = N;
-    gttoc(slots);
-
-    // Apply updates to the upper triangle
-    gttic(update);
-    // Loop over blocks of A, including RHS with j==n
-    for (DenseIndex j = 0; j <= n; ++j) {
-      DenseIndex J = slots[j]; // get block in Hessian
-      // Fill off-diagonal blocks with Ai'*Aj
-      for (DenseIndex i = 0; i < j; ++i) {
-        DenseIndex I = slots[i]; // get block in Hessian
-        info_(I, J).knownOffDiagonal() += Ab(i).transpose() * Ab(j);
-      }
-      // Fill diagonal block with Aj'*Aj
-      info_(J, J).selfadjointView().rankUpdate(Ab(j).transpose());
-    }
-    gttoc(update);
-  }
 }
 
 /* ************************************************************************* */

gtsam/linear/HessianFactor.h

@@ -363,19 +363,12 @@ namespace gtsam {
     /** Return the full augmented Hessian matrix of this factor as a SymmetricBlockMatrix object. */
     const SymmetricBlockMatrix& matrixObject() const { return info_; }
 
-    /** Update the factor by adding the information from the JacobianFactor
+    /** Update an information matrix by adding the information corresponding to this factor
      * (used internally during elimination).
-     * @param update The JacobianFactor containing the new information to add
      * @param scatter A mapping from variable index to slot index in this HessianFactor
+     * @param info The information matrix to be updated
      */
-    void updateATA(const JacobianFactor& update, const Scatter& scatter);
+    void updateATA(const Scatter& scatter, SymmetricBlockMatrix* info) const;
-
-    /** Update the factor by adding the information from the HessianFactor
-     * (used internally during elimination).
-     * @param update The HessianFactor containing the new information to add
-     * @param scatter A mapping from variable index to slot index in this HessianFactor
-     */
-    void updateATA(const HessianFactor& update, const Scatter& scatter);
 
     /** y += alpha * A'*A*x */
     void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const;

gtsam/linear/JacobianFactor.cpp

@@ -497,6 +497,49 @@ map<Key, Matrix> JacobianFactor::hessianBlockDiagonal() const {
   return blocks;
 }
 
+/* ************************************************************************* */
+void JacobianFactor::updateATA(const Scatter& scatter,
+                               SymmetricBlockMatrix* info) const {
+  gttic(updateATA_JacobianFactor);
+
+  if (rows() == 0) return;
+
+  // Whiten the factor if it has a noise model
+  const SharedDiagonal& model = get_model();
+  if (model && !model->isUnit()) {
+    if (model->isConstrained())
+      throw invalid_argument(
+          "JacobianFactor::updateATA: cannot update information with "
+          "constrained noise model");
+    JacobianFactor whitenedFactor = whiten();
+    whitenedFactor.updateATA(scatter, info);
+  } else {
+    // Ab_ is the augmented Jacobian matrix A, and we perform I += A'*A below
+    // N is number of variables in information matrix, n in JacobianFactor
+    DenseIndex N = info->nBlocks() - 1, n = Ab_.nBlocks() - 1;
+
+    // First build an array of slots
+    FastVector<DenseIndex> slots(n + 1);
+    DenseIndex slot = 0;
+    BOOST_FOREACH (Key key, *this)
+      slots[slot++] = scatter.at(key).slot;
+    slots[n] = N;
+
+    // Apply updates to the upper triangle
+    // Loop over blocks of A, including RHS with j==n
+    for (DenseIndex j = 0; j <= n; ++j) {
+      DenseIndex J = slots[j];
+      // Fill off-diagonal blocks with Ai'*Aj
+      for (DenseIndex i = 0; i < j; ++i) {
+        DenseIndex I = slots[i];
+        (*info)(I, J).knownOffDiagonal() += Ab_(i).transpose() * Ab_(j);
+      }
+      // Fill diagonal block with Aj'*Aj
+      (*info)(J, J).selfadjointView().rankUpdate(Ab_(j).transpose());
+    }
+  }
+}
+
 /* ************************************************************************* */
 Vector JacobianFactor::operator*(const VectorValues& x) const {
   Vector Ax = zero(Ab_.rows());

gtsam/linear/JacobianFactor.h

@@ -273,6 +273,13 @@ namespace gtsam {
     /** Get a view of the A matrix */
     ABlock getA() { return Ab_.range(0, size()); }
 
+    /** Update an information matrix by adding the information corresponding to this factor
+     * (used internally during elimination).
+     * @param scatter A mapping from variable index to slot index in this HessianFactor
+     * @param info The information matrix to be updated
+     */
+    void updateATA(const Scatter& scatter, SymmetricBlockMatrix* info) const;
+
     /** Return A*x */
     Vector operator*(const VectorValues& x) const;