Improve sparseJacobianEigen()

gtsam/linear/GaussianFactorGraph.cpp

@@ -100,33 +100,35 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  vector<boost::tuple<size_t, size_t, double> > GaussianFactorGraph::sparseJacobian() const {
+  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 sharedFactor& factor : *this) {
-      if (!static_cast<bool>(factor))
-        continue;
+    for (const auto& factor : *this) {
+      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;
-    for (const sharedFactor& factor : *this) {
+    SparseTriplets entries;
+    entries.reserve(60 * size());
+    nrows = 0;
+    for (const auto& factor : *this) {
       if (!static_cast<bool>(factor)) continue;
 
       // Convert to JacobianFactor if necessary
@@ -138,52 +140,61 @@ 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
+    return entries;
   }
 
   /* ************************************************************************* */
-  Matrix GaussianFactorGraph::sparseJacobian_() const {
+  SparseTriplets GaussianFactorGraph::sparseJacobian() const {
+    size_t nrows, ncols;
+    return sparseJacobian(Ordering(this->keys()), nrows, ncols);
+  }
 
+  /* ************************************************************************* */
+  Matrix GaussianFactorGraph::sparseJacobian_() const {
+    gttic_(GaussianFactorGraph_sparseJacobian_matrix);
     // call sparseJacobian
-    typedef boost::tuple<size_t, size_t, double> triplet;
-    vector<triplet> result = sparseJacobian();
+    auto result = sparseJacobian();
 
     // 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(std::get<0>(entry) + 1);
+      IJS(1, k) = double(std::get<1>(entry) + 1);
+      IJS(2, k) = std::get<2>(entry);
     }
     return IJS;
   }

gtsam/linear/GaussianFactorGraph.h

@@ -181,15 +181,25 @@ 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.
+     * 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 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] 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;

gtsam/linear/SparseEigen.h

@@ -30,89 +30,33 @@
 
 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, int> 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
-
-  // 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
-  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);
-  Ab.setFromTriplets(entries.begin(), entries.end());
+  gttic_(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.sparseJacobian(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[std::get<1>(entry)]++;
+  Ab.reserve(nnz);
+  // pass 2: insert the elements
+  for (const auto &entry : entries)
+    Ab.insert(std::get<0>(entry), std::get<1>(entry)) = std::get<2>(entry);
   return Ab;
 }
 
 SparseEigen sparseJacobianEigen(const GaussianFactorGraph &gfg) {
+  gttic_(SparseEigen_sparseJacobianEigen_defaultOrdering);
   return sparseJacobianEigen(gfg, Ordering(gfg.keys()));
 }
 

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;
 }
 
@@ -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);
 }
@@ -118,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)));
   }
 }