Merge branch 'vectorScatter2' into feature/performance

gtsam/linear/HessianFactor.cpp

@@ -237,11 +237,14 @@ HessianFactor::HessianFactor(const GaussianFactorGraph& factors,
   // Allocate and copy keys
   gttic(allocate);
   // Allocate with dimensions for each variable plus 1 at the end for the information vector
-  keys_.resize(scatter->size());
+  const size_t n = scatter->size();
-  FastVector<DenseIndex> dims(scatter->size() + 1);
+  keys_.resize(n);
-  BOOST_FOREACH(const Scatter::value_type& key_slotentry, *scatter) {
+  FastVector<DenseIndex> dims(n + 1);
-    keys_[key_slotentry.second.slot] = key_slotentry.first;
+  DenseIndex slot = 0;
-    dims[key_slotentry.second.slot] = key_slotentry.second.dimension;
+  BOOST_FOREACH(const SlotEntry& slotentry, *scatter) {
+    keys_[slot] = slotentry.key;
+    dims[slot] = slotentry.dimension;
+    ++slot;
   }
   dims.back() = 1;
   info_ = SymmetricBlockMatrix(dims);

gtsam/linear/Scatter.cpp

@@ -21,6 +21,7 @@
 #include <gtsam/inference/Ordering.h>
 
 #include <boost/foreach.hpp>
+#include <algorithm>
 
 using namespace std;
 
@@ -29,7 +30,7 @@ namespace gtsam {
 /* ************************************************************************* */
 string SlotEntry::toString() const {
   ostringstream oss;
-  oss << "SlotEntry: slot=" << slot << ", dim=" << dimension;
+  oss << "SlotEntry: key=" << key << ", dim=" << dimension;
   return oss.str();
 }
 
@@ -37,43 +38,52 @@ string SlotEntry::toString() const {
 Scatter::Scatter(const GaussianFactorGraph& gfg,
     boost::optional<const Ordering&> ordering) {
   gttic(Scatter_Constructor);
-  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
-        const JacobianFactor* asJacobian =
-            dynamic_cast<const JacobianFactor*>(factor.get());
-        if (!asJacobian || asJacobian->cols() > 1)
-          insert(
-              make_pair(*variable, SlotEntry(none, factor->getDim(variable))));
-      }
-    }
-  }
 
   // If we have an ordering, pre-fill the ordered variables first
-  size_t slot = 0;
   if (ordering) {
     BOOST_FOREACH (Key key, *ordering) {
-      const_iterator entry = find(key);
+      push_back(SlotEntry(key, 0));
-      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.");
-      at(key).slot = (slot++);
     }
   }
 
-  // Next fill in the slot indices (we can only get these after doing the set
+  iterator first = end(); // remember position
-  // union.
+
-  BOOST_FOREACH (value_type& var_slot, *this) {
+  // Now, find dimensions of variables and/or extend
-    if (var_slot.second.slot == none) var_slot.second.slot = (slot++);
+  BOOST_FOREACH (const GaussianFactor::shared_ptr& factor, gfg) {
+    if (!factor) continue;
+
+    // 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) continue;
+
+    // loop over variables
+    for (GaussianFactor::const_iterator variable = factor->begin();
+         variable != factor->end(); ++variable) {
+      const Key key = *variable;
+      iterator it = find(key); // theoretically expensive, yet cache friendly
+      if (it!=end())
+        it->dimension = factor->getDim(variable);
+      else
+        push_back(SlotEntry(key, factor->getDim(variable)));
     }
+  }
+
+  // Filter out keys with zero dimensions (if ordering had more keys)
+  erase(std::remove_if(begin(), end(), SlotEntry::Zero), end());
+
+  // To keep the same behavior as before, sort the keys after the ordering
+  if (first != end()) std::sort(begin(), end());
+}
+
+/* ************************************************************************* */
+FastVector<SlotEntry>::iterator Scatter::find(Key key) {
+  iterator it = begin();
+  while(it != end()) {
+    if (it->key == key)
+      return it;
+    ++it;
+  }
+  return it; // end()
 }
 
 /* ************************************************************************* */

gtsam/linear/Scatter.h

@@ -32,30 +32,32 @@ class Ordering;
 
 /// One SlotEntry stores the slot index for a variable, as well its dim.
 struct GTSAM_EXPORT SlotEntry {
-  DenseIndex slot;
+  Key key;
   size_t dimension;
-  SlotEntry(DenseIndex _slot, size_t _dimension)
+  SlotEntry(Key _key, size_t _dimension) : key(_key), dimension(_dimension) {}
-      : slot(_slot), dimension(_dimension) {}
   std::string toString() const;
+  friend bool operator<(const SlotEntry& p, const SlotEntry& q) {
+    return p.key < q.key;
+  }
+  static bool Zero(const SlotEntry& p) { return p.dimension==0;}
 };
 
 /**
  * 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
+ * incrementally, to get the keys in the right order. In spirit, it is a map
  * 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> {
+class Scatter : public FastVector<SlotEntry> {
  public:
   /// Constructor
   Scatter(const GaussianFactorGraph& gfg,
           boost::optional<const Ordering&> ordering = boost::none);
 
-  /// Get the slot corresponding to the given key
+ private:
-  DenseIndex slot(Key key) const { return at(key).slot; }
 
-  /// Get the dimension corresponding to the given key
+  /// Find the SlotEntry with the right key (linear time worst case)
-  DenseIndex dim(Key key) const { return at(key).dimension; }
+  iterator find(Key key);
 };
 
 }  // \ namespace gtsam