add generic optional parameters to sparseJacobian

Also, the unit test changed due to a 0 entry that was previously wrongly
included in the b-column of the sparse representation.

gtsam/base/Matrix.h

@@ -33,6 +33,8 @@
 #include <boost/tuple/tuple.hpp>
 #include <boost/math/special_functions/fpclassify.hpp>
 
+#include <vector>
+
 /**
  * Matrix is a typedef in the gtsam namespace
  * TODO: make a version to work with matlab wrapping
@@ -73,6 +75,12 @@ GTSAM_MAKE_MATRIX_DEFS(9);
 typedef Eigen::Block<Matrix> SubMatrix;
 typedef Eigen::Block<const Matrix> ConstSubMatrix;
 
+/// Sparse matrix representation as vector of tuples.
+/// See SparseMatrix.h for additional representations SparseMatrixEigenTriplets
+/// and SparseMatrixEigen
+typedef std::vector<boost::tuple<size_t, size_t, double>>
+    SparseMatrixBoostTriplets;
+
 // Matrix formatting arguments when printing.
 // Akin to Matlab style.
 const Eigen::IOFormat& matlabFormat();

gtsam/linear/GaussianFactorGraph.cpp

@@ -100,7 +100,9 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  vector<boost::tuple<size_t, size_t, double> > GaussianFactorGraph::sparseJacobian() const {
+  SparseMatrixBoostTriplets GaussianFactorGraph::sparseJacobian(
+      const Ordering& ordering, size_t& nrows, size_t& ncols) const {
+    gttic_(GaussianFactorGraph_sparseJacobian);
     // First find dimensions of each variable
     typedef std::map<Key, size_t> KeySizeMap;
     KeySizeMap dims;
@@ -108,24 +110,24 @@ namespace gtsam {
       if (!static_cast<bool>(factor))
         continue;
 
-      for (GaussianFactor::const_iterator key = factor->begin();
+      for (auto it = factor->begin(); it != factor->end(); ++it) {
-          key != factor->end(); ++key) {
+        dims[*it] = factor->getDim(it);
-        dims[*key] = factor->getDim(key);
       }
     }
 
     // Compute first scalar column of each variable
-    size_t currentColIndex = 0;
+    ncols = 0;
     KeySizeMap columnIndices = dims;
-    for (const KeySizeMap::value_type& col : dims) {
+    for (const auto key : ordering) {
-      columnIndices[col.first] = currentColIndex;
+      columnIndices[key] = ncols;
-      currentColIndex += dims[col.first];
+      ncols += dims[key];
     }
 
     // Iterate over all factors, adding sparse scalar entries
-    typedef boost::tuple<size_t, size_t, double> triplet;
+    SparseMatrixBoostTriplets entries;
-    vector<triplet> entries;
+    entries.reserve(60 * size());
-    size_t row = 0;
+
+    nrows = 0;
     for (const sharedFactor& factor : *this) {
       if (!static_cast<bool>(factor)) continue;
 
@@ -154,38 +156,79 @@ namespace gtsam {
           for (size_t j = 0; j < (size_t) whitenedA.cols(); j++) {
             double s = whitenedA(i, j);
             if (std::abs(s) > 1e-12)
-              entries.emplace_back(row + i, column_start + j, s);
+              entries.emplace_back(nrows + i, column_start + j, s);
           }
       }
 
       JacobianFactor::constBVector whitenedb(whitened.getb());
-      size_t bcolumn = currentColIndex;
+      for (size_t i = 0; i < (size_t) whitenedb.size(); i++) {
-      for (size_t i = 0; i < (size_t) whitenedb.size(); i++)
+        double s = whitenedb(i);
-        entries.push_back(boost::make_tuple(row + i, bcolumn, whitenedb(i)));
+        if (std::abs(s) > 1e-12) entries.emplace_back(nrows + i, ncols, s);
+      }
 
       // Increment row index
-      row += jacobianFactor->rows();
+      nrows += jacobianFactor->rows();
     }
-    return vector<triplet>(entries.begin(), entries.end());
+
+    ncols++;  // +1 for b-column
+    return entries;
+  }
+
+  /* ************************************************************************* */
+  SparseMatrixBoostTriplets GaussianFactorGraph::sparseJacobian(
+      const Ordering& ordering) const {
+    size_t dummy1, dummy2;
+    return sparseJacobian(ordering, dummy1, dummy2);
+  }
+
+  /* ************************************************************************* */
+  SparseMatrixBoostTriplets GaussianFactorGraph::sparseJacobian(
+      size_t& nrows, size_t& ncols) const {
+    return sparseJacobian(Ordering(this->keys()), nrows, ncols);
+  }
+
+  /* ************************************************************************* */
+  SparseMatrixBoostTriplets GaussianFactorGraph::sparseJacobian() const {
+    size_t dummy1, dummy2;
+    return sparseJacobian(dummy1, dummy2);
+  }
+
+  /* ************************************************************************* */
+  Matrix GaussianFactorGraph::sparseJacobian_(
+      const Ordering& ordering, size_t& nrows, size_t& ncols) const {
+    gttic_(GaussianFactorGraph_sparseJacobian_matrix);
+    // call sparseJacobian
+    auto result = sparseJacobian(ordering, nrows, ncols);
+
+    // translate to base 1 matrix
+    size_t nzmax = result.size();
+    Matrix IJS(3, nzmax);
+    for (size_t k = 0; k < result.size(); k++) {
+      const auto& entry = result[k];
+      IJS(0, k) = double(entry.get<0>() + 1);
+      IJS(1, k) = double(entry.get<1>() + 1);
+      IJS(2, k) = entry.get<2>();
+    }
+    return IJS;
+  }
+
+  /* ************************************************************************* */
+  Matrix GaussianFactorGraph::sparseJacobian_(
+      const Ordering& ordering) const {
+    size_t dummy1, dummy2;
+    return sparseJacobian_(ordering, dummy1, dummy2);
+  }
+
+  /* ************************************************************************* */
+  Matrix GaussianFactorGraph::sparseJacobian_(
+      size_t& nrows, size_t& ncols) const {
+    return sparseJacobian_(Ordering(this->keys()), nrows, ncols);
   }
 
   /* ************************************************************************* */
   Matrix GaussianFactorGraph::sparseJacobian_() const {
-
+    size_t dummy1, dummy2;
-    // call sparseJacobian
+    return sparseJacobian_(dummy1, dummy2);
-    typedef boost::tuple<size_t, size_t, double> triplet;
-    vector<triplet> result = sparseJacobian();
-
-    // translate to base 1 matrix
-    size_t nzmax = result.size();
-    Matrix IJS(3,nzmax);
-    for (size_t k = 0; k < result.size(); k++) {
-      const triplet& entry = result[k];
-      IJS(0,k) = double(entry.get<0>() + 1);
-      IJS(1,k) = double(entry.get<1>() + 1);
-      IJS(2,k) = entry.get<2>();
-    }
-    return IJS;
   }
 
   /* ************************************************************************* */

gtsam/linear/GaussianFactorGraph.h

@@ -181,17 +181,51 @@ namespace gtsam {
     ///@{
 
     /**
-     * Return vector of i, j, and s to generate an m-by-n sparse Jacobian matrix,
+     * Return vector of i, j, and s to generate an m-by-n sparse augmented
-     * where i(k) and j(k) are the base 0 row and column indices, s(k) a double.
+     * Jacobian matrix, where i(k) and j(k) are the base 0 row and column
-     * The standard deviations are baked into A and b
+     * indices, s(k) a double. The standard deviations are baked into A and b
+     * @param ordering the column ordering
+     * @param[out] nrows The number of rows in the Jacobian
+     * @param[out] ncols The number of columns in the Jacobian
+     * @return the sparse matrix in one of the 4 forms above
      */
-    std::vector<boost::tuple<size_t, size_t, double> > sparseJacobian() const;
+    SparseMatrixBoostTriplets sparseJacobian(const Ordering& ordering,
+                                             size_t& nrows,
+                                             size_t& ncols) const;
+
+    /// Returns a sparse augmented Jacobian without outputting its dimensions
+    SparseMatrixBoostTriplets sparseJacobian(
+        const Ordering& ordering) const;
+
+    /// Returns a sparse augmented Jacobian with default Ordering
+    SparseMatrixBoostTriplets sparseJacobian(size_t& nrows,
+                                             size_t& ncols) const;
+
+    /// Returns a sparse augmented Jacobian without with default ordering and
+    /// outputting its dimensions
+    SparseMatrixBoostTriplets sparseJacobian() const;
 
     /**
-     * Matrix version of sparseJacobian: generates a 3*m matrix with [i,j,s] entries
+     * Matrix version of sparseJacobian: generates a 3*m matrix with [i,j,s]
-     * such that S(i(k),j(k)) = s(k), which can be given to MATLAB's sparse.
+     * entries such that S(i(k),j(k)) = s(k), which can be given to MATLAB's
+     * sparse.  Note: i, j are 1-indexed.
      * The standard deviations are baked into A and b
      */
+    Matrix sparseJacobian_(const Ordering& ordering,
+                                              size_t& nrows,
+                                              size_t& ncols) const;
+
+    /// Returns a matrix-form sparse augmented Jacobian without outputting its
+    /// dimensions
+    Matrix sparseJacobian_(
+        const Ordering& ordering) const;
+
+    /// Returns a matrix-form sparse augmented Jacobian with default Ordering
+    Matrix sparseJacobian_(size_t& nrows,
+                                              size_t& ncols) const;
+
+    /// Returns a matrix-form sparse augmented Jacobian with default ordering
+    /// and without outputting its dimensions
     Matrix sparseJacobian_() const;
 
     /**

gtsam/linear/tests/testGaussianFactorGraph.cpp

@@ -66,10 +66,11 @@ TEST(GaussianFactorGraph, initialization) {
   // Test sparse, which takes a vector and returns a matrix, used in MATLAB
   // Note that this the augmented vector and the RHS is in column 7
   Matrix expectedIJS =
-      (Matrix(3, 22) << 1., 2., 1., 2., 3., 4., 3., 4., 3., 4., 5., 6., 5., 6., 5., 6., 7., 8., 7.,
+      (Matrix(3, 21) <<
-       8., 7., 8., 1., 2., 7., 7., 1., 2., 3., 4., 7., 7., 1., 2., 5., 6., 7., 7., 3., 4., 5., 6.,
+      1., 2., 1., 2., 3., 4., 3., 4., 3., 4., 5., 6., 5., 6., 6., 7., 8., 7., 8., 7., 8.,
-       7., 7., 10., 10., -1., -1., -10., -10., 10., 10., 2., -1., -5., -5., 5., 5., 0., 1., -5.,
+      1., 2., 7., 7., 1., 2., 3., 4., 7., 7., 1., 2., 5., 6., 7., 3., 4., 5., 6., 7., 7.,
-       -5., 5., 5., -1., 1.5).finished();
+      10., 10., -1., -1., -10., -10., 10., 10., 2., -1., -5., -5., 5., 5.,
+        1., -5., -5., 5., 5., -1., 1.5).finished();
   Matrix actualIJS = fg.sparseJacobian_();
   EQUALITY(expectedIJS, actualIJS);
 }