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 // for autocomplete/intellisense - -namespace gtsam { - -/* ************************************************************************* */ -template -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 KeySizeMap; - KeySizeMap dims; - for (const sharedFactor& factor : *this) { - if (!static_cast(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(factor)) continue; - - // Convert to JacobianFactor if necessary - JacobianFactor::shared_ptr jacobianFactor( - boost::dynamic_pointer_cast(factor)); - if (!jacobianFactor) { - HessianFactor::shared_ptr hessian( - boost::dynamic_pointer_cast(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 + 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 KeySizeMap; + KeySizeMap dims; + for (const sharedFactor& factor : *this) { + if (!static_cast(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(factor)) continue; + + // Convert to JacobianFactor if necessary + JacobianFactor::shared_ptr jacobianFactor( + boost::dynamic_pointer_cast(factor)); + if (!jacobianFactor) { + HessianFactor::shared_ptr hessian( + boost::dynamic_pointer_cast(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> 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 - void sparseJacobianInPlace(T& entries, const Ordering& ordering, - size_t& nrows, size_t& ncols) const; + typedef std::vector> + 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 + 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 : public Testable { }; } // \ namespace gtsam - -#include 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 SparseEigen; +typedef Eigen::SparseMatrix 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> 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 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; }