From 77c65f81763e9cd6d95a24c2fa31af6e8ebce851 Mon Sep 17 00:00:00 2001
From: Gerry Chen <gerry.chen2015@gmail.com>
Date: Tue, 19 Jan 2021 14:03:14 -0500
Subject: [PATCH 01/16] timing sparse eigen

---
 gtsam/linear/SparseEigen.h | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/gtsam/linear/SparseEigen.h b/gtsam/linear/SparseEigen.h
index 7963d3ef5..9df74f9fc 100644
--- a/gtsam/linear/SparseEigen.h
+++ b/gtsam/linear/SparseEigen.h
@@ -38,6 +38,7 @@ SparseEigen sparseJacobianEigen(
   // TODO(gerry): eliminate copy/pasta by making GaussianFactorGraph version
   // more general, or by creating an Eigen::Triplet compatible wrapper for
   // boost::tuple return type
+  gttic_(SparseEigen_sparseJacobianEigen);
 
   // First find dimensions of each variable
   std::map<Key, size_t> dims;
@@ -113,6 +114,7 @@ SparseEigen sparseJacobianEigen(
 }
 
 SparseEigen sparseJacobianEigen(const GaussianFactorGraph &gfg) {
+  gttic_(SparseEigen_sparseJacobianEigen_defaultOrdering);
   return sparseJacobianEigen(gfg, Ordering(gfg.keys()));
 }
 

From ada83d92dc8c939cb44b917c31e690d130b7d9f0 Mon Sep 17 00:00:00 2001
From: Gerry Chen <gerry.chen2015@gmail.com>
Date: Tue, 19 Jan 2021 14:38:10 -0500
Subject: [PATCH 02/16] minor efficiency modifications

---
 gtsam/linear/GaussianFactorGraph.cpp | 2 +-
 gtsam/linear/SparseEigen.h           | 4 +++-
 2 files changed, 4 insertions(+), 2 deletions(-)

diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp
index 69890c32d..9a22d6111 100644
--- a/gtsam/linear/GaussianFactorGraph.cpp
+++ b/gtsam/linear/GaussianFactorGraph.cpp
@@ -154,7 +154,7 @@ 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.push_back(boost::make_tuple(row + i, column_start + j, s));
+              entries.emplace_back(row + i, column_start + j, s);
           }
       }
 
diff --git a/gtsam/linear/SparseEigen.h b/gtsam/linear/SparseEigen.h
index 9df74f9fc..eac4178aa 100644
--- a/gtsam/linear/SparseEigen.h
+++ b/gtsam/linear/SparseEigen.h
@@ -30,7 +30,9 @@
 
 namespace gtsam {
 
-typedef Eigen::SparseMatrix<double> SparseEigen;
+/// Eigen-format sparse matrix.  Note: ColMajor is ~20% faster since
+/// InnerIndices must be sorted
+typedef Eigen::SparseMatrix<double, Eigen::ColMajor> SparseEigen;
 
 /// Constructs an Eigen-format SparseMatrix of a GaussianFactorGraph
 SparseEigen sparseJacobianEigen(

From 7a6f632f4cbb898da3c79f1a1697d22b71d3ee13 Mon Sep 17 00:00:00 2001
From: Gerry Chen <gerry.chen2015@gmail.com>
Date: Tue, 19 Jan 2021 16:59:12 -0500
Subject: [PATCH 03/16] 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                           |   8 ++
 gtsam/linear/GaussianFactorGraph.cpp          | 107 ++++++++++++------
 gtsam/linear/GaussianFactorGraph.h            |  46 +++++++-
 .../linear/tests/testGaussianFactorGraph.cpp  |   9 +-
 4 files changed, 128 insertions(+), 42 deletions(-)

diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h
index a3a14c6c3..11058ef40 100644
--- a/gtsam/base/Matrix.h
+++ b/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();
diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp
index 9a22d6111..7790b5328 100644
--- a/gtsam/linear/GaussianFactorGraph.cpp
+++ b/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();
-          key != factor->end(); ++key) {
-        dims[*key] = factor->getDim(key);
+      for (auto it = factor->begin(); it != factor->end(); ++it) {
+        dims[*it] = factor->getDim(it);
       }
     }
 
     // Compute first scalar column of each variable
-    size_t currentColIndex = 0;
+    ncols = 0;
     KeySizeMap columnIndices = dims;
-    for (const KeySizeMap::value_type& col : dims) {
-      columnIndices[col.first] = currentColIndex;
-      currentColIndex += dims[col.first];
+    for (const auto key : ordering) {
+      columnIndices[key] = ncols;
+      ncols += dims[key];
     }
 
     // Iterate over all factors, adding sparse scalar entries
-    typedef boost::tuple<size_t, size_t, double> triplet;
-    vector<triplet> entries;
-    size_t row = 0;
+    SparseMatrixBoostTriplets entries;
+    entries.reserve(60 * size());
+
+    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++)
-        entries.push_back(boost::make_tuple(row + i, bcolumn, whitenedb(i)));
+      for (size_t i = 0; i < (size_t) whitenedb.size(); i++) {
+        double s = 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 {
-
-    // call sparseJacobian
-    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;
+    size_t dummy1, dummy2;
+    return sparseJacobian_(dummy1, dummy2);
   }
 
   /* ************************************************************************* */
diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h
index 2b9e8e675..5eb47ca48 100644
--- a/gtsam/linear/GaussianFactorGraph.h
+++ b/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,
-     * 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
+     * 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. 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
-     * such that S(i(k),j(k)) = s(k), which can be given to MATLAB's sparse.
+     * 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.  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;
 
     /**
diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp
index 8b9ce94a9..493e7667c 100644
--- a/gtsam/linear/tests/testGaussianFactorGraph.cpp
+++ b/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.,
-       8., 7., 8., 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).finished();
+      (Matrix(3, 21) <<
+      1., 2., 1., 2., 3., 4., 3., 4., 3., 4., 5., 6., 5., 6., 6., 7., 8., 7., 8., 7., 8.,
+      1., 2., 7., 7., 1., 2., 3., 4., 7., 7., 1., 2., 5., 6., 7., 3., 4., 5., 6., 7., 7.,
+      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);
 }

From e3dd22925a2507805aff8c21d4e6b56e4f94a1c9 Mon Sep 17 00:00:00 2001
From: Gerry Chen <gerry.chen2015@gmail.com>
Date: Tue, 19 Jan 2021 20:25:57 -0500
Subject: [PATCH 04/16] more generic sparseJacobianInPlace function

---
 gtsam/linear/GaussianFactorGraph-impl.h | 100 ++++++++++++++++++++++++
 gtsam/linear/GaussianFactorGraph.cpp    |  68 +---------------
 gtsam/linear/GaussianFactorGraph.h      |  21 ++++-
 3 files changed, 121 insertions(+), 68 deletions(-)
 create mode 100644 gtsam/linear/GaussianFactorGraph-impl.h

diff --git a/gtsam/linear/GaussianFactorGraph-impl.h b/gtsam/linear/GaussianFactorGraph-impl.h
new file mode 100644
index 000000000..e3e6125c6
--- /dev/null
+++ b/gtsam/linear/GaussianFactorGraph-impl.h
@@ -0,0 +1,100 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    GaussianFactorGraph.cpp
+ * @brief   Linear Factor Graph where all factors are Gaussians
+ * @author  Kai Ni
+ * @author  Christian Potthast
+ * @author  Richard Roberts
+ * @author  Gerry Chen
+ * @author  Frank Dellaert
+ */
+
+#include <gtsam/linear/GaussianFactorGraph.h>  // for autocomplete/intellisense
+
+namespace gtsam {
+
+/* ************************************************************************* */
+template <typename T>
+void GaussianFactorGraph::sparseJacobianInPlace(T& entries,
+                                                const Ordering& ordering,
+                                                size_t& nrows,
+                                                size_t& ncols) const {
+  gttic_(GaussianFactorGraph_sparseJacobianInPlace);
+  // First find dimensions of each variable
+  typedef std::map<Key, size_t> KeySizeMap;
+  KeySizeMap dims;
+  for (const sharedFactor& factor : *this) {
+    if (!static_cast<bool>(factor))
+      continue;
+
+    for (auto it = factor->begin(); it != factor->end(); ++it) {
+      dims[*it] = factor->getDim(it);
+    }
+  }
+
+  // Compute first scalar column of each variable
+  ncols = 0;
+  KeySizeMap columnIndices = dims;
+  for (const auto key : ordering) {
+    columnIndices[key] = ncols;
+    ncols += dims[key];
+  }
+
+  // Iterate over all factors, adding sparse scalar entries
+  nrows = 0;
+  for (const sharedFactor& factor : *this) {
+    if (!static_cast<bool>(factor)) continue;
+
+    // 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>(factor));
+      if (hessian)
+        jacobianFactor.reset(new JacobianFactor(*hessian));
+      else
+        throw std::invalid_argument(
+            "GaussianFactorGraph contains a factor that is neither a JacobianFactor nor a HessianFactor.");
+    }
+
+    // Whiten the factor and add entries for it
+    // iterate over all variables in the factor
+    const JacobianFactor whitened(jacobianFactor->whiten());
+    for (JacobianFactor::const_iterator key = whitened.begin();
+        key < whitened.end(); ++key) {
+      JacobianFactor::constABlock whitenedA = whitened.getA(key);
+      // find first column index for this key
+      size_t column_start = columnIndices[*key];
+      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.emplace_back(nrows + i, column_start + j, s);
+        }
+    }
+
+    JacobianFactor::constBVector whitenedb(whitened.getb());
+    for (size_t i = 0; i < (size_t) whitenedb.size(); i++) {
+      double s = whitenedb(i);
+      if (std::abs(s) > 1e-12) entries.emplace_back(nrows + i, ncols, s);
+    }
+
+    // Increment row index
+    nrows += jacobianFactor->rows();
+  }
+
+  ncols++;  // +1 for b-column
+}
+
+}  // namespace gtsam
diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp
index 7790b5328..c4e4ed109 100644
--- a/gtsam/linear/GaussianFactorGraph.cpp
+++ b/gtsam/linear/GaussianFactorGraph.cpp
@@ -102,75 +102,9 @@ namespace gtsam {
   /* ************************************************************************* */
   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;
-    for (const sharedFactor& factor : *this) {
-      if (!static_cast<bool>(factor))
-        continue;
-
-      for (auto it = factor->begin(); it != factor->end(); ++it) {
-        dims[*it] = factor->getDim(it);
-      }
-    }
-
-    // Compute first scalar column of each variable
-    ncols = 0;
-    KeySizeMap columnIndices = dims;
-    for (const auto key : ordering) {
-      columnIndices[key] = ncols;
-      ncols += dims[key];
-    }
-
-    // Iterate over all factors, adding sparse scalar entries
     SparseMatrixBoostTriplets entries;
     entries.reserve(60 * size());
-
-    nrows = 0;
-    for (const sharedFactor& factor : *this) {
-      if (!static_cast<bool>(factor)) continue;
-
-      // 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>(factor));
-        if (hessian)
-          jacobianFactor.reset(new JacobianFactor(*hessian));
-        else
-          throw invalid_argument(
-              "GaussianFactorGraph contains a factor that is neither a JacobianFactor nor a HessianFactor.");
-      }
-
-      // Whiten the factor and add entries for it
-      // iterate over all variables in the factor
-      const JacobianFactor whitened(jacobianFactor->whiten());
-      for (JacobianFactor::const_iterator key = whitened.begin();
-          key < whitened.end(); ++key) {
-        JacobianFactor::constABlock whitenedA = whitened.getA(key);
-        // find first column index for this key
-        size_t column_start = columnIndices[*key];
-        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.emplace_back(nrows + i, column_start + j, s);
-          }
-      }
-
-      JacobianFactor::constBVector whitenedb(whitened.getb());
-      for (size_t i = 0; i < (size_t) whitenedb.size(); i++) {
-        double s = whitenedb(i);
-        if (std::abs(s) > 1e-12) entries.emplace_back(nrows + i, ncols, s);
-      }
-
-      // Increment row index
-      nrows += jacobianFactor->rows();
-    }
-
-    ncols++;  // +1 for b-column
+    sparseJacobianInPlace(entries, ordering, nrows, ncols);
     return entries;
   }
 
diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h
index 5eb47ca48..2f21d9619 100644
--- a/gtsam/linear/GaussianFactorGraph.h
+++ b/gtsam/linear/GaussianFactorGraph.h
@@ -180,10 +180,27 @@ namespace gtsam {
     ///@name Linear Algebra
     ///@{
 
+    /**
+     * Populates a container of triplets: (i, j, 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.
+     * The standard deviations are baked into A and b
+     * @param entries a container of triplets (i, j, s) which supports
+     * `emplace_back(size_t, size_t, double)`
+     * @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
+     */
+    template <typename T>
+    void sparseJacobianInPlace(T& entries, const Ordering& ordering,
+                               size_t& nrows, size_t& ncols) const;
+
     /**
      * 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. 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
@@ -435,3 +452,5 @@ struct traits<GaussianFactorGraph> : public Testable<GaussianFactorGraph> {
 };
 
 } // \ namespace gtsam
+
+#include <gtsam/linear/GaussianFactorGraph-impl.h>

From 9584860da0e91fecfe04ce60c569233b5a6fbb98 Mon Sep 17 00:00:00 2001
From: Gerry Chen <gerry.chen2015@gmail.com>
Date: Tue, 19 Jan 2021 20:27:30 -0500
Subject: [PATCH 05/16] eliminate copy/pasta from SparseEigen with generic
 version of sparseJacobian

---
 gtsam/linear/SparseEigen.h | 77 +++-----------------------------------
 1 file changed, 6 insertions(+), 71 deletions(-)

diff --git a/gtsam/linear/SparseEigen.h b/gtsam/linear/SparseEigen.h
index eac4178aa..5773099d5 100644
--- a/gtsam/linear/SparseEigen.h
+++ b/gtsam/linear/SparseEigen.h
@@ -37,80 +37,15 @@ typedef Eigen::SparseMatrix<double, Eigen::ColMajor> SparseEigen;
 /// Constructs an Eigen-format SparseMatrix of a GaussianFactorGraph
 SparseEigen sparseJacobianEigen(
     const GaussianFactorGraph &gfg, const Ordering &ordering) {
-  // TODO(gerry): eliminate copy/pasta by making GaussianFactorGraph version
-  // more general, or by creating an Eigen::Triplet compatible wrapper for
-  // boost::tuple return type
   gttic_(SparseEigen_sparseJacobianEigen);
-
-  // First find dimensions of each variable
-  std::map<Key, size_t> dims;
-  for (const boost::shared_ptr<GaussianFactor> &factor : gfg) {
-    if (!static_cast<bool>(factor)) continue;
-
-    for (auto it = factor->begin(); it != factor->end(); ++it) {
-      dims[*it] = factor->getDim(it);
-    }
-  }
-
-  // Compute first scalar column of each variable
-  size_t currentColIndex = 0;
-  std::map<Key, size_t> columnIndices;
-  for (const auto key : ordering) {
-    columnIndices[key] = currentColIndex;
-    currentColIndex += dims[key];
-  }
-
-  // Iterate over all factors, adding sparse scalar entries
+  // intermediate `entries` vector is kind of unavoidable due to how expensive
+  // factor->rows() is, which prevents us from populating SparseEigen directly
   std::vector<Eigen::Triplet<double>> entries;
   entries.reserve(60 * gfg.size());
-
-  size_t row = 0;
-  for (const boost::shared_ptr<GaussianFactor> &factor : gfg) {
-    if (!static_cast<bool>(factor)) continue;
-
-    // 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>(factor));
-      if (hessian)
-        jacobianFactor.reset(new JacobianFactor(*hessian));
-      else
-        throw std::invalid_argument(
-            "GaussianFactorGraph contains a factor that is neither a "
-            "JacobianFactor nor a HessianFactor.");
-    }
-
-    // Whiten the factor and add entries for it
-    // iterate over all variables in the factor
-    const JacobianFactor whitened(jacobianFactor->whiten());
-    for (JacobianFactor::const_iterator key = whitened.begin();
-         key < whitened.end(); ++key) {
-      JacobianFactor::constABlock whitenedA = whitened.getA(key);
-      // find first column index for this key
-      size_t column_start = columnIndices[*key];
-      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.emplace_back(row + 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++) {
-      double s = whitenedb(i);
-      if (std::abs(s) > 1e-12) entries.emplace_back(row + i, bcolumn, s);
-    }
-
-    // Increment row index
-    row += jacobianFactor->rows();
-  }
-
-  // ...and make a sparse matrix with it.
-  SparseEigen Ab(row, currentColIndex + 1);
+  size_t nrows, ncols;
+  gfg.sparseJacobianInPlace(entries, ordering, nrows, ncols);
+  // create sparse matrix
+  SparseEigen Ab(nrows, ncols);
   Ab.setFromTriplets(entries.begin(), entries.end());
   return Ab;
 }

From b5951d033e8fe4ba80bccc653ab5c76d4acb8e6a Mon Sep 17 00:00:00 2001
From: Gerry Chen <gerry.chen2015@gmail.com>
Date: Tue, 19 Jan 2021 20:56:18 -0500
Subject: [PATCH 06/16] populate sparse matrix with `insert` rather than
 `setFromTriplets` About 5% speed improvement.

---
 gtsam/linear/SparseEigen.h | 14 +++++++++++---
 1 file changed, 11 insertions(+), 3 deletions(-)

diff --git a/gtsam/linear/SparseEigen.h b/gtsam/linear/SparseEigen.h
index 5773099d5..72e77a021 100644
--- a/gtsam/linear/SparseEigen.h
+++ b/gtsam/linear/SparseEigen.h
@@ -39,14 +39,22 @@ SparseEigen sparseJacobianEigen(
     const GaussianFactorGraph &gfg, const Ordering &ordering) {
   gttic_(SparseEigen_sparseJacobianEigen);
   // intermediate `entries` vector is kind of unavoidable due to how expensive
-  // factor->rows() is, which prevents us from populating SparseEigen directly
+  // factor->rows() is, which prevents us from populating SparseEigen directly.
+  // Triplet is about 11% faster than boost tuple.
   std::vector<Eigen::Triplet<double>> entries;
   entries.reserve(60 * gfg.size());
   size_t nrows, ncols;
   gfg.sparseJacobianInPlace(entries, ordering, nrows, ncols);
-  // create sparse matrix
+  // declare sparse matrix
   SparseEigen Ab(nrows, ncols);
-  Ab.setFromTriplets(entries.begin(), entries.end());
+  // See Eigen::set_from_triplets.  This is about 5% faster.
+  // pass 1: count the nnz per inner-vector
+  std::vector<int> nnz(ncols, 0);
+  for (const auto &entry : entries) nnz[entry.col()]++;
+  // pass 2: insert the elements
+  Ab.reserve(nnz);
+  for (const auto &entry : entries)
+    Ab.insert(entry.row(), entry.col()) = entry.value();
   return Ab;
 }
 

From 286898a8471584310c81ae020afae1b8c553f6eb Mon Sep 17 00:00:00 2001
From: Gerry Chen <gerry.chen2015@gmail.com>
Date: Wed, 20 Jan 2021 12:25:20 -0500
Subject: [PATCH 07/16] move SparseMatrixBoostTriplets typedef to gfg

---
 gtsam/base/Matrix.h                  |  6 ------
 gtsam/linear/GaussianFactorGraph.cpp | 11 ++++++-----
 gtsam/linear/GaussianFactorGraph.h   |  3 +++
 3 files changed, 9 insertions(+), 11 deletions(-)

diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h
index 11058ef40..b4ecf6f6a 100644
--- a/gtsam/base/Matrix.h
+++ b/gtsam/base/Matrix.h
@@ -75,12 +75,6 @@ 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();
diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp
index c4e4ed109..825c6c5d2 100644
--- a/gtsam/linear/GaussianFactorGraph.cpp
+++ b/gtsam/linear/GaussianFactorGraph.cpp
@@ -99,30 +99,31 @@ namespace gtsam {
     return result;
   }
 
+  using BoostTriplets = GaussianFactorGraph::SparseMatrixBoostTriplets;
   /* ************************************************************************* */
-  SparseMatrixBoostTriplets GaussianFactorGraph::sparseJacobian(
+  BoostTriplets GaussianFactorGraph::sparseJacobian(
       const Ordering& ordering, size_t& nrows, size_t& ncols) const {
-    SparseMatrixBoostTriplets entries;
+    BoostTriplets entries;
     entries.reserve(60 * size());
     sparseJacobianInPlace(entries, ordering, nrows, ncols);
     return entries;
   }
 
   /* ************************************************************************* */
-  SparseMatrixBoostTriplets GaussianFactorGraph::sparseJacobian(
+  BoostTriplets GaussianFactorGraph::sparseJacobian(
       const Ordering& ordering) const {
     size_t dummy1, dummy2;
     return sparseJacobian(ordering, dummy1, dummy2);
   }
 
   /* ************************************************************************* */
-  SparseMatrixBoostTriplets GaussianFactorGraph::sparseJacobian(
+  BoostTriplets GaussianFactorGraph::sparseJacobian(
       size_t& nrows, size_t& ncols) const {
     return sparseJacobian(Ordering(this->keys()), nrows, ncols);
   }
 
   /* ************************************************************************* */
-  SparseMatrixBoostTriplets GaussianFactorGraph::sparseJacobian() const {
+  BoostTriplets GaussianFactorGraph::sparseJacobian() const {
     size_t dummy1, dummy2;
     return sparseJacobian(dummy1, dummy2);
   }
diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h
index 2f21d9619..f2ca3bd60 100644
--- a/gtsam/linear/GaussianFactorGraph.h
+++ b/gtsam/linear/GaussianFactorGraph.h
@@ -180,6 +180,9 @@ namespace gtsam {
     ///@name Linear Algebra
     ///@{
 
+    typedef std::vector<boost::tuple<size_t, size_t, double>>
+        SparseMatrixBoostTriplets;  ///< Sparse matrix representation as vector of tuples.
+
     /**
      * Populates a container of triplets: (i, j, s) to generate an m-by-n sparse
      * augmented Jacobian matrix, where i(k) and j(k) are the base 0 row and

From fd2d8a236a1fadbf2c67df981ff1355f7891be13 Mon Sep 17 00:00:00 2001
From: Gerry Chen <gerry.chen2015@gmail.com>
Date: Wed, 20 Jan 2021 17:01:15 -0500
Subject: [PATCH 08/16] remove templating while maintaining efficiency
 Templating still used in cpp file for generic-ness, but not exposed anymore

std::tuple has the same performance as Eigen::Triplet, but boost::tuple
is slower.  Sparse matrix indices are int instead of size_t for
efficiency (e.g. A(i, j) = s  ->  i/j are int's instead of size_t's)
---
 gtsam/linear/GaussianFactorGraph-impl.h | 100 ----------------------
 gtsam/linear/GaussianFactorGraph.cpp    | 105 +++++++++++++++++++++++-
 gtsam/linear/GaussianFactorGraph.h      |  84 +++++++++++--------
 gtsam/linear/SparseEigen.h              |  13 ++-
 4 files changed, 158 insertions(+), 144 deletions(-)
 delete mode 100644 gtsam/linear/GaussianFactorGraph-impl.h

diff --git a/gtsam/linear/GaussianFactorGraph-impl.h b/gtsam/linear/GaussianFactorGraph-impl.h
deleted file mode 100644
index e3e6125c6..000000000
--- a/gtsam/linear/GaussianFactorGraph-impl.h
+++ /dev/null
@@ -1,100 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * GTSAM Copyright 2010, Georgia Tech Research Corporation,
- * Atlanta, Georgia 30332-0415
- * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
- * See LICENSE for the license information
-
- * -------------------------------------------------------------------------- */
-
-/**
- * @file    GaussianFactorGraph.cpp
- * @brief   Linear Factor Graph where all factors are Gaussians
- * @author  Kai Ni
- * @author  Christian Potthast
- * @author  Richard Roberts
- * @author  Gerry Chen
- * @author  Frank Dellaert
- */
-
-#include <gtsam/linear/GaussianFactorGraph.h>  // for autocomplete/intellisense
-
-namespace gtsam {
-
-/* ************************************************************************* */
-template <typename T>
-void GaussianFactorGraph::sparseJacobianInPlace(T& entries,
-                                                const Ordering& ordering,
-                                                size_t& nrows,
-                                                size_t& ncols) const {
-  gttic_(GaussianFactorGraph_sparseJacobianInPlace);
-  // First find dimensions of each variable
-  typedef std::map<Key, size_t> KeySizeMap;
-  KeySizeMap dims;
-  for (const sharedFactor& factor : *this) {
-    if (!static_cast<bool>(factor))
-      continue;
-
-    for (auto it = factor->begin(); it != factor->end(); ++it) {
-      dims[*it] = factor->getDim(it);
-    }
-  }
-
-  // Compute first scalar column of each variable
-  ncols = 0;
-  KeySizeMap columnIndices = dims;
-  for (const auto key : ordering) {
-    columnIndices[key] = ncols;
-    ncols += dims[key];
-  }
-
-  // Iterate over all factors, adding sparse scalar entries
-  nrows = 0;
-  for (const sharedFactor& factor : *this) {
-    if (!static_cast<bool>(factor)) continue;
-
-    // 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>(factor));
-      if (hessian)
-        jacobianFactor.reset(new JacobianFactor(*hessian));
-      else
-        throw std::invalid_argument(
-            "GaussianFactorGraph contains a factor that is neither a JacobianFactor nor a HessianFactor.");
-    }
-
-    // Whiten the factor and add entries for it
-    // iterate over all variables in the factor
-    const JacobianFactor whitened(jacobianFactor->whiten());
-    for (JacobianFactor::const_iterator key = whitened.begin();
-        key < whitened.end(); ++key) {
-      JacobianFactor::constABlock whitenedA = whitened.getA(key);
-      // find first column index for this key
-      size_t column_start = columnIndices[*key];
-      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.emplace_back(nrows + i, column_start + j, s);
-        }
-    }
-
-    JacobianFactor::constBVector whitenedb(whitened.getb());
-    for (size_t i = 0; i < (size_t) whitenedb.size(); i++) {
-      double s = whitenedb(i);
-      if (std::abs(s) > 1e-12) entries.emplace_back(nrows + i, ncols, s);
-    }
-
-    // Increment row index
-    nrows += jacobianFactor->rows();
-  }
-
-  ncols++;  // +1 for b-column
-}
-
-}  // namespace gtsam
diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp
index 825c6c5d2..b987ba24e 100644
--- a/gtsam/linear/GaussianFactorGraph.cpp
+++ b/gtsam/linear/GaussianFactorGraph.cpp
@@ -99,8 +99,82 @@ namespace gtsam {
     return result;
   }
 
-  using BoostTriplets = GaussianFactorGraph::SparseMatrixBoostTriplets;
   /* ************************************************************************* */
+  template <typename T>
+  void GaussianFactorGraph::sparseJacobianInPlace(T& entries,
+                                                  const Ordering& ordering,
+                                                  size_t& nrows,
+                                                  size_t& ncols) const {
+    gttic_(GaussianFactorGraph_sparseJacobianInPlace);
+    // First find dimensions of each variable
+    typedef std::map<Key, size_t> KeySizeMap;
+    KeySizeMap dims;
+    for (const sharedFactor& factor : *this) {
+      if (!static_cast<bool>(factor)) continue;
+
+      for (auto it = factor->begin(); it != factor->end(); ++it) {
+        dims[*it] = factor->getDim(it);
+      }
+    }
+
+    // Compute first scalar column of each variable
+    ncols = 0;
+    KeySizeMap columnIndices = dims;
+    for (const auto key : ordering) {
+      columnIndices[key] = ncols;
+      ncols += dims[key];
+    }
+
+    // Iterate over all factors, adding sparse scalar entries
+    nrows = 0;
+    for (const sharedFactor& factor : *this) {
+      if (!static_cast<bool>(factor)) continue;
+
+      // 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>(factor));
+        if (hessian)
+          jacobianFactor.reset(new JacobianFactor(*hessian));
+        else
+          throw std::invalid_argument(
+              "GaussianFactorGraph contains a factor that is neither a "
+              "JacobianFactor nor a HessianFactor.");
+      }
+
+      // Whiten the factor and add entries for it
+      // iterate over all variables in the factor
+      const JacobianFactor whitened(jacobianFactor->whiten());
+      for (JacobianFactor::const_iterator key = whitened.begin();
+           key < whitened.end(); ++key) {
+        JacobianFactor::constABlock whitenedA = whitened.getA(key);
+        // find first column index for this key
+        size_t column_start = columnIndices[*key];
+        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.emplace_back(nrows + i, column_start + j, s);
+          }
+      }
+
+      JacobianFactor::constBVector whitenedb(whitened.getb());
+      for (size_t i = 0; i < (size_t)whitenedb.size(); i++) {
+        double s = whitenedb(i);
+        if (std::abs(s) > 1e-12) entries.emplace_back(nrows + i, ncols, s);
+      }
+
+      // Increment row index
+      nrows += jacobianFactor->rows();
+    }
+
+    ncols++;  // +1 for b-column
+  }
+
+  /* ************************************************************************* */
+  using BoostTriplets = GaussianFactorGraph::SparseMatrixBoostTriplets;
   BoostTriplets GaussianFactorGraph::sparseJacobian(
       const Ordering& ordering, size_t& nrows, size_t& ncols) const {
     BoostTriplets entries;
@@ -166,6 +240,35 @@ namespace gtsam {
     return sparseJacobian_(dummy1, dummy2);
   }
 
+  /* ************************************************************************* */
+  using GtsamTriplets = GaussianFactorGraph::SparseMatrixGtsamTriplets;
+  GtsamTriplets GaussianFactorGraph::sparseJacobianFast(
+      const Ordering& ordering, size_t& nrows, size_t& ncols) const {
+    GtsamTriplets entries;
+    entries.reserve(60 * size());
+    sparseJacobianInPlace(entries, ordering, nrows, ncols);
+    return entries;
+  }
+
+  /* ************************************************************************* */
+  GtsamTriplets GaussianFactorGraph::sparseJacobianFast(
+      const Ordering& ordering) const {
+    size_t dummy1, dummy2;
+    return sparseJacobianFast(ordering, dummy1, dummy2);
+  }
+
+  /* ************************************************************************* */
+  GtsamTriplets GaussianFactorGraph::sparseJacobianFast(
+      size_t& nrows, size_t& ncols) const {
+    return sparseJacobianFast(Ordering(this->keys()), nrows, ncols);
+  }
+
+  /* ************************************************************************* */
+  GtsamTriplets GaussianFactorGraph::sparseJacobianFast() const {
+    size_t dummy1, dummy2;
+    return sparseJacobianFast(dummy1, dummy2);
+  }
+
   /* ************************************************************************* */
   Matrix GaussianFactorGraph::augmentedJacobian(
       const Ordering& ordering) const {
diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h
index f2ca3bd60..2f5bb582a 100644
--- a/gtsam/linear/GaussianFactorGraph.h
+++ b/gtsam/linear/GaussianFactorGraph.h
@@ -182,22 +182,8 @@ namespace gtsam {
 
     typedef std::vector<boost::tuple<size_t, size_t, double>>
         SparseMatrixBoostTriplets;  ///< Sparse matrix representation as vector of tuples.
-
-    /**
-     * Populates a container of triplets: (i, j, 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.
-     * The standard deviations are baked into A and b
-     * @param entries a container of triplets (i, j, s) which supports
-     * `emplace_back(size_t, size_t, double)`
-     * @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
-     */
-    template <typename T>
-    void sparseJacobianInPlace(T& entries, const Ordering& ordering,
-                               size_t& nrows, size_t& ncols) const;
+    typedef std::vector<std::tuple<int, int, double>>
+        SparseMatrixGtsamTriplets;  ///< Sparse matrix representation as vector of SparseTriplet's.
 
     /**
      * Return vector of i, j, and s to generate an m-by-n sparse augmented
@@ -213,16 +199,16 @@ namespace gtsam {
                                              size_t& nrows,
                                              size_t& ncols) const;
 
-    /// Returns a sparse augmented Jacobian without outputting its dimensions
+    /** Returns a sparse augmented Jacobian without outputting its dimensions */
     SparseMatrixBoostTriplets sparseJacobian(
         const Ordering& ordering) const;
 
-    /// Returns a sparse augmented Jacobian with default Ordering
+    /** 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
+    /** Returns a sparse augmented Jacobian without with default ordering and
+     * outputting its dimensions */
     SparseMatrixBoostTriplets sparseJacobian() const;
 
     /**
@@ -231,23 +217,45 @@ namespace gtsam {
      * 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;
+    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 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
+     * @param[out] nrows The number of rows in the Jacobian
+     * @param[out] ncols The number of columns in the Jacobian
+     */
+    Matrix sparseJacobian_(size_t& nrows, size_t& ncols) const;
 
-    /// Returns a matrix-form sparse augmented Jacobian with default ordering
-    /// and without outputting its dimensions
+    /** Returns a matrix-form sparse augmented Jacobian with default ordering
+     * and without outputting its dimensions */
     Matrix sparseJacobian_() const;
 
+    /** Returns a sparse matrix with `int` indices instead of `size_t` for
+     * slightly faster performance */
+    SparseMatrixGtsamTriplets sparseJacobianFast(const Ordering& ordering,
+                                                 size_t& nrows,
+                                                 size_t& ncols) const;
+
+    /** Returns an int-indexed sparse matrix  without outputting its dimensions
+     */
+    SparseMatrixGtsamTriplets sparseJacobianFast(const Ordering& ordering) const;
+
+    /** Returns an int-indexed sparse matrix with default ordering
+     * @param[out] nrows The number of rows in the Jacobian
+     * @param[out] ncols The number of columns in the Jacobian
+     */
+    SparseMatrixGtsamTriplets sparseJacobianFast(size_t& nrows,
+                                                 size_t& ncols) const;
+
+    /** Returns an int-indexed sparse matrix with default ordering and without
+     * outputting its dimensions */
+    SparseMatrixGtsamTriplets sparseJacobianFast() 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
@@ -429,7 +437,15 @@ namespace gtsam {
       ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
     }
 
-  public:
+    /** Performs in-place population of a sparse jacobian.  Contains the
+     * common functionality amongst different sparseJacobian functions.
+     * @param entries a container of triplets that supports
+     * `emplace_back(size_t, size_t, double)`*/
+    template <typename T>
+    void sparseJacobianInPlace(T& entries, const Ordering& ordering,
+                               size_t& nrows, size_t& ncols) const;
+
+   public:
 
     /** \deprecated */
     VectorValues optimize(boost::none_t,
@@ -455,5 +471,3 @@ struct traits<GaussianFactorGraph> : public Testable<GaussianFactorGraph> {
 };
 
 } // \ namespace gtsam
-
-#include <gtsam/linear/GaussianFactorGraph-impl.h>
diff --git a/gtsam/linear/SparseEigen.h b/gtsam/linear/SparseEigen.h
index 72e77a021..bc99b5c89 100644
--- a/gtsam/linear/SparseEigen.h
+++ b/gtsam/linear/SparseEigen.h
@@ -32,7 +32,7 @@ namespace gtsam {
 
 /// Eigen-format sparse matrix.  Note: ColMajor is ~20% faster since
 /// InnerIndices must be sorted
-typedef Eigen::SparseMatrix<double, Eigen::ColMajor> SparseEigen;
+typedef Eigen::SparseMatrix<double, Eigen::ColMajor, int> SparseEigen;
 
 /// Constructs an Eigen-format SparseMatrix of a GaussianFactorGraph
 SparseEigen sparseJacobianEigen(
@@ -40,21 +40,18 @@ SparseEigen sparseJacobianEigen(
   gttic_(SparseEigen_sparseJacobianEigen);
   // intermediate `entries` vector is kind of unavoidable due to how expensive
   // factor->rows() is, which prevents us from populating SparseEigen directly.
-  // Triplet is about 11% faster than boost tuple.
-  std::vector<Eigen::Triplet<double>> entries;
-  entries.reserve(60 * gfg.size());
   size_t nrows, ncols;
-  gfg.sparseJacobianInPlace(entries, ordering, nrows, ncols);
+  auto entries = gfg.sparseJacobianFast(ordering, nrows, ncols);
   // declare sparse matrix
   SparseEigen Ab(nrows, ncols);
   // See Eigen::set_from_triplets.  This is about 5% faster.
   // pass 1: count the nnz per inner-vector
   std::vector<int> nnz(ncols, 0);
-  for (const auto &entry : entries) nnz[entry.col()]++;
-  // pass 2: insert the elements
+  for (const auto &entry : entries) nnz[std::get<1>(entry)]++;
   Ab.reserve(nnz);
+  // pass 2: insert the elements
   for (const auto &entry : entries)
-    Ab.insert(entry.row(), entry.col()) = entry.value();
+    Ab.insert(std::get<0>(entry), std::get<1>(entry)) = std::get<2>(entry);
   return Ab;
 }
 

From 570135c6f67ebd0652d34dfe2fef20b2c266ed2e Mon Sep 17 00:00:00 2001
From: Gerry Chen <gerry.chen2015@gmail.com>
Date: Wed, 20 Jan 2021 17:02:51 -0500
Subject: [PATCH 09/16] revert Matrix.h

---
 gtsam/base/Matrix.h | 2 --
 1 file changed, 2 deletions(-)

diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h
index b4ecf6f6a..a3a14c6c3 100644
--- a/gtsam/base/Matrix.h
+++ b/gtsam/base/Matrix.h
@@ -33,8 +33,6 @@
 #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

From b63895426680b7cf774fd1d90cc0bf383758b8f3 Mon Sep 17 00:00:00 2001
From: Gerry Chen <gerry.chen2015@gmail.com>
Date: Wed, 20 Jan 2021 17:08:18 -0500
Subject: [PATCH 10/16] formatting

---
 gtsam/linear/GaussianFactorGraph.h | 22 ++++++++++++----------
 1 file changed, 12 insertions(+), 10 deletions(-)

diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h
index 2f5bb582a..584eb02ce 100644
--- a/gtsam/linear/GaussianFactorGraph.h
+++ b/gtsam/linear/GaussianFactorGraph.h
@@ -180,10 +180,12 @@ namespace gtsam {
     ///@name Linear Algebra
     ///@{
 
+    /// Sparse matrix representation as vector of tuples.
     typedef std::vector<boost::tuple<size_t, size_t, double>>
-        SparseMatrixBoostTriplets;  ///< Sparse matrix representation as vector of tuples.
-    typedef std::vector<std::tuple<int, int, double>>
-        SparseMatrixGtsamTriplets;  ///< Sparse matrix representation as vector of SparseTriplet's.
+        SparseMatrixBoostTriplets;
+    /// Sparse matrix representation as vector of slightly more efficient
+    /// tuples.
+    typedef std::vector<std::tuple<int, int, double>> SparseMatrixGtsamTriplets;
 
     /**
      * Return vector of i, j, and s to generate an m-by-n sparse augmented
@@ -430,13 +432,6 @@ namespace gtsam {
     /// @}
 
   private:
-    /** Serialization function */
-    friend class boost::serialization::access;
-    template<class ARCHIVE>
-    void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-      ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
-    }
-
     /** Performs in-place population of a sparse jacobian.  Contains the
      * common functionality amongst different sparseJacobian functions.
      * @param entries a container of triplets that supports
@@ -445,6 +440,13 @@ namespace gtsam {
     void sparseJacobianInPlace(T& entries, const Ordering& ordering,
                                size_t& nrows, size_t& ncols) const;
 
+    /** Serialization function */
+    friend class boost::serialization::access;
+    template<class ARCHIVE>
+    void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+      ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
+    }
+
    public:
 
     /** \deprecated */

From dcf8a52b8b5ec68fdda2232e0cc333d37b9d3e39 Mon Sep 17 00:00:00 2001
From: Gerry Chen <gerry.chen2015@gmail.com>
Date: Wed, 27 Jan 2021 10:27:32 -0500
Subject: [PATCH 11/16] remove unnecessary function overloads and typedefs

---
 gtsam/linear/GaussianFactorGraph.cpp | 72 +++-------------------------
 gtsam/linear/GaussianFactorGraph.h   | 62 ++----------------------
 2 files changed, 12 insertions(+), 122 deletions(-)

diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp
index b987ba24e..c8446e6a7 100644
--- a/gtsam/linear/GaussianFactorGraph.cpp
+++ b/gtsam/linear/GaussianFactorGraph.cpp
@@ -174,40 +174,20 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  using BoostTriplets = GaussianFactorGraph::SparseMatrixBoostTriplets;
-  BoostTriplets GaussianFactorGraph::sparseJacobian(
-      const Ordering& ordering, size_t& nrows, size_t& ncols) const {
+  using BoostTriplets = std::vector<boost::tuple<size_t, size_t, double>>;
+  BoostTriplets GaussianFactorGraph::sparseJacobian() const {
     BoostTriplets entries;
     entries.reserve(60 * size());
-    sparseJacobianInPlace(entries, ordering, nrows, ncols);
+    size_t nrows, ncols;
+    sparseJacobianInPlace(entries, Ordering(this->keys()), nrows, ncols);
     return entries;
   }
 
   /* ************************************************************************* */
-  BoostTriplets GaussianFactorGraph::sparseJacobian(
-      const Ordering& ordering) const {
-    size_t dummy1, dummy2;
-    return sparseJacobian(ordering, dummy1, dummy2);
-  }
-
-  /* ************************************************************************* */
-  BoostTriplets GaussianFactorGraph::sparseJacobian(
-      size_t& nrows, size_t& ncols) const {
-    return sparseJacobian(Ordering(this->keys()), nrows, ncols);
-  }
-
-  /* ************************************************************************* */
-  BoostTriplets GaussianFactorGraph::sparseJacobian() const {
-    size_t dummy1, dummy2;
-    return sparseJacobian(dummy1, dummy2);
-  }
-
-  /* ************************************************************************* */
-  Matrix GaussianFactorGraph::sparseJacobian_(
-      const Ordering& ordering, size_t& nrows, size_t& ncols) const {
+  Matrix GaussianFactorGraph::sparseJacobian_() const {
     gttic_(GaussianFactorGraph_sparseJacobian_matrix);
     // call sparseJacobian
-    auto result = sparseJacobian(ordering, nrows, ncols);
+    auto result = sparseJacobian();
 
     // translate to base 1 matrix
     size_t nzmax = result.size();
@@ -222,26 +202,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  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;
-    return sparseJacobian_(dummy1, dummy2);
-  }
-
-  /* ************************************************************************* */
-  using GtsamTriplets = GaussianFactorGraph::SparseMatrixGtsamTriplets;
+  using GtsamTriplets = std::vector<std::tuple<int, int, double>>;
   GtsamTriplets GaussianFactorGraph::sparseJacobianFast(
       const Ordering& ordering, size_t& nrows, size_t& ncols) const {
     GtsamTriplets entries;
@@ -250,25 +211,6 @@ namespace gtsam {
     return entries;
   }
 
-  /* ************************************************************************* */
-  GtsamTriplets GaussianFactorGraph::sparseJacobianFast(
-      const Ordering& ordering) const {
-    size_t dummy1, dummy2;
-    return sparseJacobianFast(ordering, dummy1, dummy2);
-  }
-
-  /* ************************************************************************* */
-  GtsamTriplets GaussianFactorGraph::sparseJacobianFast(
-      size_t& nrows, size_t& ncols) const {
-    return sparseJacobianFast(Ordering(this->keys()), nrows, ncols);
-  }
-
-  /* ************************************************************************* */
-  GtsamTriplets GaussianFactorGraph::sparseJacobianFast() const {
-    size_t dummy1, dummy2;
-    return sparseJacobianFast(dummy1, dummy2);
-  }
-
   /* ************************************************************************* */
   Matrix GaussianFactorGraph::augmentedJacobian(
       const Ordering& ordering) const {
diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h
index 584eb02ce..70327d9f4 100644
--- a/gtsam/linear/GaussianFactorGraph.h
+++ b/gtsam/linear/GaussianFactorGraph.h
@@ -180,38 +180,14 @@ namespace gtsam {
     ///@name Linear Algebra
     ///@{
 
-    /// Sparse matrix representation as vector of tuples.
-    typedef std::vector<boost::tuple<size_t, size_t, double>>
-        SparseMatrixBoostTriplets;
-    /// Sparse matrix representation as vector of slightly more efficient
-    /// tuples.
-    typedef std::vector<std::tuple<int, int, double>> SparseMatrixGtsamTriplets;
-
     /**
      * 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.
      * 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
      */
-    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;
+    std::vector<boost::tuple<size_t, size_t, double>> sparseJacobian() const;
 
     /**
      * Matrix version of sparseJacobian: generates a 3*m matrix with [i,j,s]
@@ -219,44 +195,16 @@ namespace gtsam {
      * 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
-     * @param[out] nrows The number of rows in the Jacobian
-     * @param[out] ncols The number of columns in the Jacobian
-     */
-    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;
 
     /** Returns a sparse matrix with `int` indices instead of `size_t` for
-     * slightly faster performance */
-    SparseMatrixGtsamTriplets sparseJacobianFast(const Ordering& ordering,
-                                                 size_t& nrows,
-                                                 size_t& ncols) const;
-
-    /** Returns an int-indexed sparse matrix  without outputting its dimensions
-     */
-    SparseMatrixGtsamTriplets sparseJacobianFast(const Ordering& ordering) const;
-
-    /** Returns an int-indexed sparse matrix with default ordering
+     * 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
      */
-    SparseMatrixGtsamTriplets sparseJacobianFast(size_t& nrows,
-                                                 size_t& ncols) const;
-
-    /** Returns an int-indexed sparse matrix with default ordering and without
-     * outputting its dimensions */
-    SparseMatrixGtsamTriplets sparseJacobianFast() const;
+    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$

From a17bd5c6d5d8bfe0c8e84db3d3bce6e3fc302cd9 Mon Sep 17 00:00:00 2001
From: Gerry Chen <gerry.chen2015@gmail.com>
Date: Wed, 27 Jan 2021 10:44:31 -0500
Subject: [PATCH 12/16] remove InPlace jacobian from .h file

---
 gtsam/linear/GaussianFactorGraph.cpp | 24 ++++++++++++++++--------
 gtsam/linear/GaussianFactorGraph.h   |  8 --------
 2 files changed, 16 insertions(+), 16 deletions(-)

diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp
index c8446e6a7..4c0576934 100644
--- a/gtsam/linear/GaussianFactorGraph.cpp
+++ b/gtsam/linear/GaussianFactorGraph.cpp
@@ -100,16 +100,24 @@ 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 GaussianFactorGraph::sparseJacobianInPlace(T& entries,
-                                                  const Ordering& ordering,
-                                                  size_t& nrows,
-                                                  size_t& ncols) const {
+  void sparseJacobianInPlace(const GaussianFactorGraph& graph, T& entries,
+                             const Ordering& ordering, size_t& nrows,
+                             size_t& ncols) {
     gttic_(GaussianFactorGraph_sparseJacobianInPlace);
     // First find dimensions of each variable
     typedef std::map<Key, size_t> KeySizeMap;
     KeySizeMap dims;
-    for (const sharedFactor& factor : *this) {
+    for (const auto& factor : graph) {
       if (!static_cast<bool>(factor)) continue;
 
       for (auto it = factor->begin(); it != factor->end(); ++it) {
@@ -127,7 +135,7 @@ namespace gtsam {
 
     // Iterate over all factors, adding sparse scalar entries
     nrows = 0;
-    for (const sharedFactor& factor : *this) {
+    for (const auto& factor : graph) {
       if (!static_cast<bool>(factor)) continue;
 
       // Convert to JacobianFactor if necessary
@@ -179,7 +187,7 @@ namespace gtsam {
     BoostTriplets entries;
     entries.reserve(60 * size());
     size_t nrows, ncols;
-    sparseJacobianInPlace(entries, Ordering(this->keys()), nrows, ncols);
+    sparseJacobianInPlace(*this, entries, Ordering(this->keys()), nrows, ncols);
     return entries;
   }
 
@@ -207,7 +215,7 @@ namespace gtsam {
       const Ordering& ordering, size_t& nrows, size_t& ncols) const {
     GtsamTriplets entries;
     entries.reserve(60 * size());
-    sparseJacobianInPlace(entries, ordering, nrows, ncols);
+    sparseJacobianInPlace(*this, entries, ordering, nrows, ncols);
     return entries;
   }
 
diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h
index 70327d9f4..de4f2318b 100644
--- a/gtsam/linear/GaussianFactorGraph.h
+++ b/gtsam/linear/GaussianFactorGraph.h
@@ -380,14 +380,6 @@ namespace gtsam {
     /// @}
 
   private:
-    /** Performs in-place population of a sparse jacobian.  Contains the
-     * common functionality amongst different sparseJacobian functions.
-     * @param entries a container of triplets that supports
-     * `emplace_back(size_t, size_t, double)`*/
-    template <typename T>
-    void sparseJacobianInPlace(T& entries, const Ordering& ordering,
-                               size_t& nrows, size_t& ncols) const;
-
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>

From fa6bdd4042094f6b4f8c6e033147d1b514072f5e Mon Sep 17 00:00:00 2001
From: Gerry Chen <gerry.chen2015@gmail.com>
Date: Wed, 27 Jan 2021 10:54:22 -0500
Subject: [PATCH 13/16] fix comment and remove whitespace diff

---
 gtsam/linear/GaussianFactorGraph.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h
index de4f2318b..b54799f52 100644
--- a/gtsam/linear/GaussianFactorGraph.h
+++ b/gtsam/linear/GaussianFactorGraph.h
@@ -185,7 +185,7 @@ namespace gtsam {
      * 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
-     * @return the sparse matrix in one of the 4 forms above
+     * @return the sparse matrix as a std::vector of boost tuples
      */
     std::vector<boost::tuple<size_t, size_t, double>> sparseJacobian() const;
 
@@ -387,7 +387,7 @@ namespace gtsam {
       ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
     }
 
-   public:
+  public:
 
     /** \deprecated */
     VectorValues optimize(boost::none_t,

From 8063b9ae958b939cb8c264c19b362d7db781c3fc Mon Sep 17 00:00:00 2001
From: Gerry Chen <gerry.chen2015@gmail.com>
Date: Wed, 27 Jan 2021 10:58:26 -0500
Subject: [PATCH 14/16] disambiguate double template >>

---
 gtsam/linear/GaussianFactorGraph.cpp | 4 ++--
 gtsam/linear/GaussianFactorGraph.h   | 4 ++--
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp
index 4c0576934..6a359e9d1 100644
--- a/gtsam/linear/GaussianFactorGraph.cpp
+++ b/gtsam/linear/GaussianFactorGraph.cpp
@@ -182,7 +182,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  using BoostTriplets = std::vector<boost::tuple<size_t, size_t, double>>;
+  using BoostTriplets = std::vector<boost::tuple<size_t, size_t, double> >;
   BoostTriplets GaussianFactorGraph::sparseJacobian() const {
     BoostTriplets entries;
     entries.reserve(60 * size());
@@ -210,7 +210,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  using GtsamTriplets = std::vector<std::tuple<int, int, double>>;
+  using GtsamTriplets = std::vector<std::tuple<int, int, double> >;
   GtsamTriplets GaussianFactorGraph::sparseJacobianFast(
       const Ordering& ordering, size_t& nrows, size_t& ncols) const {
     GtsamTriplets entries;
diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h
index b54799f52..661d0a4a8 100644
--- a/gtsam/linear/GaussianFactorGraph.h
+++ b/gtsam/linear/GaussianFactorGraph.h
@@ -187,7 +187,7 @@ namespace gtsam {
      * The standard deviations are baked into A and b
      * @return the sparse matrix as a std::vector of boost tuples
      */
-    std::vector<boost::tuple<size_t, size_t, double>> sparseJacobian() const;
+    std::vector<boost::tuple<size_t, size_t, double> > sparseJacobian() const;
 
     /**
      * Matrix version of sparseJacobian: generates a 3*m matrix with [i,j,s]
@@ -203,7 +203,7 @@ namespace gtsam {
      * @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(
+    std::vector<std::tuple<int, int, double> > sparseJacobianFast(
         const Ordering& ordering, size_t& nrows, size_t& ncols) const;
 
     /**

From 2590b3b98098970eb51ba4013b7fdff6491b20e7 Mon Sep 17 00:00:00 2001
From: Gerry Chen <gerry.chen2015@gmail.com>
Date: Wed, 27 Jan 2021 15:25:01 -0500
Subject: [PATCH 15/16] replace sparseJacobian with "fast" version

---
 gtsam/linear/GaussianFactorGraph.cpp          | 50 ++++++-------------
 gtsam/linear/GaussianFactorGraph.h            | 26 +++++-----
 gtsam/linear/SparseEigen.h                    |  2 +-
 .../linear/tests/testGaussianFactorGraph.cpp  | 16 +++---
 4 files changed, 36 insertions(+), 58 deletions(-)

diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp
index 6a359e9d1..947d4745c 100644
--- a/gtsam/linear/GaussianFactorGraph.cpp
+++ b/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 {
diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h
index 661d0a4a8..e3304d5e8 100644
--- a/gtsam/linear/GaussianFactorGraph.h
+++ b/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
diff --git a/gtsam/linear/SparseEigen.h b/gtsam/linear/SparseEigen.h
index bc99b5c89..483ae7f8d 100644
--- a/gtsam/linear/SparseEigen.h
+++ b/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.
diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp
index 493e7667c..bb07a36aa 100644
--- a/gtsam/linear/tests/testGaussianFactorGraph.cpp
+++ b/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)));
   }
 }

From 53261a5e167733d8e2f0089eb69c702748fa7fed Mon Sep 17 00:00:00 2001
From: Gerry Chen <gerry.chen2015@gmail.com>
Date: Wed, 27 Jan 2021 16:58:05 -0500
Subject: [PATCH 16/16] auto and reserve fewer

---
 gtsam/linear/GaussianFactorGraph.cpp | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp
index 947d4745c..13eaee7e3 100644
--- a/gtsam/linear/GaussianFactorGraph.cpp
+++ b/gtsam/linear/GaussianFactorGraph.cpp
@@ -126,7 +126,7 @@ namespace gtsam {
 
     // Iterate over all factors, adding sparse scalar entries
     SparseTriplets entries;
-    entries.reserve(60 * size());
+    entries.reserve(30 * size());
     nrows = 0;
     for (const auto& factor : *this) {
       if (!static_cast<bool>(factor)) continue;
@@ -148,8 +148,7 @@ namespace gtsam {
       // Whiten the factor and add entries for it
       // iterate over all variables in the factor
       const JacobianFactor whitened(jacobianFactor->whiten());
-      for (JacobianFactor::const_iterator key = whitened.begin();
-           key < whitened.end(); ++key) {
+      for (auto key = whitened.begin(); key < whitened.end(); ++key) {
         JacobianFactor::constABlock whitenedA = whitened.getA(key);
         // find first column index for this key
         size_t column_start = columnIndices[*key];