Made GaussianFactorGraph::sparse and sparseJacobian functions take no arguments, and instead compute column indices internally

gtsam/linear/GaussianFactorGraph.cpp

@@ -22,6 +22,7 @@
 #include <gtsam/base/debug.h>
 #include <gtsam/base/timing.h>
 #include <gtsam/base/cholesky.h>
+#include <gtsam/base/FastVector.h>
 #include <gtsam/linear/HessianFactor.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/inference/VariableSlots.h>
@@ -80,18 +81,32 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
-	std::vector<boost::tuple<size_t, size_t, double> > GaussianFactorGraph::sparseJacobian(
+	std::vector<boost::tuple<size_t, size_t, double> > GaussianFactorGraph::sparseJacobian() const {
-			const std::vector<size_t>& columnIndices) const {
+	  // First find dimensions of each variable
+	  FastVector<size_t> dims;
+	  BOOST_FOREACH(const sharedFactor& factor, *this) {
+      for(GaussianFactor::const_iterator pos = factor->begin(); pos != factor->end(); ++pos) {
+        if(dims.size() <= *pos)
+          dims.resize(*pos + 1, 0);
+        dims[*pos] = factor->getDim(pos);
+      }
+	  }
+
+	  // Compute first scalar column of each variable
+	  vector<size_t> columnIndices(dims.size()+1, 0);
+	  for(size_t j=1; j<dims.size()+1; ++j)
+	    columnIndices[j] = columnIndices[j-1] + dims[j-1];
+
+	  // Iterate over all factors, adding sparse scalar entries
 		typedef boost::tuple<size_t, size_t, double> triplet;
-		std::vector<triplet> entries;
+		FastVector<triplet> entries;
-		size_t i = 0;
+		size_t row = 0;
 		BOOST_FOREACH(const sharedFactor& factor, *this) {
 			// Convert to JacobianFactor if necessary
 			JacobianFactor::shared_ptr jacobianFactor(
 					boost::dynamic_pointer_cast<JacobianFactor>(factor));
 			if (!jacobianFactor) {
-				HessianFactor::shared_ptr hessian(boost::dynamic_pointer_cast<
+			  HessianFactor::shared_ptr hessian(boost::dynamic_pointer_cast<HessianFactor>(factor));
-						HessianFactor>(factor));
 				if (hessian)
 					jacobianFactor.reset(new JacobianFactor(*hessian));
 				else
@@ -99,33 +114,38 @@ namespace gtsam {
 							"GaussianFactorGraph contains a factor that is neither a JacobianFactor nor a HessianFactor.");
 			}
 
-			// Add entries, adjusting the row index i
+			// Whiten the factor and add entries for it
-			std::vector<triplet> factorEntries(
+			// iterate over all variables in the factor
-					jacobianFactor->sparse(columnIndices));
+			const JacobianFactor whitened(jacobianFactor->whiten());
-			entries.reserve(entries.size() + factorEntries.size());
+			for(JacobianFactor::const_iterator pos=whitened.begin(); pos<whitened.end(); ++pos) {
-			for (size_t k = 0; k < factorEntries.size(); ++k)
+			  JacobianFactor::constABlock whitenedA = whitened.getA(pos);
-				entries.push_back(boost::make_tuple(
+			  // find first column index for this key
-						factorEntries[k].get<0> () + i, factorEntries[k].get<
+			  size_t column_start = columnIndices[*pos];
-								1> (), factorEntries[k].get<2> ()));
+			  for (size_t i = 0; i < (size_t) whitenedA.rows(); i++)
+			    for (size_t j = 0; j < (size_t) whitenedA.cols(); j++) {
+			      double s = whitenedA(i,j);
+			      if (std::abs(s) > 1e-12) entries.push_back(
+			          boost::make_tuple(row+i, column_start+j, s));
+			    }
+			}
+
+			JacobianFactor::constBVector whitenedb(whitened.getb());
+			size_t bcolumn = columnIndices.back();
+			for (size_t i = 0; i < (size_t) whitenedb.size(); i++)
+			  entries.push_back(boost::make_tuple(row+i, bcolumn, whitenedb(i)));
 
 			// Increment row index
-			i += jacobianFactor->rows();
+			row += jacobianFactor->rows();
 		}
-		return entries;
+		return vector<triplet>(entries.begin(), entries.end());
 	}
 
 	/* ************************************************************************* */
-	Matrix GaussianFactorGraph::sparse(const Vector& columnIndices) const {
+	Matrix GaussianFactorGraph::sparse() const {
-
-		// translate from base 1 Vector to vector of base 0 indices
-		std::vector<size_t> indices;
-		for (int i = 0; i < columnIndices.size(); i++)
-			indices.push_back(columnIndices[i] - 1);
 
 		// call sparseJacobian
 		typedef boost::tuple<size_t, size_t, double> triplet;
-		std::vector < boost::tuple<size_t, size_t, double> > result =
+		std::vector<triplet> result = sparseJacobian();
-				sparseJacobian(indices);
 
 		// translate to base 1 matrix
 		size_t nzmax = result.size();

gtsam/linear/GaussianFactorGraph.h

@@ -152,18 +152,15 @@ namespace gtsam {
      * Return vector of i, j, and s to generate an m-by-n sparse Jacobian matrix,
 		 * where i(k) and j(k) are the base 0 row and column indices, s(k) a double.
 		 * The standard deviations are baked into A and b
-		 * @param columnIndices First column index for each variable.
 		 */
-		std::vector<boost::tuple<size_t, size_t, double> > sparseJacobian(
+		std::vector<boost::tuple<size_t, size_t, double> > sparseJacobian() const;
-				const std::vector<size_t>& columnIndices) const;
 
 		/**
 		 * Matrix version of sparseJacobian: generates a 3*m matrix with [i,j,s] entries
 		 * such that S(i(k),j(k)) = s(k), which can be given to MATLAB's sparse.
 		 * The standard deviations are baked into A and b
-		 * @param columnIndices First column index for each variable, base 1, in vector format.
 		 */
-		Matrix sparse(const Vector& columnIndices) const;
+		Matrix sparse() const;
 
     /**
      * Return a dense \f$ m \times n \f$ Jacobian matrix, augmented with b

gtsam/linear/tests/testGaussianFactorGraph.cpp

@@ -62,8 +62,7 @@ TEST(GaussianFactorGraph, initialization) {
           1.,   2.,  7.,  7.,     1.,   2.,   3.,   4.,  7.,  7.,    1.,  2., 5., 6., 7., 7.,    3.,  4., 5., 6.,  7., 7.,
           10., 10., -1., -1.,   -10., -10.,  10.,  10.,  2., -1.,   -5., -5., 5., 5., 0., 1.,   -5., -5., 5., 5., -1., 1.5
   );
-  Vector columnIndices = Vector_(4,1.0,3.0,5.0,7.0);
+  Matrix actualIJS = fg.sparse();
-  Matrix actualIJS = fg.sparse(columnIndices);
   EQUALITY(expectedIJS, actualIJS);
 }
 
@@ -171,7 +170,7 @@ TEST(GaussianFactorGraph, Combine2)
 }
 
 /* ************************************************************************* */
-TEST_UNSAFE(GaussianFactor, CombineAndEliminate)
+TEST(GaussianFactor, CombineAndEliminate)
 {
   Matrix A01 = Matrix_(3,3,
       1.0, 0.0, 0.0,
@@ -573,6 +572,44 @@ TEST(GaussianFactor, permuteWithInverse)
 #endif
 }
 
+/* ************************************************************************* */
+TEST(GaussianFactorGraph, sparseJacobian) {
+  // Create factor graph:
+  // x1 x2 x3 x4 x5  b
+  //  1  2  3  0  0  4
+  //  5  6  7  0  0  8
+  //  9 10  0 11 12 13
+  //  0  0  0 14 15 16
+
+  // Expected - NOTE that we transpose this!
+  Matrix expected = Matrix_(16,3,
+      1., 1., 2.,
+      1., 2., 4.,
+      1., 3., 6.,
+      2., 1.,10.,
+      2., 2.,12.,
+      2., 3.,14.,
+      1., 6., 8.,
+      2., 6.,16.,
+      3., 1.,18.,
+      3., 2.,20.,
+      3., 4.,22.,
+      3., 5.,24.,
+      4., 4.,28.,
+      4., 5.,30.,
+      3., 6.,26.,
+      4., 6.,32.).transpose();
+
+  GaussianFactorGraph gfg;
+  SharedDiagonal model = sharedSigma(2, 0.5);
+  gfg.add(0, Matrix_(2,3, 1., 2., 3., 5., 6., 7.), Vector_(2, 4., 8.), model);
+  gfg.add(0, Matrix_(2,3, 9.,10., 0., 0., 0., 0.), 1, Matrix_(2,2, 11., 12., 14., 15.), Vector_(2, 13.,16.), model);
+
+  Matrix actual = gfg.sparse();
+
+  EXPECT(assert_equal(expected, actual));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */

tests/testGaussianFactorGraph.cpp

@@ -957,7 +957,7 @@ TEST(GaussianFactorGraph, replace)
 //}
 
 /* ************************************************************************* */
-/**
+/*
  *   x2 x1 x3 b
  *    1  1    1       1  1  0  1
  *    1    1  1  ->      1  1  1
@@ -1011,7 +1011,6 @@ TEST(GaussianFactorGraph, createSmoother2)
   CHECK(assert_equal(*p_x1.back(),*p_x3.front())); // should be the same because of symmetry
 }
 
-
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */