Moved findScatterAndDims to Scatter class constructor, moved code to combine multiple Hessians into Hessian constructor, and removed need to separately provide a vector of dimensions to this constructor - instead pulls dimensions out of Scatter.

gtsam/linear/GaussianFactorGraph.cpp

@@ -326,51 +326,10 @@ break;
     return make_pair(gbn, jointFactor);
   }
 
-  /* ************************************************************************* */
-  static FastMap<Index, SlotEntry> findScatterAndDims(const FactorGraph<GaussianFactor>& factors) {
-
-    const bool debug = ISDEBUG("findScatterAndDims");
-
-    // The "scatter" is a map from global variable indices to slot indices in the
-    // union of involved variables.  We also include the dimensionality of the
-    // variable.
-
-    Scatter scatter;
-
-    // First do the set union.
-    BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) {
-      if(factor) {
-        for(GaussianFactor::const_iterator variable = factor->begin(); variable != factor->end(); ++variable) {
-          scatter.insert(make_pair(*variable, SlotEntry(0, factor->getDim(variable))));
-        }
-      }
-    }
-
-    // Next fill in the slot indices (we can only get these after doing the set
-    // union.
-    size_t slot = 0;
-    BOOST_FOREACH(Scatter::value_type& var_slot, scatter) {
-      var_slot.second.slot = (slot ++);
-      if(debug)
-        cout << "scatter[" << var_slot.first << "] = (slot " << var_slot.second.slot << ", dim " << var_slot.second.dimension << ")" << endl;
-    }
-
-    return scatter;
-  }
-
   /* ************************************************************************* */
   Matrix GaussianFactorGraph::augmentedHessian() const {
-
-    Scatter scatter = findScatterAndDims(*this);
-
-    vector<size_t> dims; dims.reserve(scatter.size() + 1);
-    BOOST_FOREACH(const Scatter::value_type& index_entry, scatter) {
-      dims.push_back(index_entry.second.dimension);
-    }
-    dims.push_back(1);
-
     // combine all factors and get upper-triangular part of Hessian
-    HessianFactor combined(*this, dims, scatter);
+    HessianFactor combined(*this);
     Matrix result = combined.info();
     // Fill in lower-triangular part of Hessian
     result.triangularView<Eigen::StrictlyLower>() = result.transpose();
@@ -391,24 +350,9 @@ break;
 
     const bool debug = ISDEBUG("EliminateCholesky");
 
-    // Find the scatter and variable dimensions
-    gttic(find_scatter);
-    Scatter scatter(findScatterAndDims(factors));
-    gttoc(find_scatter);
-
-    // Pull out keys and dimensions
-    gttic(keys);
-    vector<size_t> dimensions(scatter.size() + 1);
-    BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) {
-      dimensions[var_slot.second.slot] = var_slot.second.dimension;
-    }
-    // This is for the r.h.s. vector
-    dimensions.back() = 1;
-    gttoc(keys);
-
     // Form Ab' * Ab
     gttic(combine);
-    HessianFactor::shared_ptr combinedFactor(new HessianFactor(factors, dimensions, scatter));
+    HessianFactor::shared_ptr combinedFactor(new HessianFactor(factors));
     gttoc(combine);
 
     // Do Cholesky, note that after this, the lower triangle still contains

gtsam/linear/HessianFactor.cpp

@@ -49,6 +49,25 @@ string SlotEntry::toString() const {
   return oss.str();
 }
 
+/* ************************************************************************* */
+Scatter::Scatter(const FactorGraph<GaussianFactor>& gfg) {
+  // First do the set union.
+  BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, gfg) {
+    if(factor) {
+      for(GaussianFactor::const_iterator variable = factor->begin(); variable != factor->end(); ++variable) {
+        this->insert(make_pair(*variable, SlotEntry(0, factor->getDim(variable))));
+      }
+    }
+  }
+
+  // Next fill in the slot indices (we can only get these after doing the set
+  // union.
+  size_t slot = 0;
+  BOOST_FOREACH(value_type& var_slot, *this) {
+    var_slot.second.slot = (slot ++);
+  }
+}
+
 /* ************************************************************************* */
 void HessianFactor::assertInvariants() const {
   GaussianFactor::assertInvariants(); // The base class checks for unique keys
@@ -245,9 +264,19 @@ HessianFactor::HessianFactor(const GaussianFactor& gf) : info_(matrix_) {
 }
 
 /* ************************************************************************* */
-HessianFactor::HessianFactor(const FactorGraph<GaussianFactor>& factors,
-    const vector<size_t>& dimensions, const Scatter& scatter) :
-    info_(matrix_) {
+HessianFactor::HessianFactor(const FactorGraph<GaussianFactor>& factors) : info_(matrix_)
+{
+  Scatter scatter(factors);
+
+  // Pull out keys and dimensions
+  gttic(keys);
+  vector<size_t> dimensions(scatter.size() + 1);
+  BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) {
+    dimensions[var_slot.second.slot] = var_slot.second.dimension;
+  }
+  // This is for the r.h.s. vector
+  dimensions.back() = 1;
+  gttoc(keys);
 
   const bool debug = ISDEBUG("EliminateCholesky");
   // Form Ab' * Ab

gtsam/linear/HessianFactor.h

@@ -39,6 +39,9 @@ namespace gtsam {
   // Definition of Scatter, which is an intermediate data structure used when
   // building a HessianFactor incrementally, to get the keys in the right
   // order.
+  // The "scatter" is a map from global variable indices to slot indices in the
+  // union of involved variables.  We also include the dimensionality of the
+  // variable.
   struct GTSAM_EXPORT SlotEntry {
     size_t slot;
     size_t dimension;
@@ -46,7 +49,10 @@ namespace gtsam {
     : slot(_slot), dimension(_dimension) {}
     std::string toString() const;
   };
-  typedef FastMap<Index, SlotEntry> Scatter;
+  class Scatter : public FastMap<Index, SlotEntry> {
+  public:
+    Scatter(const FactorGraph<GaussianFactor>& gfg);
+  };
 
   /**
    * @brief A Gaussian factor using the canonical parameters (information form)
@@ -187,9 +193,8 @@ namespace gtsam {
     /** Convert from a JacobianFactor (computes A^T * A) or HessianFactor */
     explicit HessianFactor(const GaussianFactor& factor);
 
-    /** Special constructor used in EliminateCholesky which combines the given factors */
-    HessianFactor(const FactorGraph<GaussianFactor>& factors,
-        const std::vector<size_t>& dimensions, const Scatter& scatter);
+    /** Combine a set of factors into a single dense HessianFactor */
+    HessianFactor(const FactorGraph<GaussianFactor>& factors);
 
     /** Destructor */
     virtual ~HessianFactor() {}

gtsam/linear/tests/testHessianFactor.cpp

@@ -615,16 +615,8 @@ TEST(HessianFactor, combine) {
   FactorGraph<GaussianFactor> factors;
   factors.push_back(f);
 
-  vector<size_t> dimensions;
-  dimensions += 2, 2, 2, 1;
-
-  Scatter scatter;
-  scatter += make_pair(0, SlotEntry(0, 2));
-  scatter += make_pair(1, SlotEntry(1, 2));
-  scatter += make_pair(2, SlotEntry(2, 2));
-
   // Form Ab' * Ab
-  HessianFactor actual(factors, dimensions, scatter);
+  HessianFactor actual(factors);
 
   Matrix expected = Matrix_(7, 7,
   125.0000,       0.0,  -25.0000,       0.0, -100.0000,       0.0,   25.0000,