replace sparseJacobian with "fast" version

gtsam/linear/GaussianFactorGraph.cpp

@@ -100,24 +100,15 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  /** Performs in-place population of a sparse jacobian.  Contains the
-   * common functionality amongst different sparseJacobian functions.
-   * @param graph the factor graph to get the Jacobian from
-   * @param entries a container of triplets that supports
-   * `emplace_back(size_t, size_t, double)
-   * @param ordering the variable ordering
-   * @param[out] nrows the nurmber of rows in the Jacobian
-   * @param[out] ncols the number of columns in the Jacobian
-   */
-  template <typename T>
-  void sparseJacobianInPlace(const GaussianFactorGraph& graph, T& entries,
-                             const Ordering& ordering, size_t& nrows,
-                             size_t& ncols) {
-    gttic_(GaussianFactorGraph_sparseJacobianInPlace);
+  using SparseTriplets = std::vector<std::tuple<int, int, double> >;
+  SparseTriplets 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;
-    for (const auto& factor : graph) {
+    for (const auto& factor : *this) {
       if (!static_cast<bool>(factor)) continue;
 
       for (auto it = factor->begin(); it != factor->end(); ++it) {
@@ -134,8 +125,10 @@ namespace gtsam {
     }
 
     // Iterate over all factors, adding sparse scalar entries
+    SparseTriplets entries;
+    entries.reserve(60 * size());
     nrows = 0;
-    for (const auto& factor : graph) {
+    for (const auto& factor : *this) {
       if (!static_cast<bool>(factor)) continue;
 
       // Convert to JacobianFactor if necessary
@@ -179,16 +172,13 @@ namespace gtsam {
     }
 
     ncols++;  // +1 for b-column
+    return entries;
   }
 
   /* ************************************************************************* */
-  using BoostTriplets = std::vector<boost::tuple<size_t, size_t, double> >;
-  BoostTriplets GaussianFactorGraph::sparseJacobian() const {
-    BoostTriplets entries;
-    entries.reserve(60 * size());
+  SparseTriplets GaussianFactorGraph::sparseJacobian() const {
     size_t nrows, ncols;
-    sparseJacobianInPlace(*this, entries, Ordering(this->keys()), nrows, ncols);
-    return entries;
+    return sparseJacobian(Ordering(this->keys()), nrows, ncols);
   }
 
   /* ************************************************************************* */
@@ -202,23 +192,13 @@ namespace gtsam {
     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>();
+      IJS(0, k) = double(std::get<0>(entry) + 1);
+      IJS(1, k) = double(std::get<1>(entry) + 1);
+      IJS(2, k) = std::get<2>(entry);
     }
     return IJS;
   }
 
-  /* ************************************************************************* */
-  using GtsamTriplets = std::vector<std::tuple<int, int, double> >;
-  GtsamTriplets GaussianFactorGraph::sparseJacobianFast(
-      const Ordering& ordering, size_t& nrows, size_t& ncols) const {
-    GtsamTriplets entries;
-    entries.reserve(60 * size());
-    sparseJacobianInPlace(*this, entries, ordering, nrows, ncols);
-    return entries;
-  }
-
   /* ************************************************************************* */
   Matrix GaussianFactorGraph::augmentedJacobian(
       const Ordering& ordering) const {

gtsam/linear/GaussianFactorGraph.h

@@ -181,13 +181,20 @@ namespace gtsam {
     ///@{
 
     /**
-     * Return vector of i, j, and s to generate an m-by-n sparse augmented
-     * Jacobian matrix, where i(k) and j(k) are the base 0 row and column
-     * indices, s(k) a double.
+     * Returns a sparse augmented Jacbian matrix as a vector of i, j, and s,
+     * where i(k) and j(k) are the base 0 row and column indices, and s(k) is
+     * the entry as a double.
      * The standard deviations are baked into A and b
-     * @return the sparse matrix as a std::vector of boost tuples
+     * @return the sparse matrix as a std::vector of std::tuples
+     * @param ordering the column ordering
+     * @param[out] nrows The number of rows in the augmented Jacobian
+     * @param[out] ncols The number of columns in the augmented Jacobian
      */
-    std::vector<boost::tuple<size_t, size_t, double> > sparseJacobian() const;
+    std::vector<std::tuple<int, int, double> > sparseJacobian(
+        const Ordering& ordering, size_t& nrows, size_t& ncols) const;
+
+    /** Returns a sparse augmented Jacobian matrix with default ordering */
+    std::vector<std::tuple<int, int, double> > sparseJacobian() const;
 
     /**
      * Matrix version of sparseJacobian: generates a 3*m matrix with [i,j,s]
@@ -197,15 +204,6 @@ namespace gtsam {
      */
     Matrix sparseJacobian_() const;
 
-    /** Returns a sparse matrix with `int` indices instead of `size_t` for
-     * slightly faster performance
-     * @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
-     */
-    std::vector<std::tuple<int, int, double> > sparseJacobianFast(
-        const Ordering& ordering, size_t& nrows, size_t& ncols) const;
-
     /**
      * Return a dense \f$ [ \;A\;b\; ] \in \mathbb{R}^{m \times n+1} \f$
      * Jacobian matrix, augmented with b with the noise models baked

gtsam/linear/SparseEigen.h

@@ -41,7 +41,7 @@ SparseEigen sparseJacobianEigen(
   // intermediate `entries` vector is kind of unavoidable due to how expensive
   // factor->rows() is, which prevents us from populating SparseEigen directly.
   size_t nrows, ncols;
-  auto entries = gfg.sparseJacobianFast(ordering, nrows, ncols);
+  auto entries = gfg.sparseJacobian(ordering, nrows, ncols);
   // declare sparse matrix
   SparseEigen Ab(nrows, ncols);
   // See Eigen::set_from_triplets.  This is about 5% faster.

gtsam/linear/tests/testGaussianFactorGraph.cpp

@@ -36,16 +36,16 @@ using namespace boost::assign;
 using namespace std;
 using namespace gtsam;
 
-typedef boost::tuple<size_t, size_t, double> BoostTriplet;
-bool triplet_equal(BoostTriplet a, BoostTriplet b) {
-  if (a.get<0>() == b.get<0>() && a.get<1>() == b.get<1>() &&
-      a.get<2>() == b.get<2>()) return true;
+typedef std::tuple<size_t, size_t, double> SparseTriplet;
+bool triplet_equal(SparseTriplet a, SparseTriplet b) {
+  if (get<0>(a) == get<0>(b) && get<1>(a) == get<1>(b) &&
+      get<2>(a) == get<2>(b)) return true;
 
   cout << "not equal:" << endl;
   cout << "\texpected: "
-      "(" << a.get<0>() << ", " << a.get<1>() << ") = " << a.get<2>() << endl;
+      "(" << get<0>(a) << ", " << get<1>(a) << ") = " << get<2>(a) << endl;
   cout << "\tactual:   "
-      "(" << b.get<0>() << ", " << b.get<1>() << ") = " << b.get<2>() << endl;
+      "(" << get<0>(b) << ", " << get<1>(b) << ") = " << get<2>(b) << endl;
   return false;
 }
 
@@ -119,14 +119,14 @@ TEST(GaussianFactorGraph, sparseJacobian) {
 
   EXPECT(assert_equal(expectedMatlab, actual));
 
-  // BoostTriplets
+  // SparseTriplets
   auto boostActual = gfg.sparseJacobian();
   // check the triplets size...
   EXPECT_LONGS_EQUAL(16, boostActual.size());
   // check content
   for (int i = 0; i < 16; i++) {
     EXPECT(triplet_equal(
-        BoostTriplet(expected(i, 0) - 1, expected(i, 1) - 1, expected(i, 2)),
+        SparseTriplet(expected(i, 0) - 1, expected(i, 1) - 1, expected(i, 2)),
         boostActual.at(i)));
   }
 }