Merge branch 'slotMethod' into feature/LinearSolverCleanup

gtsam/linear/HessianFactor.cpp

@@ -349,21 +349,12 @@ double HessianFactor::error(const VectorValues& c) const {
 void HessianFactor::updateHessian(const Scatter& scatter,
                                   SymmetricBlockMatrix* info) const {
   gttic(updateHessian_HessianFactor);
-  // N is number of variables in information matrix, n in HessianFactor
-  DenseIndex N = info->nBlocks() - 1, n = size();
-
-  // 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
+  DenseIndex n = size(), N = info->nBlocks()-1;
   for (DenseIndex j = 0; j <= n; ++j) {
-    DenseIndex J = slots[j];
+    const DenseIndex J = j==n ? N : scatter.slot(keys_[j]);
     for (DenseIndex i = 0; i <= j; ++i) {
-      DenseIndex I = slots[i];
+      const DenseIndex I = i==n ? N : scatter.slot(keys_[i]);
       (*info)(I, J) += info_(i, j);
     }
   }

gtsam/linear/JacobianFactor.cpp

@@ -515,23 +515,15 @@ void JacobianFactor::updateHessian(const Scatter& scatter,
     whitenedFactor.updateHessian(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;
+    DenseIndex n = Ab_.nBlocks() - 1, N = info->nBlocks() - 1;
 
     // 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];
+      const DenseIndex J = j==n ? N : scatter.slot(keys_[j]);
       // Fill off-diagonal blocks with Ai'*Aj
       for (DenseIndex i = 0; i < j; ++i) {
-        DenseIndex I = slots[i];
+        const DenseIndex I = scatter.slot(keys_[i]);
         (*info)(I, J).knownOffDiagonal() += Ab_(i).transpose() * Ab_(j);
       }
       // Fill diagonal block with Aj'*Aj

gtsam/linear/Scatter.cpp

@@ -36,14 +36,15 @@ string SlotEntry::toString() const {
 Scatter::Scatter(const GaussianFactorGraph& gfg,
                  boost::optional<const Ordering&> ordering) {
   gttic(Scatter_Constructor);
-  static const size_t none = std::numeric_limits<size_t>::max();
+  static const DenseIndex none = std::numeric_limits<size_t>::max();
 
   // 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) {
-        // TODO: Fix this hack to cope with zero-row Jacobians that come from BayesTreeOrphanWrappers
+        // TODO: Fix this hack to cope with zero-row Jacobians that come from
+        // BayesTreeOrphanWrappers
         const JacobianFactor* asJacobian =
             dynamic_cast<const JacobianFactor*>(factor.get());
         if (!asJacobian || asJacobian->cols() > 1)
@@ -61,7 +62,8 @@ Scatter::Scatter(const GaussianFactorGraph& gfg,
       if (entry == end())
         throw std::invalid_argument(
             "The ordering provided to the HessianFactor Scatter constructor\n"
-                "contained extra variables that did not appear in the factors to combine.");
+            "contained extra variables that did not appear in the factors to "
+            "combine.");
       at(key).slot = (slot++);
     }
   }
@@ -69,9 +71,22 @@ Scatter::Scatter(const GaussianFactorGraph& gfg,
   // Next fill in the slot indices (we can only get these after doing the set
   // union.
   BOOST_FOREACH (value_type& var_slot, *this) {
-    if (var_slot.second.slot == none)
-      var_slot.second.slot = (slot++);
+    if (var_slot.second.slot == none) var_slot.second.slot = (slot++);
   }
 }
 
+/* ************************************************************************* */
+FastVector<DenseIndex> Scatter::getSlotsForKeys(
+    const FastVector<Key>& keys) const {
+  gttic(getSlotsForKeys);
+  FastVector<DenseIndex> slots(keys.size() + 1);
+  DenseIndex slot = 0;
+  BOOST_FOREACH (Key key, keys)
+    slots[slot++] = at(key).slot;
+  slots.back() = size();
+  return slots;
+}
+
+/* ************************************************************************* */
+
 }  // gtsam

gtsam/linear/Scatter.h

@@ -30,12 +30,11 @@ namespace gtsam {
 class GaussianFactorGraph;
 class Ordering;
 
-/**
- * One SlotEntry stores the slot index for a variable, as well its dimension.
- */
+/// One SlotEntry stores the slot index for a variable, as well its dimension.
 struct GTSAM_EXPORT SlotEntry {
-  size_t slot, dimension;
-  SlotEntry(size_t _slot, size_t _dimension)
+  DenseIndex slot;
+  size_t dimension;
+  SlotEntry(DenseIndex _slot, size_t _dimension)
       : slot(_slot), dimension(_dimension) {}
   std::string toString() const;
 };
@@ -43,14 +42,21 @@ struct GTSAM_EXPORT SlotEntry {
 /**
  * Scatter 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.
+ * from global variable indices to slot indices in the union of involved
+ * variables. We also include the dimensionality of the variable.
  */
 class Scatter : public FastMap<Key, SlotEntry> {
  public:
   Scatter(const GaussianFactorGraph& gfg,
           boost::optional<const Ordering&> ordering = boost::none);
+
+  DenseIndex slot(Key key) const { return at(key).slot; }
+
+  /**
+   * For the subset of keys given, return the slots in the same order,
+   * terminated by the a RHS slot equal to N, the size of the Scatter
+   */
+  FastVector<DenseIndex> getSlotsForKeys(const FastVector<Key>& keys) const;
 };
 
 }  // \ namespace gtsam

gtsam/slam/GeneralSFMFactor.h

@@ -166,8 +166,8 @@ namespace gtsam {
           DenseIndex N = info->nBlocks() - 1;
 
           // First build an array of slots
-          DenseIndex slotC = scatter.at(this->keys().front()).slot;
-          DenseIndex slotL = scatter.at(this->keys().back()).slot;
+          DenseIndex slotC = scatter.slot(keys_.front());
+          DenseIndex slotL = scatter.slot(keys_.back());
           DenseIndex slotB = N;
 
           // We perform I += A'*A to the upper triangle