From 4cb66dcdcc2aa2b16855eaa5b766356c994ce2eb Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Tue, 18 Dec 2012 14:21:58 +0000
Subject: [PATCH] For speed, added partial permutation version of
 VectorValues::permuteInPlace and removed Permutation::partialPermutation
 (which generated a full-length permutation with only a few entries
 rearranged)

---
 gtsam/inference/Permutation.cpp | 12 ------------
 gtsam/inference/Permutation.h   |  8 --------
 gtsam/linear/VectorValues.cpp   | 14 ++++++++++++++
 gtsam/linear/VectorValues.h     |  5 +++++
 gtsam/nonlinear/ISAM2.cpp       |  8 +++-----
 5 files changed, 22 insertions(+), 25 deletions(-)

diff --git a/gtsam/inference/Permutation.cpp b/gtsam/inference/Permutation.cpp
index bd6d50c22..94528f64f 100644
--- a/gtsam/inference/Permutation.cpp
+++ b/gtsam/inference/Permutation.cpp
@@ -123,18 +123,6 @@ Permutation::shared_ptr Permutation::permute(const Permutation& permutation) con
   return result;
 }
 
-/* ************************************************************************* */
-Permutation::shared_ptr Permutation::partialPermutation(
-    const Permutation& selector, const Permutation& partialPermutation) const {
-  assert(selector.size() == partialPermutation.size());
-  Permutation::shared_ptr result(new Permutation(*this));
-
-  for(Index subsetPos=0; subsetPos<selector.size(); ++subsetPos)
-    (*result)[selector[subsetPos]] = (*this)[selector[partialPermutation[subsetPos]]];
-
-  return result;
-}
-
 /* ************************************************************************* */
 Permutation::shared_ptr Permutation::inverse() const {
   Permutation::shared_ptr result(new Permutation(this->size()));
diff --git a/gtsam/inference/Permutation.h b/gtsam/inference/Permutation.h
index 4fbd06199..985cc192f 100644
--- a/gtsam/inference/Permutation.h
+++ b/gtsam/inference/Permutation.h
@@ -167,14 +167,6 @@ public:
   /// @name Advanced Interface
   /// @{
 
-
-  /**
-   * A partial permutation, reorders the variables selected by selector through
-   * partialPermutation.  selector and partialPermutation should have the same
-   * size, this is checked if NDEBUG is not defined.
-   */
-  Permutation::shared_ptr partialPermutation(const Permutation& selector, const Permutation& partialPermutation) const;
-
   iterator begin() { return rangeIndices_.begin(); }  ///< Iterate through the indices
   iterator end() { return rangeIndices_.end(); }      ///< Iterate through the indices
 
diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp
index f4f350c23..3c6c7133f 100644
--- a/gtsam/linear/VectorValues.cpp
+++ b/gtsam/linear/VectorValues.cpp
@@ -145,6 +145,20 @@ void VectorValues::permuteInPlace(const Permutation& permutation) {
   values_.swap(newValues);
 }
 
+/* ************************************************************************* */
+void VectorValues::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
+  Values reorderedEntries(selector.size());
+  // Permute the affected entries into the new index
+  for(size_t dstSlot = 0; dstSlot < selector.size(); ++dstSlot)
+    reorderedEntries[dstSlot].swap(values_[selector[permutation[dstSlot]]]);
+  // Put the affected entries back in the new order
+  for(size_t slot = 0; slot < selector.size(); ++slot)
+    values_[selector[slot]].swap(reorderedEntries[slot]);
+}
+
 /* ************************************************************************* */
 void VectorValues::swap(VectorValues& other) {
   this->values_.swap(other.values_);
diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h
index 980c74dc0..1997a4bd5 100644
--- a/gtsam/linear/VectorValues.h
+++ b/gtsam/linear/VectorValues.h
@@ -253,6 +253,11 @@ namespace gtsam {
      */
     bool hasSameStructure(const VectorValues& other) const;
 
+    /**  
+     * Permute the variables in the VariableIndex according to the given partial permutation
+     */
+    void VectorValues::permuteInPlace(const Permutation& selector, const Permutation& permutation);
+
     /**
      * Permute the entries of this VectorValues in place
      */
diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp
index d19654ac9..7d579f206 100644
--- a/gtsam/nonlinear/ISAM2.cpp
+++ b/gtsam/nonlinear/ISAM2.cpp
@@ -484,11 +484,9 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& mark
     variableIndex_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation);
     gttoc(permute_global_variable_index);
     gttic(permute_delta);
-    const Permutation fullReordering = *Permutation::Identity(delta_.size()).
-      partialPermutation(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation);
-    delta_.permuteInPlace(fullReordering);
-    deltaNewton_.permuteInPlace(fullReordering);
-    RgProd_.permuteInPlace(fullReordering);
+    delta_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation);
+    deltaNewton_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation);
+    RgProd_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation);
     gttoc(permute_delta);
     gttic(permute_ordering);
     ordering_.reduceWithInverse(partialSolveResult.reorderingInverse);