diff --git a/gtsam/linear/GaussianFactorGraph-impl.h b/gtsam/linear/GaussianFactorGraph-impl.h new file mode 100644 index 000000000..e3e6125c6 --- /dev/null +++ b/gtsam/linear/GaussianFactorGraph-impl.h @@ -0,0 +1,100 @@ +/* ---------------------------------------------------------------------------- + + * 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 7790b5328..c4e4ed109 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -102,75 +102,9 @@ namespace gtsam { /* ************************************************************************* */ 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 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 SparseMatrixBoostTriplets entries; entries.reserve(60 * size()); - - 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 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 + sparseJacobianInPlace(entries, ordering, nrows, ncols); return entries; } diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 5eb47ca48..2f21d9619 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -180,10 +180,27 @@ namespace gtsam { ///@name Linear Algebra ///@{ + /** + * 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; + /** * 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 + * 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 @@ -435,3 +452,5 @@ struct traits : public Testable { }; } // \ namespace gtsam + +#include