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

more generic sparseJacobianInPlace function

release/4.3a0
Gerry Chen 2021-01-19 20:25:57 -05:00
parent 7a6f632f4c
commit e3dd22925a
3 changed files with 121 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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 <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

68
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -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<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
SparseMatrixBoostTriplets entries;
entries.reserve(60 * size());
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 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;
}

21
gtsam/linear/GaussianFactorGraph.h
View File

@ -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 <typename T>
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<GaussianFactorGraph> : public Testable<GaussianFactorGraph> {
};
} // \ namespace gtsam
#include <gtsam/linear/GaussianFactorGraph-impl.h>