diff --git a/CppUnitLite/TestRegistry.cpp b/CppUnitLite/TestRegistry.cpp
index c2fb05a63..e7de39821 100644
--- a/CppUnitLite/TestRegistry.cpp
+++ b/CppUnitLite/TestRegistry.cpp
@@ -8,6 +8,8 @@
 #include "TestRegistry.h"
 #include "SimpleString.h"
 
+//#include <iostream>
+//using namespace std;
 
 void TestRegistry::addTest (Test *test) 
 {
@@ -50,6 +52,7 @@ int TestRegistry::run (TestResult& result)
 	for (Test *test = tests; test != 0; test = test->getNext ()) {
 		if (test->safe()) {
 			try {
+//			  cout << test->getName().asCharString() << ", " << test->getFilename().asCharString() << ":" << test->getLineNumber() << endl;
 				test->run (result);
 			} catch (std::exception& e) {
 				// catch standard exceptions and derivatives
diff --git a/base/blockMatrices.h b/base/blockMatrices.h
index 87389848c..c3ceb58f4 100644
--- a/base/blockMatrices.h
+++ b/base/blockMatrices.h
@@ -103,9 +103,9 @@ public:
   template<typename Iterator>
   VerticalBlockView(matrix_type& matrix, Iterator firstBlockDim, Iterator lastBlockDim, size_t matrixNewHeight);
 
-  size_t size1() const { checkInvariants(); return rowEnd_ - rowStart_; }
-  size_t size2() const { checkInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; }
-  size_t nBlocks() const { checkInvariants(); return variableColOffsets_.size() - 1 - blockStart_; }
+  size_t size1() const { assertInvariants(); return rowEnd_ - rowStart_; }
+  size_t size2() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; }
+  size_t nBlocks() const { assertInvariants(); return variableColOffsets_.size() - 1 - blockStart_; }
 
   block_type operator()(size_t block) {
     return range(block, block+1);
@@ -116,7 +116,7 @@ public:
   }
 
   block_type range(size_t startBlock, size_t endBlock) {
-    checkInvariants();
+    assertInvariants();
     size_t actualStartBlock = startBlock + blockStart_;
     size_t actualEndBlock = endBlock + blockStart_;
     checkBlock(actualStartBlock);
@@ -127,7 +127,7 @@ public:
   }
 
   const_block_type range(size_t startBlock, size_t endBlock) const {
-    checkInvariants();
+    assertInvariants();
     size_t actualStartBlock = startBlock + blockStart_;
     size_t actualEndBlock = endBlock + blockStart_;
     checkBlock(actualStartBlock);
@@ -138,7 +138,7 @@ public:
   }
 
   column_type column(size_t block, size_t columnOffset) {
-    checkInvariants();
+    assertInvariants();
     size_t actualBlock = block + blockStart_;
     checkBlock(actualBlock);
     assert(variableColOffsets_[actualBlock] + columnOffset < matrix_.size2());
@@ -147,7 +147,7 @@ public:
   }
 
   const_column_type column(size_t block, size_t columnOffset) const {
-    checkInvariants();
+    assertInvariants();
     size_t actualBlock = block + blockStart_;
     checkBlock(actualBlock);
     assert(variableColOffsets_[actualBlock] + columnOffset < matrix_.size2());
@@ -156,7 +156,7 @@ public:
   }
 
   size_t offset(size_t block) const {
-    checkInvariants();
+    assertInvariants();
     size_t actualBlock = block + blockStart_;
     checkBlock(actualBlock);
     return variableColOffsets_[actualBlock] - variableColOffsets_[blockStart_];
@@ -184,8 +184,13 @@ public:
   template<class RhsMatrix>
   VerticalBlockView<Matrix>& assignNoalias(const VerticalBlockView<RhsMatrix>& rhs);
 
+  /** Swap the contents of the underlying matrix and the block information with
+   * another VerticalBlockView.
+   */
+  void swap(VerticalBlockView<Matrix>& other);
+
 protected:
-  void checkInvariants() const {
+  void assertInvariants() const {
     assert(matrix_.size2() == variableColOffsets_.back());
     assert(blockStart_ < variableColOffsets_.size());
     assert(rowStart_ <= matrix_.size1());
@@ -214,30 +219,34 @@ protected:
   friend class VerticalBlockView<Matrix>;
 };
 
+/* ************************************************************************* */
 template<class Matrix>
 VerticalBlockView<Matrix>::VerticalBlockView(matrix_type& matrix) :
 matrix_(matrix), rowStart_(0), rowEnd_(matrix_.size1()), blockStart_(0) {
   fillOffsets((size_t*)0, (size_t*)0);
-  checkInvariants();
+  assertInvariants();
 }
 
+/* ************************************************************************* */
 template<class Matrix>
 template<typename Iterator>
 VerticalBlockView<Matrix>::VerticalBlockView(matrix_type& matrix, Iterator firstBlockDim, Iterator lastBlockDim) :
 matrix_(matrix), rowStart_(0), rowEnd_(matrix_.size1()), blockStart_(0) {
   fillOffsets(firstBlockDim, lastBlockDim);
-  checkInvariants();
+  assertInvariants();
 }
 
+/* ************************************************************************* */
 template<class Matrix>
 template<typename Iterator>
 VerticalBlockView<Matrix>::VerticalBlockView(matrix_type& matrix, Iterator firstBlockDim, Iterator lastBlockDim, size_t matrixNewHeight) :
 matrix_(matrix), rowStart_(0), rowEnd_(matrixNewHeight), blockStart_(0) {
   fillOffsets(firstBlockDim, lastBlockDim);
   matrix_.resize(matrixNewHeight, variableColOffsets_.back(), false);
-  checkInvariants();
+  assertInvariants();
 }
 
+/* ************************************************************************* */
 template<class Matrix>
 template<class RhsMatrix>
 void VerticalBlockView<Matrix>::copyStructureFrom(const VerticalBlockView<RhsMatrix>& rhs) {
@@ -257,9 +266,10 @@ void VerticalBlockView<Matrix>::copyStructureFrom(const VerticalBlockView<RhsMat
   rowStart_ = 0;
   rowEnd_ = matrix_.size1();
   blockStart_ = 0;
-  checkInvariants();
+  assertInvariants();
 }
 
+/* ************************************************************************* */
 template<class Matrix>
 template<class RhsMatrix>
 VerticalBlockView<Matrix>& VerticalBlockView<Matrix>::assignNoalias(const VerticalBlockView<RhsMatrix>& rhs) {
@@ -268,5 +278,17 @@ VerticalBlockView<Matrix>& VerticalBlockView<Matrix>::assignNoalias(const Vertic
   return *this;
 }
 
+/* ************************************************************************* */
+template<class Matrix>
+void VerticalBlockView<Matrix>::swap(VerticalBlockView<Matrix>& other) {
+  matrix_.swap(other.matrix_);
+  variableColOffsets_.swap(other.variableColOffsets_);
+  std::swap(rowStart_, other.rowStart_);
+  std::swap(rowEnd_, other.rowEnd_);
+  std::swap(blockStart_, other.blockStart_);
+  assertInvariants();
+  other.assertInvariants();
+}
+
 
 }
diff --git a/inference/Conditional.h b/inference/Conditional.h
index 59e21229f..0a86de0d9 100644
--- a/inference/Conditional.h
+++ b/inference/Conditional.h
@@ -27,21 +27,13 @@ namespace gtsam {
  * kept in pointer containers. To be safe, you should make them
  * immutable, i.e., practicing functional programming.
  */
-class Conditional: boost::noncopyable, public Testable<Conditional> {
+class Conditional: public Factor, boost::noncopyable, public Testable<Conditional> {
 
 protected:
-  /** Conditional just uses an internal Factor for storage (a conditional is
-   * really just a special factor anyway, but we do this instead of inherit
-   * to prevent "diamond" inheritance with GaussianFactor and
-   * GaussianConditional.
-   */
-  Factor factor_;
 
   /** The first nFrontal variables are frontal and the rest are parents. */
   size_t nrFrontals_;
 
-  ValueWithDefault<bool, true> permuted_;
-
 public:
 
   /** convenience typename for a shared pointer to this class */
@@ -56,20 +48,20 @@ public:
   Conditional() : nrFrontals_(0) {}
 
   /** No parents */
-  Conditional(Index key) : factor_(key), nrFrontals_(1) {}
+  Conditional(Index key) : Factor(key), nrFrontals_(1) {}
 
   /** Single parent */
-  Conditional(Index key, Index parent) : factor_(key, parent), nrFrontals_(1) {}
+  Conditional(Index key, Index parent) : Factor(key, parent), nrFrontals_(1) {}
 
   /** Two parents */
-  Conditional(Index key, Index parent1, Index parent2) : factor_(key, parent1, parent2), nrFrontals_(1) {}
+  Conditional(Index key, Index parent1, Index parent2) : Factor(key, parent1, parent2), nrFrontals_(1) {}
 
   /** Three parents */
-  Conditional(Index key, Index parent1, Index parent2, Index parent3) : factor_(key, parent1, parent2, parent3), nrFrontals_(1) {}
+  Conditional(Index key, Index parent1, Index parent2, Index parent3) : Factor(key, parent1, parent2, parent3), nrFrontals_(1) {}
 
   /** Constructor from a frontal variable and a vector of parents */
   Conditional(Index key, const std::vector<Index>& parents) : nrFrontals_(1) {
-			factor_.keys_.resize(1 + parents.size());
+			keys_.resize(1 + parents.size());
 			*(beginFrontals()) = key;
 			std::copy(parents.begin(), parents.end(), beginParents());
 		}
@@ -79,37 +71,34 @@ public:
   static shared_ptr fromRange(Iterator firstKey, Iterator lastKey, size_t nrFrontals) {
   	shared_ptr conditional(new Conditional);
   	conditional->nrFrontals_ = nrFrontals;
-		std::copy(firstKey, lastKey, back_inserter(conditional->factor_.keys_));
+		std::copy(firstKey, lastKey, back_inserter(conditional->keys_));
 		return conditional;
   }
 
   /** check equality */
   bool equals(const Conditional& c, double tol = 1e-9) const {
-    return nrFrontals_ == c.nrFrontals_ && factor_.equals(c.factor_, tol); }
+    return nrFrontals_ == c.nrFrontals_ && Factor::equals(c, tol); }
 
 	/** return the number of frontals */
 	size_t nrFrontals() const { return nrFrontals_; }
 
 	/** return the number of parents */
-	size_t nrParents() const { return factor_.keys_.size() - nrFrontals_; }
+	size_t nrParents() const { return keys_.size() - nrFrontals_; }
 
 	/** Special accessor when there is only one frontal variable. */
-	Index key() const { assert(nrFrontals_==1); return factor_.keys_[0]; }
-
-  /** return a const reference to all keys */
-  const std::vector<Index>& keys() const { return factor_.keys(); }
+	Index key() const { assert(nrFrontals_==1); return keys_[0]; }
 
   /** Iterators over frontal and parent variables. */
-  const_iterator beginFrontals() const { return factor_.keys_.begin(); }
-  const_iterator endFrontals() const { return factor_.keys_.begin()+nrFrontals_; }
-  const_iterator beginParents() const { return factor_.keys_.begin()+nrFrontals_; }
-  const_iterator endParents() const { return factor_.keys_.end(); }
+  const_iterator beginFrontals() const { return keys_.begin(); }
+  const_iterator endFrontals() const { return keys_.begin()+nrFrontals_; }
+  const_iterator beginParents() const { return keys_.begin()+nrFrontals_; }
+  const_iterator endParents() const { return keys_.end(); }
 
   /** Mutable iterators and accessors */
-  iterator beginFrontals() { return factor_.keys_.begin(); }
-  iterator endFrontals() { return factor_.keys_.begin()+nrFrontals_; }
-  iterator beginParents() { return factor_.keys_.begin()+nrFrontals_; }
-  iterator endParents() { return factor_.keys_.end(); }
+  iterator beginFrontals() { return keys_.begin(); }
+  iterator endFrontals() { return keys_.begin()+nrFrontals_; }
+  iterator beginParents() { return keys_.begin()+nrFrontals_; }
+  iterator endParents() { return keys_.end(); }
   boost::iterator_range<iterator> frontals() { return boost::make_iterator_range(beginFrontals(), endFrontals()); }
   boost::iterator_range<iterator> parents() { return boost::make_iterator_range(beginParents(), endParents()); }
 
@@ -154,13 +143,22 @@ public:
    * to already be inverted.
    */
   void permuteWithInverse(const Permutation& inversePermutation) {
-    factor_.permuteWithInverse(inversePermutation); }
+    // The permutation may not move the separators into the frontals
+#ifndef NDEBUG
+    BOOST_FOREACH(const Index frontal, this->frontals()) {
+      BOOST_FOREACH(const Index separator, this->parents()) {
+        assert(inversePermutation[frontal] < inversePermutation[separator]);
+      }
+    }
+#endif
+    Factor::permuteWithInverse(inversePermutation);
+  }
 
 protected:
   /** Debugging invariant that the keys should be in order, including that the
    * conditioned variable is numbered lower than the parents.
    */
-  void checkSorted() const;
+  void assertInvariants() const;
 
   friend class Factor;
 
@@ -169,7 +167,6 @@ private:
 	friend class boost::serialization::access;
 	template<class Archive>
 	void serialize(Archive & ar, const unsigned int version) {
-		ar & BOOST_SERIALIZATION_NVP(factor_);
     ar & BOOST_SERIALIZATION_NVP(nrFrontals_);
 	}
 };
diff --git a/inference/Factor-inl.h b/inference/Factor-inl.h
index 6903e23af..cb0fd18d5 100644
--- a/inference/Factor-inl.h
+++ b/inference/Factor-inl.h
@@ -29,13 +29,9 @@ namespace gtsam {
 //  checkSorted();
 //}
 
-/* ************************************************************************* */
-template<class Derived> Factor::Factor(const Derived& c) : keys_(c.keys()), permuted_(c.permuted_) {
-}
-
 /* ************************************************************************* */
 template<class KeyIterator> Factor::Factor(KeyIterator beginKey, KeyIterator endKey) :
-    keys_(beginKey, endKey) { checkSorted(); }
+    keys_(beginKey, endKey) { assertInvariants(); }
 
 /* ************************************************************************* */
 template<class FactorGraphType, class VariableIndexStorage>
@@ -43,7 +39,7 @@ Factor::shared_ptr Factor::Combine(const FactorGraphType& factorGraph,
     const VariableIndex<VariableIndexStorage>& variableIndex, const std::vector<size_t>& factors,
     const std::vector<Index>& variables, const std::vector<std::vector<size_t> >& variablePositions) {
 
-  return shared_ptr(boost::make_shared<Factor>(variables.begin(), variables.end()));
+  return shared_ptr(new Factor(variables.begin(), variables.end()));
 }
 
 /* ************************************************************************* */
diff --git a/inference/Factor.cpp b/inference/Factor.cpp
index 7ae8c7f71..553d4d3c0 100644
--- a/inference/Factor.cpp
+++ b/inference/Factor.cpp
@@ -20,7 +20,10 @@ using namespace boost::lambda;
 namespace gtsam {
 
 /* ************************************************************************* */
-Factor::Factor(const Factor& f) : keys_(f.keys_), permuted_(f.permuted_) {}
+Factor::Factor(const Factor& f) : keys_(f.keys_) {}
+
+/* ************************************************************************* */
+Factor::Factor(const Conditional& c) : keys_(c.keys()) {}
 
 /* ************************************************************************* */
 void Factor::print(const std::string& s) const {
@@ -37,7 +40,7 @@ bool Factor::equals(const Factor& other, double tol) const {
 /* ************************************************************************* */
 Conditional::shared_ptr Factor::eliminateFirst() {
   assert(!keys_.empty());
-  checkSorted();
+  assertInvariants();
   Index eliminated = keys_.front();
   keys_.erase(keys_.begin());
   return Conditional::shared_ptr(new Conditional(eliminated, keys_));
@@ -46,7 +49,7 @@ Conditional::shared_ptr Factor::eliminateFirst() {
 /* ************************************************************************* */
 boost::shared_ptr<BayesNet<Conditional> > Factor::eliminate(size_t nrFrontals) {
   assert(keys_.size() >= nrFrontals);
-  checkSorted();
+  assertInvariants();
   BayesNet<Conditional>::shared_ptr fragment(new BayesNet<Conditional>());
   const_iterator nextFrontal = this->begin();
   for(Index n = 0; n < nrFrontals; ++n, ++nextFrontal)
@@ -58,7 +61,6 @@ boost::shared_ptr<BayesNet<Conditional> > Factor::eliminate(size_t nrFrontals) {
 
 /* ************************************************************************* */
 void Factor::permuteWithInverse(const Permutation& inversePermutation) {
-  this->permuted_.value = true;
   BOOST_FOREACH(Index& key, keys_) { key = inversePermutation[key]; }
 }
 
diff --git a/inference/Factor.h b/inference/Factor.h
index f10e2f1bf..b00034f7d 100644
--- a/inference/Factor.h
+++ b/inference/Factor.h
@@ -40,11 +40,10 @@ class Factor : public Testable<Factor> {
 protected:
 
   std::vector<Index> keys_;
-  ValueWithDefault<bool,true> permuted_;
 
-  /** Internal check to make sure keys are sorted (invariant during elimination).
+  /** Internal check to make sure keys are sorted.
    * If NDEBUG is defined, this is empty and optimized out. */
-  void checkSorted() const;
+  void assertInvariants() const;
 
 public:
 
@@ -57,29 +56,29 @@ public:
   Factor(const Factor& f);
 
   /** Construct from derived type */
-  template<class Derived> Factor(const Derived& c);
+  Factor(const Conditional& c);
 
   /** Constructor from a collection of keys */
   template<class KeyIterator> Factor(KeyIterator beginKey, KeyIterator endKey);
 
   /** Default constructor for I/O */
-  Factor() : permuted_(false) {}
+  Factor() {}
 
   /** Construct unary factor */
-  Factor(Index key) : keys_(1), permuted_(false) {
-    keys_[0] = key; checkSorted(); }
+  Factor(Index key) : keys_(1) {
+    keys_[0] = key; assertInvariants(); }
 
   /** Construct binary factor */
-  Factor(Index key1, Index key2) : keys_(2), permuted_(false) {
-    keys_[0] = key1; keys_[1] = key2; checkSorted(); }
+  Factor(Index key1, Index key2) : keys_(2) {
+    keys_[0] = key1; keys_[1] = key2; assertInvariants(); }
 
   /** Construct ternary factor */
-  Factor(Index key1, Index key2, Index key3) : keys_(3), permuted_(false) {
-    keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; checkSorted(); }
+  Factor(Index key1, Index key2, Index key3) : keys_(3) {
+    keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; assertInvariants(); }
 
   /** Construct 4-way factor */
-  Factor(Index key1, Index key2, Index key3, Index key4) : keys_(4), permuted_(false) {
-    keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; checkSorted(); }
+  Factor(Index key1, Index key2, Index key3, Index key4) : keys_(4) {
+    keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; assertInvariants(); }
 
   /** Named constructor for combining a set of factors with pre-computed set of
    * variables.  (Old style - will be removed when scalar elimination is
@@ -158,10 +157,11 @@ protected:
 };
 
 /* ************************************************************************* */
-inline void Factor::checkSorted() const {
+inline void Factor::assertInvariants() const {
 #ifndef NDEBUG
-  for(size_t pos=1; pos<keys_.size(); ++pos)
-    assert(keys_[pos-1] < keys_[pos]);
+  std::set<Index> uniqueSorted(keys_.begin(), keys_.end());
+  assert(uniqueSorted.size() == keys_.size());
+  assert(std::equal(uniqueSorted.begin(), uniqueSorted.end(), keys_.begin()));
 #endif
 }
 
diff --git a/inference/FactorGraph-inl.h b/inference/FactorGraph-inl.h
index 9acdeb520..46aa075bc 100644
--- a/inference/FactorGraph-inl.h
+++ b/inference/FactorGraph-inl.h
@@ -34,17 +34,6 @@ using namespace std;
 
 namespace gtsam {
 
-	/* ************************************************************************* */
-	template<class Factor>
-	template<class Conditional>
-	FactorGraph<Factor>::FactorGraph(const BayesNet<Conditional>& bayesNet) {
-		typename BayesNet<Conditional>::const_iterator it = bayesNet.begin();
-		for (; it != bayesNet.end(); it++) {
-			sharedFactor factor(new Factor(**it));
-			push_back(factor);
-		}
-	}
-
 //  /* ************************************************************************* */
 //  template<class Conditional>
 //  FactorGraph::FactorGraph(const BayesNet<Conditional>& bayesNet, const Inference::Permutation& permutation) {
diff --git a/inference/FactorGraph.h b/inference/FactorGraph.h
index 4f715184a..7e4923fa7 100644
--- a/inference/FactorGraph.h
+++ b/inference/FactorGraph.h
@@ -63,13 +63,9 @@ namespace gtsam {
 		template<class Conditional>
 		FactorGraph(const BayesNet<Conditional>& bayesNet);
 
-//    /** convert from Bayes net while permuting at the same time */
-//    template<class Conditional>
-//    FactorGraph(const BayesNet<Conditional>& bayesNet, const Inference::Permutation& permutation);
-
 		/** convert from a derived type */
-		template<class Derived>
-		FactorGraph(const Derived& factorGraph);
+		template<class DerivedFactor>
+		FactorGraph(const FactorGraph<DerivedFactor>& factorGraph);
 
 		/** Add a factor */
 		template<class DerivedFactor>
@@ -210,10 +206,10 @@ namespace gtsam {
 
   /* ************************************************************************* */
   template<class Factor>
-  template<class Derived>
-  FactorGraph<Factor>::FactorGraph(const Derived& factorGraph) {
+  template<class DerivedFactor>
+  FactorGraph<Factor>::FactorGraph(const FactorGraph<DerivedFactor>& factorGraph) {
     factors_.reserve(factorGraph.size());
-    BOOST_FOREACH(const typename Derived::sharedFactor& factor, factorGraph) {
+    BOOST_FOREACH(const typename DerivedFactor::shared_ptr& factor, factorGraph) {
       if(factor)
         this->push_back(sharedFactor(new Factor(*factor)));
       else
@@ -221,6 +217,16 @@ namespace gtsam {
     }
   }
 
+  /* ************************************************************************* */
+  template<class Factor>
+  template<class Conditional>
+  FactorGraph<Factor>::FactorGraph(const BayesNet<Conditional>& bayesNet) {
+    factors_.reserve(bayesNet.size());
+    BOOST_FOREACH(const typename Conditional::shared_ptr& cond, bayesNet) {
+      this->push_back(sharedFactor(new Factor(*cond)));
+    }
+  }
+
   /* ************************************************************************* */
   template<class Factor>
   template<class DerivedFactor>
diff --git a/inference/VariableIndex.h b/inference/VariableIndex.h
index c83fb797b..96276dd2c 100644
--- a/inference/VariableIndex.h
+++ b/inference/VariableIndex.h
@@ -212,9 +212,13 @@ void VariableIndex<Storage>::rebaseFactors(ptrdiff_t baseIndexChange) {
 template<class Storage>
 template<class Derived>
 bool VariableIndex<Storage>::equals(const Derived& other, double tol) const {
-  if(this->nEntries_ == other.nEntries_ && this->nFactors_ == other.nFactors_ && this->index_.size() == other.index_.size()) {
-    for(size_t var=0; var<this->index_.size(); ++var)
-      if(this->index_[var] != other.index_[var])
+  if(this->nEntries_ == other.nEntries_ && this->nFactors_ == other.nFactors_) {
+    for(size_t var=0; var < std::max(this->index_.size(), other.index_.size()); ++var)
+      if(var >= this->index_.size() || var >= other.index_.size()) {
+        if(!((var >= this->index_.size() && other.index_[var].empty()) ||
+            (var >= other.index_.size() && this->index_[var].empty())))
+          return false;
+      } else if(this->index_[var] != other.index_[var])
         return false;
   } else
     return false;
@@ -229,7 +233,7 @@ void VariableIndex<Storage>::print(const std::string& str) const {
   BOOST_FOREACH(const mapped_type& variable, index_.container()) {
     Permutation::const_iterator rvar = find(index_.permutation().begin(), index_.permutation().end(), var);
     assert(rvar != index_.permutation().end());
-    std::cout << "var " << *rvar << ":";
+    std::cout << "var " << (rvar-index_.permutation().begin()) << ":";
     BOOST_FOREACH(const mapped_factor_type& factor, variable) {
       std::cout << " " << factor.factorIndex << "-" << factor.variablePosition;
     }
diff --git a/inference/tests/testSymbolicFactorGraph.cpp b/inference/tests/testSymbolicFactorGraph.cpp
index 5039c8b82..8f827a67f 100644
--- a/inference/tests/testSymbolicFactorGraph.cpp
+++ b/inference/tests/testSymbolicFactorGraph.cpp
@@ -220,32 +220,31 @@ typedef vector<boost::optional<Index> > optionalIndices;
  */
 optionalIndices buildETree(const StructA& structA) {
 
-	// todo: get n
-	size_t n = 5;
-	optionalIndices parent(n);
+  // todo: get n
+  size_t n = 5;
+  optionalIndices parent(n);
 
-	// todo: get m
-	size_t m = 5;
-	optionalIndices prev_col(m);
+  // todo: get m
+  size_t m = 5;
+  optionalIndices prev_col(m);
 
-	// for column j \in 1 to n do
-	for (Index j = 0; j < n; j++) {
-		// for row i \in Struct[A*j] do
-		BOOST_FOREACH(RowIndex i, structA[j])
-					{
-						if (prev_col[i]) {
-							Index k = *(prev_col[i]);
-							// find root r of the current tree that contains k
-							Index r = k;
-							while (parent[r])
-								r = *parent[r];
-							if (r != j) parent[r].reset(j);
-						}
-						prev_col[i].reset(j);
-					}
-	}
+  // for column j \in 1 to n do
+  for (Index j = 0; j < n; j++) {
+    // for row i \in Struct[A*j] do
+    BOOST_FOREACH(RowIndex i, structA[j]) {
+      if (prev_col[i]) {
+        Index k = *(prev_col[i]);
+        // find root r of the current tree that contains k
+        Index r = k;
+        while (parent[r])
+          r = *parent[r];
+        if (r != j) parent[r].reset(j);
+      }
+      prev_col[i].reset(j);
+    }
+  }
 
-	return parent;
+  return parent;
 }
 
 /**
diff --git a/linear/GaussianConditional.cpp b/linear/GaussianConditional.cpp
index 6d3e78aad..c871227fa 100644
--- a/linear/GaussianConditional.cpp
+++ b/linear/GaussianConditional.cpp
@@ -64,13 +64,13 @@ GaussianConditional::GaussianConditional(Index key, const Vector& d, const Matri
     rsd_(matrix_), sigmas_(sigmas) {
   assert(R.size1() <= R.size2());
   Conditional::nrFrontals_ = 1;
-  Conditional::factor_.keys_.resize(1+parents.size());
+  keys_.resize(1+parents.size());
   size_t dims[1+parents.size()+1];
   dims[0] = R.size2();
-  Conditional::factor_.keys_[0] = key;
+  keys_[0] = key;
   size_t j=1;
   for(std::list<std::pair<Index, Matrix> >::const_iterator parent=parents.begin(); parent!=parents.end(); ++parent) {
-    Conditional::factor_.keys_[j] = parent->first;
+    keys_[j] = parent->first;
     dims[j] = parent->second.size2();
     ++ j;
   }
diff --git a/linear/GaussianConditional.h b/linear/GaussianConditional.h
index 0d0de3999..681b14732 100644
--- a/linear/GaussianConditional.h
+++ b/linear/GaussianConditional.h
@@ -113,7 +113,7 @@ public:
 	/** return stuff contained in GaussianConditional */
 	rsd_type::const_column_type get_d() const { return rsd_.column(1+nrParents(), 0); }
 	rsd_type::const_block_type get_R() const { return rsd_(0); }
-	rsd_type::const_block_type get_S(const_iterator variable) const { return rsd_(variable-Conditional::factor_.begin()); }
+	rsd_type::const_block_type get_S(const_iterator variable) const { return rsd_(variable - this->begin()); }
 	const Vector& get_sigmas() const {return sigmas_;}
 
 	/**
@@ -133,7 +133,7 @@ public:
 protected:
   rsd_type::column_type get_d_() { return rsd_.column(1+nrParents(), 0); }
   rsd_type::block_type get_R_() { return rsd_(0); }
-  rsd_type::block_type get_S_(iterator variable) { return rsd_(variable-const_cast<const Factor&>(Conditional::factor_).begin()); }
+  rsd_type::block_type get_S_(iterator variable) { return rsd_(variable - this->begin()); }
 
   friend class GaussianFactor;
 
@@ -157,7 +157,7 @@ private:
 			const Vector& sigmas) :
 		rsd_(matrix_), sigmas_(sigmas) {
   	nrFrontals_ = nrFrontals;
-		std::copy(firstKey, lastKey, back_inserter(factor_.keys_));
+		std::copy(firstKey, lastKey, back_inserter(keys_));
 		rsd_.assignNoalias(matrices);
 	}
 
diff --git a/linear/GaussianFactor.cpp b/linear/GaussianFactor.cpp
index 7f7459314..c686b77fe 100644
--- a/linear/GaussianFactor.cpp
+++ b/linear/GaussianFactor.cpp
@@ -32,15 +32,10 @@ using namespace boost::lambda;
 namespace gtsam {
 
 /* ************************************************************************* */
-inline void GaussianFactor::checkSorted() const {
+inline void GaussianFactor::assertInvariants() const {
 #ifndef NDEBUG
-  // Make sure variables are sorted
-  assert(keys_.size()+1 == Ab_.nBlocks());
-  for(size_t varpos=0; varpos<keys_.size(); ++varpos) {
-    if(varpos > 0) {
-      assert(keys_[varpos] > keys_[varpos-1]);
-    }
-  }
+  Factor::assertInvariants();
+  assert((keys_.size() == 0 && Ab_.size1() == 0 && Ab_.nBlocks() == 0) || keys_.size()+1 == Ab_.nBlocks());
 #endif
 }
 
@@ -48,16 +43,18 @@ inline void GaussianFactor::checkSorted() const {
 GaussianFactor::GaussianFactor(const GaussianFactor& gf) :
     Factor(gf), model_(gf.model_), firstNonzeroBlocks_(gf.firstNonzeroBlocks_), Ab_(matrix_) {
   Ab_.assignNoalias(gf.Ab_);
+  assertInvariants();
 }
 
 /* ************************************************************************* */
-GaussianFactor::GaussianFactor() : Ab_(matrix_) {}
+GaussianFactor::GaussianFactor() : Ab_(matrix_) { assertInvariants(); }
 
 /* ************************************************************************* */
 GaussianFactor::GaussianFactor(const Vector& b_in) : firstNonzeroBlocks_(b_in.size(), 0), Ab_(matrix_) {
   size_t dims[] = { 1 };
   Ab_.copyStructureFrom(ab_type(matrix_, dims, dims+1, b_in.size()));
   getb() = b_in;
+  assertInvariants();
 }
 
 /* ************************************************************************* */
@@ -68,6 +65,7 @@ GaussianFactor::GaussianFactor(Index i1, const Matrix& A1,
   Ab_.copyStructureFrom(ab_type(matrix_, dims, dims+2, b.size()));
 	Ab_(0) = A1;
 	getb() = b;
+	assertInvariants();
 }
 
 /* ************************************************************************* */
@@ -79,6 +77,7 @@ GaussianFactor::GaussianFactor(Index i1, const Matrix& A1, Index i2, const Matri
   Ab_(0) = A1;
   Ab_(1) = A2;
   getb() = b;
+  assertInvariants();
 }
 
 /* ************************************************************************* */
@@ -91,6 +90,7 @@ GaussianFactor::GaussianFactor(Index i1, const Matrix& A1, Index i2, const Matri
   Ab_(1) = A2;
   Ab_(2) = A3;
   getb() = b;
+  assertInvariants();
 }
 
 /* ************************************************************************* */
@@ -108,8 +108,7 @@ GaussianFactor::GaussianFactor(const std::vector<std::pair<Index, Matrix> > &ter
   for(size_t j=0; j<terms.size(); ++j)
     Ab_(j) = terms[j].second;
   getb() = b;
-  Factor::permuted_ = false;
-  checkSorted();
+  assertInvariants();
 }
 
 /* ************************************************************************* */
@@ -133,8 +132,7 @@ GaussianFactor::GaussianFactor(const std::list<std::pair<Index, Matrix> > &terms
     ++ j;
   }
   getb() = b;
-  Factor::permuted_ = false;
-  checkSorted();
+  assertInvariants();
 }
 
 /* ************************************************************************* */
@@ -142,60 +140,9 @@ GaussianFactor::GaussianFactor(const GaussianConditional& cg) : Factor(cg), mode
   Ab_.assignNoalias(cg.rsd_);
   // todo SL: make firstNonzeroCols triangular?
   firstNonzeroBlocks_.resize(cg.get_d().size(), 0);	// set sigmas from precisions
+  assertInvariants();
 }
 
-///* ************************************************************************* */
-//GaussianFactor::GaussianFactor(const vector<shared_ptr> & factors)
-//{
-//	bool verbose = false;
-//	if (verbose) cout << "GaussianFactor::GaussianFactor (factors)" << endl;
-//
-//	// Create RHS and sigmas of right size by adding together row counts
-//  size_t m = 0;
-//  BOOST_FOREACH(const shared_ptr& factor, factors) m += factor->numberOfRows();
-//  b_ = Vector(m);
-//  Vector sigmas(m);
-//
-//  size_t pos = 0; // save last position inserted into the new rhs vector
-//
-//  // iterate over all factors
-//  bool constrained = false;
-//  BOOST_FOREACH(const shared_ptr& factor, factors){
-//  	if (verbose) factor->print();
-//    // number of rows for factor f
-//    const size_t mf = factor->numberOfRows();
-//
-//    // copy the rhs vector from factor to b
-//    const Vector bf = factor->get_b();
-//    for (size_t i=0; i<mf; i++) b_(pos+i) = bf(i);
-//
-//    // copy the model_
-//    for (size_t i=0; i<mf; i++) sigmas(pos+i) = factor->model_->sigma(i);
-//
-//    // update the matrices
-//    append_factor(factor,m,pos);
-//
-//    // check if there are constraints
-//    if (verbose) factor->model_->print("Checking for zeros");
-//    if (!constrained && factor->model_->isConstrained()) {
-//    	constrained = true;
-//    	if (verbose) cout << "Found a constraint!" << endl;
-//    }
-//
-//    pos += mf;
-//  }
-//
-//  if (verbose) cout << "GaussianFactor::GaussianFactor done" << endl;
-//
-//  if (constrained) {
-//	  model_ = noiseModel::Constrained::MixedSigmas(sigmas);
-//	  if (verbose) model_->print("Just created Constraint ^");
-//  } else {
-//	  model_ = noiseModel::Diagonal::Sigmas(sigmas);
-//	  if (verbose) model_->print("Just created Diagonal");
-//  }
-//}
-
 /* ************************************************************************* */
 void GaussianFactor::print(const string& s) const {
   cout << s << "\n";
@@ -211,15 +158,6 @@ void GaussianFactor::print(const string& s) const {
   }
 }
 
-///* ************************************************************************* */
-//size_t GaussianFactor::getDim(Index key) const {
-//	const_iterator it = findA(key);
-//	if (it != end())
-//		return it->second.size2();
-//	else
-//		return 0;
-//}
-
 /* ************************************************************************* */
 // Check if two linear factors are equal
 bool GaussianFactor::equals(const GaussianFactor& f, double tol) const {
@@ -241,11 +179,39 @@ bool GaussianFactor::equals(const GaussianFactor& f, double tol) const {
 
 /* ************************************************************************* */
 void GaussianFactor::permuteWithInverse(const Permutation& inversePermutation) {
-  this->permuted_.value = true;
-  BOOST_FOREACH(Index& key, keys_) { key = inversePermutation[key]; }
+
+  // Build a map from the new variable indices to the old slot positions.
+  typedef map<size_t, size_t, std::less<size_t>, boost::fast_pool_allocator<std::pair<const size_t, size_t> > > SourceSlots;
+  SourceSlots sourceSlots;
+  for(size_t j=0; j<keys_.size(); ++j)
+    sourceSlots.insert(make_pair(inversePermutation[keys_[j]], j));
+
+  // Build a vector of variable dimensions in the new order
+  vector<size_t> dimensions(keys_.size() + 1);
+  size_t j = 0;
+  BOOST_FOREACH(const SourceSlots::value_type& sourceSlot, sourceSlots) {
+    dimensions[j++] = Ab_(sourceSlot.second).size2();
+  }
+  assert(j == keys_.size());
+  dimensions.back() = 1;
+
+  // Copy the variables and matrix into the new order
+  vector<Index> oldKeys(keys_.size());
+  keys_.swap(oldKeys);
+  matrix_type oldMatrix;
+  ab_type oldAb(oldMatrix, dimensions.begin(), dimensions.end(), Ab_.size1());
+  Ab_.swap(oldAb);
+  j = 0;
+  BOOST_FOREACH(const SourceSlots::value_type& sourceSlot, sourceSlots) {
+    keys_[j] = sourceSlot.first;
+    ublas::noalias(Ab_(j++)) = oldAb(sourceSlot.second);
+  }
+  ublas::noalias(Ab_(j)) = oldAb(j);
+
   // Since we're permuting the variables, ensure that entire rows from this
   // factor are copied when Combine is called
   BOOST_FOREACH(size_t& varpos, firstNonzeroBlocks_) { varpos = 0; }
+  assertInvariants();
 }
 
 /* ************************************************************************* */
@@ -334,21 +300,6 @@ pair<Matrix,Vector> GaussianFactor::matrix(bool weight) const {
 
 /* ************************************************************************* */
 Matrix GaussianFactor::matrix_augmented(bool weight) const {
-//	// get pointers to the matrices
-//	vector<const Matrix *> matrices;
-//	BOOST_FOREACH(Index j, ordering) {
-//		const Matrix& Aj = get_A(j);
-//		matrices.push_back(&Aj);
-//	}
-//
-//	// load b into a matrix
-//	size_t rows = b_.size();
-//	Matrix B_mat(rows, 1);
-//	memcpy(B_mat.data().begin(), b_.data().begin(), rows*sizeof(double));
-//	matrices.push_back(&B_mat);
-
-	// divide in sigma so error is indeed 0.5*|Ax-b|
-//	Matrix Ab = collect(matrices);
 	if (weight) { Matrix Ab(Ab_.range(0,Ab_.nBlocks())); model_->WhitenInPlace(Ab); return Ab; }
 	else return Ab_.range(0, Ab_.nBlocks());
 }
@@ -381,40 +332,12 @@ GaussianFactor::sparse(const Dimensions& columnIndices) const {
 	return boost::tuple<list<int>, list<int>, list<double> >(I,J,S);
 }
 
-///* ************************************************************************* */
-//void GaussianFactor::append_factor(GaussianFactor::shared_ptr f, size_t m, size_t pos) {
-//
-//	// iterate over all matrices from the factor f
-//	BOOST_FOREACH(const NamedMatrix& p, f->As_) {
-//		Index key = p.first;
-//		const Matrix& Aj = p.second;
-//
-//		// find the corresponding matrix among As
-//		iterator mine = findA(key);
-//		const bool exists = (mine != end());
-//
-//		// find rows and columns
-//		const size_t n = Aj.size2();
-//
-//		// use existing or create new matrix
-//		if (exists)
-//		  copy(Aj.data().begin(), Aj.data().end(), (mine->second).data().begin()+pos*n);
-//		else {
-//			Matrix Z = zeros(m, n);
-//			copy(Aj.data().begin(), Aj.data().end(), Z.data().begin()+pos*n);
-//			insert(key, Z);
-//		}
-//
-//	} // FOREACH
-//}
-
 /* ************************************************************************* */
 GaussianConditional::shared_ptr GaussianFactor::eliminateFirst() {
 
   assert(Ab_.rowStart() == 0 && Ab_.rowEnd() == matrix_.size1() && Ab_.firstBlock() == 0);
-  assert(!permuted_.value);
   assert(!keys_.empty());
-  checkSorted();
+  assertInvariants();
 
   static const bool debug = false;
 
@@ -514,12 +437,12 @@ GaussianConditional::shared_ptr GaussianFactor::eliminateFirst() {
   }
   toc("eliminateFirst: rowstarts");
 
-  checkSorted();
-
   if(debug) print("Eliminated factor: ");
 
   toc("eliminateFirst");
 
+  assertInvariants();
+
   return conditional;
 }
 
@@ -527,9 +450,8 @@ GaussianConditional::shared_ptr GaussianFactor::eliminateFirst() {
 GaussianBayesNet::shared_ptr GaussianFactor::eliminate(size_t nrFrontals) {
 
   assert(Ab_.rowStart() == 0 && Ab_.rowEnd() == matrix_.size1() && Ab_.firstBlock() == 0);
-  assert(!permuted_.value);
   assert(keys_.size() >= nrFrontals);
-  checkSorted();
+  assertInvariants();
 
   static const bool debug = false;
 
@@ -635,12 +557,12 @@ GaussianBayesNet::shared_ptr GaussianFactor::eliminate(size_t nrFrontals) {
   }
   toc("eliminate: rowstarts");
 
-  checkSorted();
-
   if(debug) print("Eliminated factor: ");
 
   toc("eliminate");
 
+  assertInvariants();
+
   return conditionals;
 
 }
@@ -727,10 +649,7 @@ GaussianFactor::shared_ptr GaussianFactor::Combine(const GaussianFactorGraph& fa
     assert(factor.firstNonzeroBlocks_.size() == factor.numberOfRows());
     BOOST_FOREACH(const size_t factorFirstNonzeroVarpos, factor.firstNonzeroBlocks_) {
       Index firstNonzeroVar;
-      if(factor.permuted_.value == true)
-        firstNonzeroVar = *std::min_element(factor.keys_.begin(), factor.keys_.end());
-      else
-        firstNonzeroVar = factor.keys_[factorFirstNonzeroVarpos];
+      firstNonzeroVar = factor.keys_[factorFirstNonzeroVarpos];
       rowSources.push_back(_RowSource(firstNonzeroVar, factorI, factorRowI));
       ++ factorRowI;
     }
@@ -779,10 +698,6 @@ GaussianFactor::shared_ptr GaussianFactor::Combine(const GaussianFactorGraph& fa
     std::vector<Index>::const_iterator keyitend = factor.keys_.end();
     while(keyit != keyitend) {
       const size_t varpos = *varposIt;
-      // If the factor is permuted, the varpos's in the joint factor could be
-      // out of order.
-      if(factor.permuted_.value == true && varpos < ret->firstNonzeroBlocks_[row])
-        ret->firstNonzeroBlocks_[row] = varpos;
       assert(variables[varpos] == *keyit);
       ab_type::block_type retBlock(ret->Ab_(varpos));
       const ab_type::const_block_type factorBlock(factor.getA(keyit));
@@ -811,7 +726,7 @@ GaussianFactor::shared_ptr GaussianFactor::Combine(const GaussianFactorGraph& fa
 
   if(debug) ret->print("Combined factor: ");
 
-  ret->checkSorted();
+  ret->assertInvariants();
 
   return ret;
 }
@@ -889,10 +804,7 @@ GaussianFactor::shared_ptr GaussianFactor::Combine(const GaussianFactorGraph& fa
     const GaussianFactor& sourceFactor(*factors[sourceFactorI]);
     for(size_t sourceFactorRow = 0; sourceFactorRow < sourceFactor.numberOfRows(); ++sourceFactorRow) {
       Index firstNonzeroVar;
-      if(sourceFactor.permuted_.value)
-        firstNonzeroVar = *std::min_element(sourceFactor.begin(), sourceFactor.end());
-      else
-        firstNonzeroVar = sourceFactor.keys_[sourceFactor.firstNonzeroBlocks_[sourceFactorRow]];
+      firstNonzeroVar = sourceFactor.keys_[sourceFactor.firstNonzeroBlocks_[sourceFactorRow]];
       rowSources.push_back(_RowSource(firstNonzeroVar, sourceFactorI, sourceFactorRow));
     }
     if(sourceFactor.model_->isConstrained()) anyConstrained = true;
@@ -922,7 +834,6 @@ GaussianFactor::shared_ptr GaussianFactor::Combine(const GaussianFactorGraph& fa
       if(sourceSlot != numeric_limits<Index>::max()) {
         const GaussianFactor& source(*factors[rowSources[row].factorI]);
         const size_t sourceRow = rowSources[row].factorRowI;
-        assert(!source.permuted_.value || source.firstNonzeroBlocks_[sourceRow] == 0);
         if(source.firstNonzeroBlocks_[sourceRow] <= sourceSlot) {
           const ab_type::const_block_type sourceBlock(source.Ab_(sourceSlot));
           ublas::noalias(ublas::row(combinedBlock, row)) = ublas::row(sourceBlock, sourceRow);
@@ -955,159 +866,9 @@ GaussianFactor::shared_ptr GaussianFactor::Combine(const GaussianFactorGraph& fa
   else combined->model_ = noiseModel::Diagonal::Sigmas(sigmas);
   toc("Combine 6: noise model");
 
-  combined->checkSorted();
+  combined->assertInvariants();
 
   return combined;
 }
 
-
-///* ************************************************************************* */
-//static GaussianFactor::shared_ptr
-//GaussianFactor::Combine(const GaussianFactorGraph& factorGraph, const std::vector<size_t>& factors) {
-//
-//  // Determine row count
-//  size_t m = 0;
-//  BOOST_FOREACH(const size_t& factor, factors) {
-//    m += factorGraph[factor]->numberOfRows();
-//  }
-//
-//}
-
-///* ************************************************************************* */
-///* Note, in place !!!!
-// * Do incomplete QR factorization for the first n columns
-// * We will do QR on all matrices and on RHS
-// * Then take first n rows and make a GaussianConditional,
-// * and last rows to make a new joint linear factor on separator
-// */
-///* ************************************************************************* */
-//
-//pair<GaussianBayesNet, GaussianFactor::shared_ptr>
-//GaussianFactor::eliminateMatrix(Matrix& Ab, SharedDiagonal model,
-//		        const Ordering& frontals, const Ordering& separators,
-//		        const Dimensions& dimensions) {
-//	bool verbose = false;
-//
-//	// Use in-place QR on dense Ab appropriate to NoiseModel
-//	if (verbose) model->print("Before QR");
-//	SharedDiagonal noiseModel = model->QR(Ab);
-//	if (verbose) model->print("After QR");
-//
-//	// get dimensions of the eliminated variable
-//	// TODO: this is another map find that should be avoided !
-//	size_t n1 = dimensions.at(frontals.front()), n = Ab.size2() - 1;
-//
-//	// Get alias to augmented RHS d
-//	ublas::matrix_column<Matrix> d(Ab,n);
-//
-//	// extract the conditionals
-//	GaussianBayesNet bn;
-//	size_t n0 = 0;
-//	Ordering::const_iterator itFrontal1 = frontals.begin(), itFrontal2;
-//	for(; itFrontal1!=frontals.end(); itFrontal1++) {
-//		n1 = n0 + dimensions.at(*itFrontal1);
-//		// create base conditional Gaussian
-//		GaussianConditional::shared_ptr conditional(new GaussianConditional(*itFrontal1,
-//				sub(d,  n0, n1),                   // form d vector
-//				sub(Ab, n0, n1, n0, n1),           // form R matrix
-//				sub(noiseModel->sigmas(),n0,n1))); // get standard deviations
-//
-//		// add parents to the conditional
-//		itFrontal2 = itFrontal1;
-//		itFrontal2 ++;
-//		size_t j = n1;
-//		for (; itFrontal2!=frontals.end(); itFrontal2++) {
-//			size_t dim = dimensions.at(*itFrontal2);
-//			conditional->add(*itFrontal2, sub(Ab, n0, n1, j, j+dim));
-//			j+=dim;
-//		}
-//		BOOST_FOREACH(Index cur_key, separators) {
-//			size_t dim = dimensions.at(cur_key);
-//			conditional->add(cur_key, sub(Ab, n0, n1, j, j+dim));
-//			j+=dim;
-//		}
-//		n0 = n1;
-//		bn.push_back(conditional);
-//	}
-//
-//	// if m<n1, this factor cannot be eliminated
-//	size_t maxRank = noiseModel->dim();
-//	if (maxRank<n1) {
-//		cout << "Perhaps your factor graph is singular." << endl;
-//		cout << "Here are the keys involved in the factor now being eliminated:" << endl;
-//		separators.print("Keys");
-//		cout << "The first key, '" << frontals.front() << "', corresponds to the variable being eliminated" << endl;
-//		throw(domain_error("GaussianFactor::eliminate: fewer constraints than unknowns"));
-//	}
-//
-//	// extract the new factor
-//	GaussianFactor::shared_ptr factor(new GaussianFactor);
-//	size_t j = n1;
-//	BOOST_FOREACH(Index cur_key, separators) {
-//		size_t dim = dimensions.at(cur_key); // TODO avoid find !
-//		factor->insert(cur_key, sub(Ab, n1, maxRank, j, j+dim)); // TODO: handle zeros properly
-//		j+=dim;
-//	}
-//
-//	// Set sigmas
-//	// set the right model here
-//	if (noiseModel->isConstrained())
-//		factor->model_ = noiseModel::Constrained::MixedSigmas(sub(noiseModel->sigmas(),n1,maxRank));
-//	else
-//		factor->model_ = noiseModel::Diagonal::Sigmas(sub(noiseModel->sigmas(),n1,maxRank));
-//
-//	// extract ds vector for the new b
-//	factor->set_b(sub(d, n1, maxRank));
-//
-//	return make_pair(bn, factor);
-//
-//}
-//
-///* ************************************************************************* */
-//pair<GaussianConditional::shared_ptr, GaussianFactor::shared_ptr>
-//GaussianFactor::eliminateMatrix(Matrix& Ab, SharedDiagonal model,
-//		        Index frontal, const Ordering& separator,
-//		        const Dimensions& dimensions) {
-//	Ordering frontals; frontals += frontal;
-//	pair<GaussianBayesNet, shared_ptr> ret =
-//			eliminateMatrix(Ab, model, frontals, separator, dimensions);
-//	return make_pair(*ret.first.begin(), ret.second);
-//}
-///* ************************************************************************* */
-//pair<GaussianConditional::shared_ptr, GaussianFactor::shared_ptr>
-//GaussianFactor::eliminate(Index key) const
-//{
-//	// if this factor does not involve key, we exit with empty CG and LF
-//	const_iterator it = findA(key);
-//	if (it==end()) {
-//		// Conditional Gaussian is just a parent-less node with P(x)=1
-//		GaussianFactor::shared_ptr lf(new GaussianFactor);
-//		GaussianConditional::shared_ptr cg(new GaussianConditional(key));
-//		return make_pair(cg,lf);
-//	}
-//
-//	// create an internal ordering that eliminates key first
-//	Ordering ordering;
-//	ordering += key;
-//	BOOST_FOREACH(Index k, keys())
-//		if (k != key) ordering += k;
-//
-//	// extract [A b] from the combined linear factor (ensure that x is leading)
-//	Matrix Ab = matrix_augmented(ordering,false);
-//
-//	// TODO: this is where to split
-//	ordering.pop_front();
-//	return eliminateMatrix(Ab, model_, key, ordering, dimensions());
-//}
-
-/* ************************************************************************* */
-
-	string symbol(char c, int index) {
-		stringstream ss;
-		ss << c << index;
-		return ss.str();
-	}
-
 }
-/* ************************************************************************* */
-;
diff --git a/linear/GaussianFactor.h b/linear/GaussianFactor.h
index c4de6bcb3..c0580d63c 100644
--- a/linear/GaussianFactor.h
+++ b/linear/GaussianFactor.h
@@ -153,7 +153,7 @@ protected:
   /** Protected mutable accessor for the r.h.s. b. */
 
   /** Internal debug check to make sure variables are sorted */
-  void checkSorted() const;
+  void assertInvariants() const;
 
 public:
 
@@ -215,14 +215,5 @@ public:
 
 }; // GaussianFactor
 
-/* ************************************************************************* */
-
-/**
- * creates a C++ string a la "x3", "m768"
- * @param c the base character
- * @param index the integer to be added
- * @return a C++ string
- */
-std::string symbol(char c, int index);
 
 } // namespace gtsam
diff --git a/linear/GaussianFactorGraph.cpp b/linear/GaussianFactorGraph.cpp
index c3071bade..569c370c4 100644
--- a/linear/GaussianFactorGraph.cpp
+++ b/linear/GaussianFactorGraph.cpp
@@ -358,11 +358,8 @@ GaussianFactorGraph GaussianFactorGraph::add_priors(double sigma, const Gaussian
 	GaussianFactorGraph result = *this;
 
 	// for each of the variables, add a prior
-
 	for(Index var=0; var<variableIndex.size(); ++var) {
-	  const GaussianVariableIndex<>::mapped_factor_type& factor_pos(variableIndex[var].front());
-	  const GaussianFactor& factor(*operator[](factor_pos.factorIndex));
-	  size_t dim = factor.getDim(factor.begin() + factor_pos.variablePosition);
+	  size_t dim = variableIndex.dim(var);
 		Matrix A = eye(dim);
 		Vector b = zero(dim);
 		SharedDiagonal model = noiseModel::Isotropic::Sigma(dim,sigma);
diff --git a/linear/VectorValues.h b/linear/VectorValues.h
index 16632b1fc..e01a9c65c 100644
--- a/linear/VectorValues.h
+++ b/linear/VectorValues.h
@@ -59,6 +59,11 @@ public:
    */
   VectorValues(const std::vector<size_t>& dimensions, const Vector& values);
 
+  /** Named constructor to create a VectorValues that matches the structure of
+   * the specified VectorValues, but do not initialize the new values.
+   */
+  static VectorValues SameStructure(const VectorValues& otherValues);
+
   /** Element access */
   mapped_type operator[](Index variable);
   const_mapped_type operator[](Index variable) const;
@@ -201,6 +206,13 @@ inline VectorValues::VectorValues(const std::vector<size_t>& dimensions, const V
   assert(varStarts_.back() == values.size());
 }
 
+inline VectorValues VectorValues::SameStructure(const VectorValues& otherValues) {
+  VectorValues ret;
+  ret.varStarts_ = otherValues.varStarts_;
+  ret.values_.resize(ret.varStarts_.back(), false);
+  return ret;
+}
+
 inline VectorValues::mapped_type VectorValues::operator[](Index variable) {
   checkVariable(variable);
   return boost::numeric::ublas::project(values_,
diff --git a/linear/tests/testGaussianFactor.cpp b/linear/tests/testGaussianFactor.cpp
index 35ee94083..079066d47 100644
--- a/linear/tests/testGaussianFactor.cpp
+++ b/linear/tests/testGaussianFactor.cpp
@@ -678,6 +678,52 @@ TEST ( GaussianFactor, constraint_eliminate2 )
 	CHECK(assert_equal(expectedCG, *actualCG, 1e-4));
 }
 
+/* ************************************************************************* */
+TEST(GaussianFactor, permuteWithInverse)
+{
+  Matrix A1 = Matrix_(2,2,
+      1.0, 2.0,
+      3.0, 4.0);
+  Matrix A2 = Matrix_(2,1,
+      5.0,
+      6.0);
+  Matrix A3 = Matrix_(2,3,
+      7.0, 8.0, 9.0,
+      10.0, 11.0, 12.0);
+  Vector b = Vector_(2, 13.0, 14.0);
+
+  Permutation inversePermutation(6);
+  inversePermutation[0] = 5;
+  inversePermutation[1] = 4;
+  inversePermutation[2] = 3;
+  inversePermutation[3] = 2;
+  inversePermutation[4] = 1;
+  inversePermutation[5] = 0;
+
+  GaussianFactor actual(1, A1, 3, A2, 5, A3, b, sharedSigma(2, 1.0));
+  GaussianFactorGraph actualFG; actualFG.push_back(GaussianFactor::shared_ptr(new GaussianFactor(actual)));
+  GaussianVariableIndex<> actualIndex(actualFG);
+  actual.permuteWithInverse(inversePermutation);
+  actualIndex.permute(*inversePermutation.inverse());
+
+  GaussianFactor expected(0, A3, 2, A2, 4, A1, b, sharedSigma(2, 1.0));
+  GaussianFactorGraph expectedFG; expectedFG.push_back(GaussianFactor::shared_ptr(new GaussianFactor(expected)));
+  GaussianVariableIndex<> expectedIndex(expectedFG);
+
+  CHECK(assert_equal(expected, actual));
+
+  // todo: fix this!!!  VariableIndex should not hold slots
+  for(Index j=0; j<actualIndex.size(); ++j) {
+    BOOST_FOREACH(typename GaussianVariableIndex<>::mapped_factor_type& factor_pos, actualIndex[j]) {
+      factor_pos.variablePosition = numeric_limits<Index>::max(); }
+  }
+  for(Index j=0; j<expectedIndex.size(); ++j) {
+    BOOST_FOREACH(typename GaussianVariableIndex<>::mapped_factor_type& factor_pos, expectedIndex[j]) {
+      factor_pos.variablePosition = numeric_limits<Index>::max(); }
+  }
+  CHECK(assert_equal(expectedIndex, actualIndex));
+}
+
 ///* ************************************************************************* */
 //TEST( GaussianFactor, erase)
 //{
@@ -728,6 +774,7 @@ TEST ( GaussianFactor, constraint_eliminate2 )
 //	CHECK(assert_equal(factor_expected, *factor));
 //}
 
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */