additional sparseJacobian functions

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();

gtsam/base/SparseMatrix.cpp

@@ -0,0 +1,59 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    SparseMatrix.h
+ * @brief   Interface utilities with Eigen's SparseMatrix representation
+ * @author  Gerry Chen
+ */
+
+#include <gtsam/base/SparseMatrix.h>
+
+namespace gtsam {
+
+/** Returns the sparse jacobian of a factor graph as an Eigen::SparseMatrix
+ * The standard deviations are baked into A and b
+ */
+SparseMatrixEigen SparseMatrix::JacobianEigen(const GaussianFactorGraph& graph,
+                                              const Ordering& ordering,
+                                              size_t& nrows, size_t& ncols) {
+  gttic_(Jacobian);
+  gttic_(obtainSparseJacobian);
+  auto entries = JacobianBoostTriplets(graph, ordering, nrows, ncols);
+  gttoc_(obtainSparseJacobian);
+  gttic_(convertSparseJacobian);
+  SparseMatrixEigen Ab(nrows, ncols);
+  Ab.reserve(entries.size());
+  for (auto entry : entries) {
+    Ab.insert(entry.get<0>(), entry.get<1>()) = entry.get<2>();
+  }
+  Ab.makeCompressed();
+  // TODO(gerry): benchmark to see if setFromTriplets is faster
+  // Ab.setFromTriplets(entries.begin(), entries.end());
+  return Ab;
+}
+
+/* ******************************* Overloads ******************************* */
+SparseMatrixEigen SparseMatrix::JacobianEigen(const GaussianFactorGraph& graph,
+                                              const Ordering& ordering) {
+  size_t dummy1, dummy2;
+  return JacobianEigen(graph, ordering, dummy1, dummy2);
+}
+SparseMatrixEigen SparseMatrix::JacobianEigen(const GaussianFactorGraph& graph,
+                                              size_t& nrows, size_t& ncols) {
+  return JacobianEigen(graph, Ordering(graph.keys()), nrows, ncols);
+}
+SparseMatrixEigen SparseMatrix::JacobianEigen(
+    const GaussianFactorGraph& graph) {
+  return JacobianEigen(graph, Ordering(graph.keys()));
+}
+
+}  // namespace gtsam

gtsam/base/SparseMatrix.h

@@ -0,0 +1,68 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    SparseMatrix.h
+ * @brief   Interface utilities with Eigen's SparseMatrix representation
+ * @author  Gerry Chen
+ */
+
+#pragma once
+
+#include "gtsam/linear/GaussianFactorGraph.h"
+
+#include <Eigen/Sparse>
+
+namespace gtsam {
+
+/// Sparse matrix as Eigen::SparseMatrix
+//  note: Eigen only supports signed (e.g. not size_t) StorageIndex
+typedef Eigen::SparseMatrix<double, Eigen::ColMajor, int> SparseMatrixEigen;
+
+class GTSAM_EXPORT SparseMatrix {
+ public:
+  /** Returns the sparse augmented jacobian of a factor graph as a vector of
+   * boost tuples (row, col, entry).  Equivalent to graph.sparseJacobian().
+   * The standard deviations are baked into A and b
+   */
+  template <typename... Args>
+  static SparseMatrixBoostTriplets JacobianBoostTriplets(
+      const GaussianFactorGraph& graph, Args&&... args) {
+    return graph.sparseJacobian(std::forward<Args>(args)...);
+  }
+
+  /** Returns the sparse augmented jacobian of a factor graph as a matlab
+   * `sparse` -compatible 3xm matrix with each column representing an entry of
+   * the form [row; col; entry].  graph.sparseJacobian_().
+   * The standard deviations are baked into A and b
+   */
+  template <typename... Args>
+  static Matrix JacobianMatrix(const GaussianFactorGraph& graph,
+                               Args&&... args) {
+    return graph.sparseJacobian_(std::forward<Args>(args)...);
+  }
+
+  /** Returns the sparse augmented jacobian of a factor graph as an
+   * Eigen::SparseMatrix
+   * The standard deviations are baked into A and b
+   */
+  static SparseMatrixEigen JacobianEigen(const GaussianFactorGraph& graph,
+                                         const Ordering& ordering,
+                                         size_t& nrows, size_t& ncols);
+
+  // Overloads
+  static SparseMatrixEigen JacobianEigen(const GaussianFactorGraph& graph,
+                                         const Ordering& ordering);
+  static SparseMatrixEigen JacobianEigen(const GaussianFactorGraph& graph,
+                                         size_t& nrows, size_t& ncols);
+  static SparseMatrixEigen JacobianEigen(const GaussianFactorGraph& graph);
+};
+}  // namespace gtsam

gtsam/linear/GaussianFactorGraph.cpp

@@ -100,32 +100,33 @@ 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;
+    std::map<Key, size_t> dims;
     for (const sharedFactor& 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;
-    KeySizeMap columnIndices = dims;
-    for (const KeySizeMap::value_type& col : dims) {
-      columnIndices[col.first] = currentColIndex;
-      currentColIndex += dims[col.first];
+    ncols = 0;
+    std::map<Key, size_t> columnIndices;
+    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,40 +155,82 @@ 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(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)));
+      size_t bcolumn = ncols;
+      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, bcolumn, 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 {
+  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);
     // 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);
+  }
+
   /* ************************************************************************* */
   Matrix GaussianFactorGraph::augmentedJacobian(
       const Ordering& ordering) const {

gtsam/linear/GaussianFactorGraph.h

@@ -66,7 +66,7 @@ namespace gtsam {
     public FactorGraph<GaussianFactor>,
     public EliminateableFactorGraph<GaussianFactorGraph>
   {
-  public:
+   public:
 
     typedef GaussianFactorGraph This; ///< Typedef to this class
     typedef FactorGraph<GaussianFactor> Base; ///< Typedef to base factor graph type
@@ -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.
      * 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;
 
     /**
@@ -367,15 +401,15 @@ namespace gtsam {
 
     /// @}
 
-  private:
+   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);
     }
 
-  public:
+   public:
 
     /** \deprecated */
     VectorValues optimize(boost::none_t,
@@ -395,9 +429,8 @@ namespace gtsam {
   //GTSAM_EXPORT void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r);
   //GTSAM_EXPORT void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r);
 
-/// traits
-template<>
-struct traits<GaussianFactorGraph> : public Testable<GaussianFactorGraph> {
-};
+  /// traits
+  template<>
+  struct traits<GaussianFactorGraph> : public Testable<GaussianFactorGraph> {};
 
 } // \ namespace gtsam