Some usage (still more to do) of sparse and partial permutations in iSAM2 partial reordering to improve asymptotic performance.

gtsam/inference/BayesNet-inl.h

@@ -118,16 +118,16 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  template<class CONDITIONAL>
+  //template<class CONDITIONAL>
-  bool BayesNet<CONDITIONAL>::permuteSeparatorWithInverse(
+  //bool BayesNet<CONDITIONAL>::permuteSeparatorWithInverse(
-      const Permutation& inversePermutation) {
+  //    const Permutation& inversePermutation) {
-    bool separatorChanged = false;
+  //  bool separatorChanged = false;
-    BOOST_FOREACH(sharedConditional conditional, conditionals_) {
+  //  BOOST_FOREACH(sharedConditional conditional, conditionals_) {
-      if (conditional->permuteSeparatorWithInverse(inversePermutation))
+  //    if (conditional->permuteSeparatorWithInverse(inversePermutation))
-        separatorChanged = true;
+  //      separatorChanged = true;
-    }
+  //  }
-    return separatorChanged;
+  //  return separatorChanged;
-  }
+  //}
 
   /* ************************************************************************* */
   template<class CONDITIONAL>

gtsam/inference/BayesNet.h

@@ -225,7 +225,7 @@ public:
    * Returns true if any reordered variables appeared in the separator and
    * false if not.
    */
-  bool permuteSeparatorWithInverse(const Permutation& inversePermutation);
+  //bool permuteSeparatorWithInverse(const Permutation& inversePermutation);
 
   iterator begin()          {return conditionals_.begin();}    ///<TODO: comment
   iterator end()            {return conditionals_.end();}      ///<TODO: comment

gtsam/inference/BayesTreeCliqueBase-inl.h

@@ -132,24 +132,44 @@ namespace gtsam {
     assertInvariants();
   }
 
+  /* ************************************************************************* */
+//  template<class DERIVED, class CONDITIONAL>
+//  bool BayesTreeCliqueBase<DERIVED, CONDITIONAL>::permuteSeparatorWithInverse(
+//      const Permutation& inversePermutation) {
+//    bool changed = conditional_->permuteSeparatorWithInverse(
+//        inversePermutation);
+//#ifndef NDEBUG
+//    if(!changed) {
+//      BOOST_FOREACH(Index& separatorKey, conditional_->parents()) {assert(separatorKey == inversePermutation[separatorKey]);}
+//      BOOST_FOREACH(const derived_ptr& child, children_) {
+//        assert(child->permuteSeparatorWithInverse(inversePermutation) == false);
+//      }
+//    }
+//#endif
+//    if (changed) {
+//      BOOST_FOREACH(const derived_ptr& child, children_) {
+//        (void) child->permuteSeparatorWithInverse(inversePermutation);
+//      }
+//    }
+//    assertInvariants();
+//    return changed;
+//  }
+
   /* ************************************************************************* */
   template<class DERIVED, class CONDITIONAL>
-  bool BayesTreeCliqueBase<DERIVED, CONDITIONAL>::permuteSeparatorWithInverse(
+  bool BayesTreeCliqueBase<DERIVED, CONDITIONAL>::reduceSeparatorWithInverse(
-      const Permutation& inversePermutation) {
+    const internal::Reduction& inverseReduction)
-    bool changed = conditional_->permuteSeparatorWithInverse(
+  {
-        inversePermutation);
+    bool changed = conditional_->reduceSeparatorWithInverse(inverseReduction);
 #ifndef NDEBUG
     if(!changed) {
-      BOOST_FOREACH(Index& separatorKey, conditional_->parents()) {assert(separatorKey == inversePermutation[separatorKey]);}
       BOOST_FOREACH(const derived_ptr& child, children_) {
-        assert(child->permuteSeparatorWithInverse(inversePermutation) == false);
+        assert(child->reduceSeparatorWithInverse(inverseReduction) == false); }
-      }
     }
 #endif
-    if (changed) {
+    if(changed) {
       BOOST_FOREACH(const derived_ptr& child, children_) {
-        (void) child->permuteSeparatorWithInverse(inversePermutation);
+        (void) child->reduceSeparatorWithInverse(inverseReduction); }
-      }
     }
     assertInvariants();
     return changed;

gtsam/inference/BayesTreeCliqueBase.h

@@ -183,7 +183,14 @@ namespace gtsam {
      * traversing the whole tree.  Returns whether any separator variables in
      * this subtree were reordered.
      */
-    bool permuteSeparatorWithInverse(const Permutation& inversePermutation);
+    //bool permuteSeparatorWithInverse(const Permutation& inversePermutation);
+
+    /** Permute variables when they only appear in the separators.  In this
+     * case the running intersection property will be used to prevent always
+     * traversing the whole tree.  Returns whether any separator variables in
+     * this subtree were reordered.
+     */
+    bool reduceSeparatorWithInverse(const internal::Reduction& inverseReduction);
 
     /** return the conditional P(S|Root) on the separator given the root */
     BayesNet<ConditionalType> shortcut(derived_ptr root, Eliminate function) const;

gtsam/inference/IndexConditional.cpp

@@ -44,16 +44,33 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  bool IndexConditional::permuteSeparatorWithInverse(const Permutation& inversePermutation) {
+  //bool IndexConditional::permuteSeparatorWithInverse(const Permutation& inversePermutation) {
-  #ifndef NDEBUG
+  //#ifndef NDEBUG
-    BOOST_FOREACH(KeyType key, frontals()) { assert(key == inversePermutation[key]); }
+  //  BOOST_FOREACH(KeyType key, frontals()) { assert(key == inversePermutation[key]); }
-  #endif
+  //#endif
+  //  bool parentChanged = false;
+  //  BOOST_FOREACH(KeyType& parent, parents()) {
+  //    KeyType newParent = inversePermutation[parent];
+  //    if(parent != newParent) {
+  //      parentChanged = true;
+  //      parent = newParent;
+  //    }
+  //  }
+  //  assertInvariants();
+  //  return parentChanged;
+  //}
+
+  /* ************************************************************************* */
+  bool IndexConditional::reduceSeparatorWithInverse(const internal::Reduction& inverseReduction) {
+#ifndef NDEBUG
+    BOOST_FOREACH(KeyType key, frontals()) { assert(inverseReduction.find(key) == inverseReduction.end()); }
+#endif
     bool parentChanged = false;
     BOOST_FOREACH(KeyType& parent, parents()) {
-      KeyType newParent = inversePermutation[parent];
+      internal::Reduction::const_iterator it = inverseReduction.find(parent);
-      if(parent != newParent) {
+      if(it != inverseReduction.end()) {
         parentChanged = true;
-        parent = newParent;
+        parent = it->second;
       }
     }
     assertInvariants();

gtsam/inference/IndexConditional.h

@@ -112,7 +112,13 @@ namespace gtsam {
      * Returns true if any reordered variables appeared in the separator and
      * false if not.
      */
-    bool permuteSeparatorWithInverse(const Permutation& inversePermutation);
+    //bool permuteSeparatorWithInverse(const Permutation& inversePermutation);
+
+    /** Permute the variables when only separator variables need to be permuted.
+     * Returns true if any reordered variables appeared in the separator and
+     * false if not.
+     */
+    bool reduceSeparatorWithInverse(const internal::Reduction& inverseReduction);
 
     /**
      * Permutes the Conditional, but for efficiency requires the permutation

gtsam/inference/Permutation.cpp

@@ -161,6 +161,16 @@ namespace internal {
     return result;
   }
 
+  /* ************************************************************************* */
+  Reduction Reduction::CreateFromPartialPermutation(const Permutation& selector, const Permutation& p) {
+    if(selector.size() != p.size())
+      throw invalid_argument("internal::Reduction::CreateFromPartialPermutation called with selector and permutation of different sizes");
+    Reduction result;
+    for(size_t dstSlot = 0; dstSlot < p.size(); ++dstSlot)
+      result.insert(make_pair(selector[dstSlot], selector[p[dstSlot]]));
+    return result;
+  }
+
   /* ************************************************************************* */
   void Reduction::applyInverse(std::vector<Index>& js) const {
     BOOST_FOREACH(Index& j, js) {
@@ -183,10 +193,10 @@ namespace internal {
   }
 
   /* ************************************************************************* */
-  Index& Reduction::operator[](const Index& j) {
+  const Index& Reduction::operator[](const Index& j) {
     iterator it = this->find(j);
     if(it == this->end())
-      throw std::out_of_range("Index to Reduction::operator[] not present");
+      return j;
     else
       return it->second;
   }
@@ -195,7 +205,7 @@ namespace internal {
   const Index& Reduction::operator[](const Index& j) const {
     const_iterator it = this->find(j);
     if(it == this->end())
-      throw std::out_of_range("Index to Reduction::operator[] not present");
+      return j;
     else
       return it->second;
   }
@@ -207,6 +217,11 @@ namespace internal {
       cout << "  " << p.first << " : " << p.second << endl;
   }
 
+  /* ************************************************************************* */
+  bool Reduction::equals(const Reduction& other, double tol) const {
+    return (const Base&)(*this) == (const Base&)other;
+  }
+
   /* ************************************************************************* */
   Permutation createReducingPermutation(const std::set<Index>& indices) {
     Permutation p(indices.size());

gtsam/inference/Permutation.h

@@ -192,12 +192,14 @@ namespace internal {
     typedef gtsam::FastMap<Index,Index> Base;
 
     static Reduction CreateAsInverse(const Permutation& p);
+    static Reduction CreateFromPartialPermutation(const Permutation& selector, const Permutation& p);
     void applyInverse(std::vector<Index>& js) const;
     Permutation inverse() const;
-    Index& operator[](const Index& j);
+    const Index& operator[](const Index& j);
     const Index& operator[](const Index& j) const;
 
     void print(const std::string& s="") const;
+    bool equals(const Reduction& other, double tol = 1e-9) const;
   };
 
   // Reduce the variable indices so that those in the set are mapped to start at zero

gtsam/inference/VariableIndex.cpp

@@ -76,6 +76,20 @@ void VariableIndex::permuteInPlace(const Permutation& permutation) {
   index_.swap(newIndex);
 }
 
+/* ************************************************************************* */
+void VariableIndex::permuteInPlace(const Permutation& selector, const Permutation& permutation) {
+  if(selector.size() != permutation.size())
+    throw invalid_argument("VariableIndex::permuteInPlace (partial permutation version) called with selector and permutation of different sizes.");
+  // Create new index the size of the permuted entries
+  vector<VariableIndex::Factors> newIndex(selector.size());
+  // Permute the affected entries into the new index
+  for(size_t dstSlot = 0; dstSlot < selector.size(); ++dstSlot)
+    newIndex[dstSlot].swap(this->index_[selector[permutation[dstSlot]]]);
+  // Put the affected entries back in the new order
+  for(size_t slot = 0; slot < selector.size(); ++slot)
+    this->index_[selector[slot]].swap(newIndex[slot]);
+}
+
 /* ************************************************************************* */
 void VariableIndex::removeUnusedAtEnd(size_t nToRemove) {
 #ifndef NDEBUG

gtsam/inference/VariableIndex.h

@@ -135,6 +135,9 @@ public:
   /// Permute the variables in the VariableIndex according to the given permutation
   void permuteInPlace(const Permutation& permutation);
 
+  /// Permute the variables in the VariableIndex according to the given partial permutation
+  void permuteInPlace(const Permutation& selector, const Permutation& permutation);
+
   /** Remove unused empty variables at the end of the ordering (in debug mode
    * verifies they are empty).
    * @param nToRemove The number of unused variables at the end to remove

gtsam/inference/tests/testPermutation.cpp

@@ -25,6 +25,7 @@
 using namespace gtsam;
 using namespace std;
 
+/* ************************************************************************* */
 TEST(Permutation, Identity) {
   Permutation expected(5);
   expected[0] = 0;
@@ -38,6 +39,7 @@ TEST(Permutation, Identity) {
   EXPECT(assert_equal(expected, actual));
 }
 
+/* ************************************************************************* */
 TEST(Permutation, PullToFront) {
   Permutation expected(5);
   expected[0] = 4;
@@ -55,6 +57,7 @@ TEST(Permutation, PullToFront) {
   EXPECT(assert_equal(expected, actual));
 }
 
+/* ************************************************************************* */
 TEST(Permutation, PushToBack) {
   Permutation expected(5);
   expected[0] = 1;
@@ -72,6 +75,7 @@ TEST(Permutation, PushToBack) {
   EXPECT(assert_equal(expected, actual));
 }
 
+/* ************************************************************************* */
 TEST(Permutation, compose) {
   Permutation p1(5);
   p1[0] = 1;
@@ -104,6 +108,25 @@ TEST(Permutation, compose) {
   LONGS_EQUAL(p1[p2[4]], actual[4]);
 }
 
+/* ************************************************************************* */
+TEST(Reduction, CreateFromPartialPermutation) {
+  Permutation selector(3);
+  selector[0] = 2;
+  selector[1] = 4;
+  selector[2] = 6;
+  Permutation p(3);
+  p[0] = 2;
+  p[1] = 0;
+  p[2] = 1;
+
+  internal::Reduction expected;
+  expected.insert(make_pair(2,6));
+  expected.insert(make_pair(4,2));
+  expected.insert(make_pair(6,4));
+
+  internal::Reduction actual = internal::Reduction::CreateFromPartialPermutation(selector, p);
+}
+
 /* ************************************************************************* */
 int main() {  TestResult tr;  return TestRegistry::runAllTests(tr); }
 

gtsam/nonlinear/ISAM2-impl.cpp

@@ -353,11 +353,9 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors,
   Permutation::shared_ptr affectedColamdInverse(affectedColamd->inverse());
   if(debug) affectedColamd->print("affectedColamd: ");
   if(debug) affectedColamdInverse->print("affectedColamdInverse: ");
-  result.fullReordering =
+  result.reorderingSelector = affectedKeysSelector;
-      *Permutation::Identity(reorderingMode.nFullSystemVars).partialPermutation(affectedKeysSelector, *affectedColamd);
+  result.reorderingPermutation = *affectedColamd;
-  result.fullReorderingInverse =
+  result.reorderingInverse = internal::Reduction::CreateFromPartialPermutation(affectedKeysSelector, *affectedColamdInverse);
-      *Permutation::Identity(reorderingMode.nFullSystemVars).partialPermutation(affectedKeysSelector, *affectedColamdInverse);
-  if(debug) result.fullReordering.print("partialReordering: ");
   gttoc(ccolamd_permutations);
 
   gttic(permute_affected_variable_index);

gtsam/nonlinear/ISAM2-impl.h

@@ -26,8 +26,9 @@ struct ISAM2::Impl {
 
   struct PartialSolveResult {
     ISAM2::sharedClique bayesTree;
-    Permutation fullReordering;
+    Permutation reorderingSelector;
-    Permutation fullReorderingInverse;
+    Permutation reorderingPermutation;
+    internal::Reduction reorderingInverse;
   };
 
   struct ReorderingMode {

gtsam/nonlinear/ISAM2.cpp

@@ -481,22 +481,24 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& mark
     // re-eliminate.  The reordered variables are also mentioned in the
     // orphans and the leftover cached factors.
     gttic(permute_global_variable_index);
-    variableIndex_.permuteInPlace(partialSolveResult.fullReordering);
+    variableIndex_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation);
     gttoc(permute_global_variable_index);
     gttic(permute_delta);
-    delta_.permuteInPlace(partialSolveResult.fullReordering);
+    const Permutation fullReordering = *Permutation::Identity(delta_.size()).
-    deltaNewton_.permuteInPlace(partialSolveResult.fullReordering);
+      partialPermutation(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation);
-    RgProd_.permuteInPlace(partialSolveResult.fullReordering);
+    delta_.permuteInPlace(fullReordering);
+    deltaNewton_.permuteInPlace(fullReordering);
+    RgProd_.permuteInPlace(fullReordering);
     gttoc(permute_delta);
     gttic(permute_ordering);
-    ordering_.permuteWithInverse(partialSolveResult.fullReorderingInverse);
+    ordering_.reduceWithInverse(partialSolveResult.reorderingInverse);
     gttoc(permute_ordering);
     if(params_.cacheLinearizedFactors) {
       gttic(permute_cached_linear);
       //linearFactors_.permuteWithInverse(partialSolveResult.fullReorderingInverse);
       FastList<size_t> permuteLinearIndices = getAffectedFactors(affectedAndNewKeys);
       BOOST_FOREACH(size_t idx, permuteLinearIndices) {
-        linearFactors_[idx]->permuteWithInverse(partialSolveResult.fullReorderingInverse);
+        linearFactors_[idx]->reduceWithInverse(partialSolveResult.reorderingInverse);
       }
       gttoc(permute_cached_linear);
     }
@@ -514,7 +516,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& mark
     gttic(orphans);
     gttic(permute);
     BOOST_FOREACH(sharedClique orphan, orphans) {
-      (void)orphan->permuteSeparatorWithInverse(partialSolveResult.fullReorderingInverse);
+      (void)orphan->reduceSeparatorWithInverse(partialSolveResult.reorderingInverse);
     }
     gttoc(permute);
     gttic(insert);

gtsam/nonlinear/ISAM2.h

@@ -405,9 +405,15 @@ public:
     Base::permuteWithInverse(inversePermutation);
   }
 
-  bool permuteSeparatorWithInverse(const Permutation& inversePermutation) {
+  //bool permuteSeparatorWithInverse(const Permutation& inversePermutation) {
-    bool changed = Base::permuteSeparatorWithInverse(inversePermutation);
+  //  bool changed = Base::permuteSeparatorWithInverse(inversePermutation);
-    if(changed) if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation);
+  //  if(changed) if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation);
+  //  return changed;
+  //}
+
+  bool reduceSeparatorWithInverse(const internal::Reduction& inverseReduction) {
+    bool changed = Base::reduceSeparatorWithInverse(inverseReduction);
+    if(changed) if(cachedFactor_) cachedFactor_->reduceWithInverse(inverseReduction);
     return changed;
   }
 

gtsam/nonlinear/Ordering.cpp

@@ -40,6 +40,13 @@ void Ordering::permuteWithInverse(const Permutation& inversePermutation) {
   }
 }
 
+/* ************************************************************************* */
+void Ordering::reduceWithInverse(const internal::Reduction& inverseReduction) {
+  BOOST_FOREACH(Ordering::value_type& key_order, *this) {
+    key_order.second = inverseReduction[key_order.second];
+  }
+}
+
 /* ************************************************************************* */
 void Ordering::print(const string& str, const KeyFormatter& keyFormatter) const {
   cout << str;

gtsam/nonlinear/Ordering.h

@@ -203,6 +203,13 @@ public:
    */
   void permuteWithInverse(const Permutation& inversePermutation);
 
+  /**
+   * Reorder the variables with a permutation.  This is typically used
+   * internally, permuting an initial key-sorted ordering into a fill-reducing
+   * ordering.
+   */
+  void reduceWithInverse(const internal::Reduction& inverseReduction);
+
   /// Synonym for operator[](Key)
   Index& at(Key key) { return operator[](key); }