12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

remove templating while maintaining efficiency 

Templating still used in cpp file for generic-ness, but not exposed
anymore

std::tuple has the same performance as Eigen::Triplet, but boost::tuple
is slower.  Sparse matrix indices are int instead of size_t for
efficiency (e.g. A(i, j) = s  ->  i/j are int's instead of size_t's)
release/4.3a0
Gerry Chen 2021-01-20 17:01:15 -05:00
parent 286898a847
commit fd2d8a236a
4 changed files with 158 additions and 144 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

100
gtsam/linear/GaussianFactorGraph-impl.h
View File

@ -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 <gtsam/linear/GaussianFactorGraph.h> // for autocomplete/intellisense
namespace gtsam {
/* ************************************************************************* */
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;
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<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(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

105
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -99,8 +99,82 @@ namespace gtsam {
return result;
}
using BoostTriplets = GaussianFactorGraph::SparseMatrixBoostTriplets;
/* ************************************************************************* */
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;
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<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(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 {

84
gtsam/linear/GaussianFactorGraph.h
View File

@ -182,22 +182,8 @@ namespace gtsam {
typedef std::vector<boost::tuple<size_t, size_t, double>>
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 <typename T>
void sparseJacobianInPlace(T& entries, const Ordering& ordering,
size_t& nrows, size_t& ncols) const;
typedef std::vector<std::tuple<int, int, double>>
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 <typename T>
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<GaussianFactorGraph> : public Testable<GaussianFactorGraph> {
};
} // \ namespace gtsam
#include <gtsam/linear/GaussianFactorGraph-impl.h>

13
gtsam/linear/SparseEigen.h
View File

@ -32,7 +32,7 @@ namespace gtsam {
/// Eigen-format sparse matrix. Note: ColMajor is ~20% faster since
/// InnerIndices must be sorted
typedef Eigen::SparseMatrix<double, Eigen::ColMajor> SparseEigen;
typedef Eigen::SparseMatrix<double, Eigen::ColMajor, int> 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<Eigen::Triplet<double>> 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<int> 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;
}