diff --git a/cpp/FactorGraph-inl.h b/cpp/FactorGraph-inl.h
index 609e21944..79d3b2e80 100644
--- a/cpp/FactorGraph-inl.h
+++ b/cpp/FactorGraph-inl.h
@@ -261,6 +261,46 @@ void FactorGraph<Factor>::associateFactor(int index, sharedFactor factor) {
   }
 }
 
+/* ************************************************************************* */
+template<class Factor> void FactorGraph<Factor>::checkGraphConsistency() const {
+  // Consistency check for debugging
+
+	// Make sure each factor is listed in its variables index lists
+  for(size_t i=0; i<factors_.size(); i++) {
+  	if(factors_[i] == NULL) {
+  		cout << "** Warning: factor " << i << " is NULL" << endl;
+  	} else {
+  		// Get involved variables
+  		list<Symbol> keys = factors_[i]->keys();
+
+  		// Make sure each involved variable is listed as being associated with this factor
+  		BOOST_FOREACH(const Symbol& var, keys) {
+  			if(std::find(indices_.at(var).begin(), indices_.at(var).end(), i) == indices_.at(var).end())
+  				cout << "*** Factor graph inconsistency: " << (string)var << " is not mapped to factor " << i << endl;
+  		}
+  	}
+  }
+
+  // Make sure each factor listed for a variable actually involves that variable
+  BOOST_FOREACH(const SymbolMap<list<int> >::value_type& var, indices_) {
+  	BOOST_FOREACH(int i, var.second) {
+  		if(i >= factors_.size()) {
+  			cout << "*** Factor graph inconsistency: " << (string)var.first << " lists factor " <<
+  					i << " but the graph does not contain this many factors." << endl;
+  		}
+  		if(factors_[i] == NULL) {
+  			cout << "*** Factor graph inconsistency: " << (string)var.first << " lists factor " <<
+  					i << " but this factor is set to NULL." << endl;
+  		}
+    	list<Symbol> keys = factors_[i]->keys();
+  		if(std::find(keys.begin(), keys.end(), var.first) == keys.end()) {
+  			cout << "*** Factor graph inconsistency: " << (string)var.first << " lists factor " <<
+  					i << " but this factor does not involve this variable." << endl;
+  		}
+  	}
+  }
+}
+
 /* ************************************************************************* */
 template<class Factor> template <class Key, class Factor2>
 PredecessorMap<Key> FactorGraph<Factor>::findMinimumSpanningTree() const {
diff --git a/cpp/FactorGraph.h b/cpp/FactorGraph.h
index e1f00ebde..101a5e9ee 100644
--- a/cpp/FactorGraph.h
+++ b/cpp/FactorGraph.h
@@ -132,6 +132,11 @@ namespace gtsam {
 		template<class Key, class Factor2> void split(const PredecessorMap<Key>& tree,
 				FactorGraph<Factor>& Ab1, FactorGraph<Factor>& Ab2) const;
 
+		/**
+		 * Check consistency of the index map, useful for debugging
+		 */
+		void checkGraphConsistency() const;
+
 	private:
 		/** Associate factor index with the variables connected to the factor */
 		void associateFactor(int index, sharedFactor factor);
diff --git a/cpp/testGaussianFactorGraph.cpp b/cpp/testGaussianFactorGraph.cpp
index 75833a82b..a19ab2e5c 100644
--- a/cpp/testGaussianFactorGraph.cpp
+++ b/cpp/testGaussianFactorGraph.cpp
@@ -842,6 +842,41 @@ TEST( GaussianFactorGraph, split )
 	LONGS_EQUAL(2, Ab2.size());
 }
 
+/* ************************************************************************* */
+TEST(GaussianFactorGraph, replace)
+{
+	SharedDiagonal noise(sharedSigma(3, 1.0));
+
+	GaussianFactorGraph::sharedFactor f1(new GaussianFactor(
+			"x1", eye(3,3), "x2", eye(3,3), zero(3), noise));
+	GaussianFactorGraph::sharedFactor f2(new GaussianFactor(
+			"x2", eye(3,3), "x3", eye(3,3), zero(3), noise));
+	GaussianFactorGraph::sharedFactor f3(new GaussianFactor(
+			"x3", eye(3,3), "x4", eye(3,3), zero(3), noise));
+	GaussianFactorGraph::sharedFactor f4(new GaussianFactor(
+			"x5", eye(3,3), "x6", eye(3,3), zero(3), noise));
+
+	GaussianFactorGraph actual;
+	actual.push_back(f1);
+	actual.checkGraphConsistency();
+	actual.push_back(f2);
+	actual.checkGraphConsistency();
+	actual.push_back(f3);
+	actual.checkGraphConsistency();
+	actual.replace(0, f4);
+	actual.checkGraphConsistency();
+
+	GaussianFactorGraph expected;
+	expected.push_back(f4);
+	actual.checkGraphConsistency();
+	expected.push_back(f2);
+	actual.checkGraphConsistency();
+	expected.push_back(f3);
+	actual.checkGraphConsistency();
+
+	CHECK(assert_equal(expected, actual));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */