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

@@ -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

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 {

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.
-
+    typedef std::vector<std::tuple<int, int, double>>
-    /**
+        SparseMatrixGtsamTriplets;  ///< Sparse matrix representation as vector of SparseTriplet's.
-     * 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
@@ -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
+    /** Returns a sparse augmented Jacobian without with default ordering and
-    /// outputting its dimensions
+     * 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,
+    Matrix sparseJacobian_(const Ordering& ordering, size_t& nrows,
-                                              size_t& nrows,
+                           size_t& ncols) const;
-                                              size_t& ncols) const;
 
-    /// Returns a matrix-form sparse augmented Jacobian without outputting its
+    /** Returns a matrix-form sparse augmented Jacobian without outputting its
-    /// dimensions
+     * dimensions
-    Matrix sparseJacobian_(
+     */
-        const Ordering& ordering) const;
+    Matrix sparseJacobian_(const Ordering& ordering) const;
 
-    /// Returns a matrix-form sparse augmented Jacobian with default Ordering
+    /** Returns a matrix-form sparse augmented Jacobian with default Ordering
-    Matrix sparseJacobian_(size_t& nrows,
+     * @param[out] nrows The number of rows in the Jacobian
-                                              size_t& ncols) const;
+     * @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
+    /** Returns a matrix-form sparse augmented Jacobian with default ordering
-    /// and without outputting its dimensions
+     * 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>

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()]++;
+  for (const auto &entry : entries) nnz[std::get<1>(entry)]++;
-  // pass 2: insert the elements
   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;
 }