Improve sparseJacobianEigen()

gtsam/linear/GaussianFactorGraph.cpp

@@ -100,32 +100,33 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  vector<boost::tuple<size_t, size_t, double> > GaussianFactorGraph::sparseJacobian() const {
+  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;
+      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;
+    nrows = 0;
     for (const sharedFactor& factor : *this) {
       if (!static_cast<bool>(factor)) continue;
 
@@ -138,56 +139,136 @@ namespace gtsam {
         if (hessian)
           jacobianFactor.reset(new JacobianFactor(*hessian));
         else
-          throw invalid_argument(
-              "GaussianFactorGraph contains a factor that is neither a JacobianFactor nor a HessianFactor.");
+          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) {
+           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++) {
+        for (size_t i = 0; i < (size_t)whitenedA.rows(); i++)
+          for (size_t j = 0; j < (size_t)whitenedA.cols(); j++) {
             double s = whitenedA(i, j);
             if (std::abs(s) > 1e-12)
-              entries.push_back(boost::make_tuple(row + i, column_start + j, s));
+              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
   }
 
   /* ************************************************************************* */
-  Matrix GaussianFactorGraph::sparseJacobian_() const {
+  using BoostTriplets = GaussianFactorGraph::SparseMatrixBoostTriplets;
+  BoostTriplets GaussianFactorGraph::sparseJacobian(
+      const Ordering& ordering, size_t& nrows, size_t& ncols) const {
+    BoostTriplets entries;
+    entries.reserve(60 * size());
+    sparseJacobianInPlace(entries, ordering, 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 {
+    gttic_(GaussianFactorGraph_sparseJacobian_matrix);
     // call sparseJacobian
-    typedef boost::tuple<size_t, size_t, double> triplet;
-    vector<triplet> result = sparseJacobian();
+    auto result = sparseJacobian(ordering, nrows, ncols);
 
     // translate to base 1 matrix
     size_t nzmax = result.size();
-    Matrix IJS(3,nzmax);
+    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>();
+      const auto& entry = result[k];
+      IJS(0, k) = double(entry.get<0>() + 1);
+      IJS(1, k) = double(entry.get<1>() + 1);
+      IJS(2, k) = entry.get<2>();
     }
     return IJS;
   }
 
+  /* ************************************************************************* */
+  Matrix GaussianFactorGraph::sparseJacobian_(
+      const Ordering& ordering) const {
+    size_t dummy1, dummy2;
+    return sparseJacobian_(ordering, dummy1, dummy2);
+  }
+
+  /* ************************************************************************* */
+  Matrix GaussianFactorGraph::sparseJacobian_(
+      size_t& nrows, size_t& ncols) const {
+    return sparseJacobian_(Ordering(this->keys()), nrows, ncols);
+  }
+
+  /* ************************************************************************* */
+  Matrix GaussianFactorGraph::sparseJacobian_() const {
+    size_t dummy1, dummy2;
+    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

@@ -180,20 +180,84 @@ 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 Jacobian matrix,
-     * where i(k) and j(k) are the base 0 row and column indices, s(k) a double.
+     * 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
+     * @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
+     * @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
@@ -368,14 +432,22 @@ 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>
     void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
       ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
     }
 
-  public:
+   public:
 
     /** \deprecated */
     VectorValues optimize(boost::none_t,