From 145f273e9854d1289f8201bfaae2c4b106ef6c29 Mon Sep 17 00:00:00 2001
From: Alex Cunningham <alexgcunningham@gmail.com>
Date: Thu, 21 Oct 2010 08:18:20 +0000
Subject: [PATCH] Yet even more utility functions

---
 linear/GaussianFactor.cpp | 41 +++++++++++++++++++++++----------------
 linear/GaussianFactor.h   |  4 ++++
 2 files changed, 28 insertions(+), 17 deletions(-)

diff --git a/linear/GaussianFactor.cpp b/linear/GaussianFactor.cpp
index 6b8dee4ff..f23b6b8a8 100644
--- a/linear/GaussianFactor.cpp
+++ b/linear/GaussianFactor.cpp
@@ -669,6 +669,27 @@ vector<_RowSource> computeRowPermutation(size_t m, const vector<size_t>& factorI
 	return rowSources;
 }
 
+void copyMatrices(boost::shared_ptr<GaussianFactor> combinedFactor, size_t row,
+		const GaussianFactor& factor, const std::vector<std::vector<size_t> >& variablePositions,
+		const size_t factorRow, const size_t factorI, const vector<Index>& variables) {
+	const static bool debug = false;
+    const size_t factorFirstNonzeroVarpos = factor.get_firstNonzeroBlocks()[factorRow];
+	std::vector<Index>::const_iterator keyit = factor.keys().begin() + factorFirstNonzeroVarpos;
+	std::vector<size_t>::const_iterator varposIt = variablePositions[factorI].begin() + factorFirstNonzeroVarpos;
+	combinedFactor->set_firstNonzeroBlocks(row, *varposIt);
+	if(debug) cout << "  copying starting at varpos " << *varposIt << " (variable " << variables[*varposIt] << ")" << endl;
+	std::vector<Index>::const_iterator keyitend = factor.keys().end();
+	while(keyit != keyitend) {
+		const size_t varpos = *varposIt;
+		assert(variables[varpos] == *keyit);
+		GaussianFactor::ab_type::block_type retBlock(combinedFactor->getAb(varpos));
+		const GaussianFactor::ab_type::const_block_type factorBlock(factor.getA(keyit));
+		ublas::noalias(ublas::row(retBlock, row)) = ublas::row(factorBlock, factorRow);
+		++ keyit;
+		++ varposIt;
+	}
+}
+
 template<class STORAGE>
 GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph<GaussianFactor>& factorGraph,
     const GaussianVariableIndex<STORAGE>& variableIndex, const vector<size_t>& factorIndices,
@@ -722,7 +743,6 @@ GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph<GaussianFac
     const size_t factorI = rowSource.factorI;
     const GaussianFactor& factor(*factorGraph[factorIndices[factorI]]);
     const size_t factorRow = rowSource.factorRowI;
-    const size_t factorFirstNonzeroVarpos = factor.firstNonzeroBlocks_[factorRow];
 
     if(debug)
       cout << "Combined row " << row << " is from row " << factorRow << " of factor " << factorIndices[factorI] << endl;
@@ -731,22 +751,9 @@ GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph<GaussianFac
     combinedFactor->getb()(row) = factor.getb()(factorRow);
     sigmas(row) = factor.get_sigmas()(factorRow);
 
-    // Copy the row of A variable by variable, starting at the first nonzero
-    // variable.
-    std::vector<Index>::const_iterator keyit = factor.keys_.begin() + factorFirstNonzeroVarpos;
-    std::vector<size_t>::const_iterator varposIt = variablePositions[factorI].begin() + factorFirstNonzeroVarpos;
-    combinedFactor->firstNonzeroBlocks_[row] = *varposIt;
-    if(debug) cout << "  copying starting at varpos " << *varposIt << " (variable " << variables[*varposIt] << ")" << endl;
-    std::vector<Index>::const_iterator keyitend = factor.keys_.end();
-    while(keyit != keyitend) {
-      const size_t varpos = *varposIt;
-      assert(variables[varpos] == *keyit);
-      ab_type::block_type retBlock(combinedFactor->Ab_(varpos));
-      const ab_type::const_block_type factorBlock(factor.getA(keyit));
-      ublas::noalias(ublas::row(retBlock, row)) = ublas::row(factorBlock, factorRow);
-      ++ keyit;
-      ++ varposIt;
-    }
+    // Copy the row of A variable by variable, starting at the first nonzero variable.
+    copyMatrices(combinedFactor, row, factor, variablePositions, factorRow, factorI, variables);
+
 #ifndef NDEBUG
     // Debug check, make sure the first column of nonzeros increases monotonically
     if(row != 0)
diff --git a/linear/GaussianFactor.h b/linear/GaussianFactor.h
index 004523770..be644e574 100644
--- a/linear/GaussianFactor.h
+++ b/linear/GaussianFactor.h
@@ -134,6 +134,8 @@ public:
 	/** Get a view of the A matrix for the variable pointed to be the given key iterator */
 	ab_type::const_block_type getA(const_iterator variable) const { return Ab_(variable - keys_.begin());	}
   ab_type::block_type getA(iterator variable) { return Ab_(variable - keys_.begin()); }
+  ab_type::block_type getAb(size_t block) { return Ab_(block); }
+
 
 	/** Return the dimension of the variable pointed to by the given key iterator
 	 * todo: Remove this in favor of keeping track of dimensions with variables?
@@ -227,6 +229,8 @@ public:
 
   GaussianBayesNet::shared_ptr eliminate(size_t nrFrontals = 1);
 
+  void set_firstNonzeroBlocks(size_t row, size_t varpos) { firstNonzeroBlocks_[row] = varpos; }
+
 	friend class GaussianFactorGraph;
 	friend class Inference;