From 35e5953d343cea45687e4544b0fcfea2c164f0fb Mon Sep 17 00:00:00 2001
From: Alex Cunningham <alexgcunningham@gmail.com>
Date: Thu, 21 Oct 2010 07:08:12 +0000
Subject: [PATCH] More utility functions

---
 linear/GaussianFactor.cpp | 50 ++++++++++++++++++++++-----------------
 linear/GaussianFactor.h   |  3 +++
 2 files changed, 31 insertions(+), 22 deletions(-)

diff --git a/linear/GaussianFactor.cpp b/linear/GaussianFactor.cpp
index 4c484aaf5..6b8dee4ff 100644
--- a/linear/GaussianFactor.cpp
+++ b/linear/GaussianFactor.cpp
@@ -643,6 +643,32 @@ static vector<size_t> columnDimensions(
 	return dims;
 }
 
+// To do this, we merge-sort the rows so that the column indices of the first structural
+// non-zero in each row increase monotonically.
+vector<_RowSource> computeRowPermutation(size_t m, const vector<size_t>& factorIndices,
+		const FactorGraph<GaussianFactor>& factorGraph) {
+	vector<_RowSource> rowSources;
+	rowSources.reserve(m);
+	size_t i1 = 0;
+	BOOST_FOREACH(const size_t i2, factorIndices) {
+		const GaussianFactor& factor(*factorGraph[i2]);
+		size_t factorRowI = 0;
+		assert(factor.get_firstNonzeroBlocks().size() == factor.numberOfRows());
+		BOOST_FOREACH(const size_t factorFirstNonzeroVarpos, factor.get_firstNonzeroBlocks()) {
+			Index firstNonzeroVar;
+			firstNonzeroVar = factor.keys()[factorFirstNonzeroVarpos];
+			rowSources.push_back(_RowSource(firstNonzeroVar, i1, factorRowI));
+			++ factorRowI;
+		}
+		assert(factorRowI == factor.numberOfRows());
+		++ i1;
+	}
+	assert(rowSources.size() == m);
+	assert(i1 == factorIndices.size());
+	sort(rowSources.begin(), rowSources.end());
+	return rowSources;
+}
+
 template<class STORAGE>
 GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph<GaussianFactor>& factorGraph,
     const GaussianVariableIndex<STORAGE>& variableIndex, const vector<size_t>& factorIndices,
@@ -674,29 +700,9 @@ GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph<GaussianFac
   combinedFactor->keys_.insert(combinedFactor->keys_.end(), variables.begin(), variables.end());
   toc("Combine: set up empty");
 
-  // Compute a row permutation that maintains a staircase pattern in the new
-  // combined factor.  To do this, we merge-sort the rows so that the column
-  // indices of the first structural non-zero in each row increase monotonically.
+  // Compute a row permutation that maintains a staircase pattern in the new combined factor.
   tic("Combine: sort rows");
-  vector<_RowSource> rowSources;
-  rowSources.reserve(m);
-  size_t i1 = 0;
-  BOOST_FOREACH(const size_t i2, factorIndices) {
-    const GaussianFactor& factor(*factorGraph[i2]);
-    size_t factorRowI = 0;
-    assert(factor.firstNonzeroBlocks_.size() == factor.numberOfRows());
-    BOOST_FOREACH(const size_t factorFirstNonzeroVarpos, factor.firstNonzeroBlocks_) {
-      Index firstNonzeroVar;
-      firstNonzeroVar = factor.keys_[factorFirstNonzeroVarpos];
-      rowSources.push_back(_RowSource(firstNonzeroVar, i1, factorRowI));
-      ++ factorRowI;
-    }
-    assert(factorRowI == factor.numberOfRows());
-    ++ i1;
-  }
-  assert(rowSources.size() == m);
-  assert(i1 == factorIndices.size());
-  sort(rowSources.begin(), rowSources.end());
+  vector<_RowSource> rowSources = computeRowPermutation(m, factorIndices, factorGraph);
   toc("Combine: sort rows");
 
   // Fill in the rows of the new factor in sorted order.  Fill in the array of
diff --git a/linear/GaussianFactor.h b/linear/GaussianFactor.h
index 65e3b301a..004523770 100644
--- a/linear/GaussianFactor.h
+++ b/linear/GaussianFactor.h
@@ -175,6 +175,9 @@ public:
 	/** get a copy of model */
 	const SharedDiagonal& get_model() const { return model_;  }
 
+	/** get the indices list */
+	const std::vector<size_t>& get_firstNonzeroBlocks() const { return firstNonzeroBlocks_; }
+
 	bool isConstrained() const {return model_->isConstrained();}
 
 	/**