more generic sparseJacobianInPlace function
parent
7a6f632f4c
commit
e3dd22925a
|
@ -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
|
|
@ -102,75 +102,9 @@ namespace gtsam {
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
SparseMatrixBoostTriplets GaussianFactorGraph::sparseJacobian(
|
SparseMatrixBoostTriplets GaussianFactorGraph::sparseJacobian(
|
||||||
const Ordering& ordering, size_t& nrows, size_t& ncols) const {
|
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;
|
SparseMatrixBoostTriplets entries;
|
||||||
entries.reserve(60 * size());
|
entries.reserve(60 * size());
|
||||||
|
sparseJacobianInPlace(entries, ordering, nrows, ncols);
|
||||||
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
|
|
||||||
return entries;
|
return entries;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -180,10 +180,27 @@ namespace gtsam {
|
||||||
///@name Linear Algebra
|
///@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
|
* 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
|
* 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 ordering the column ordering
|
||||||
* @param[out] nrows The number of rows in the Jacobian
|
* @param[out] nrows The number of rows in the Jacobian
|
||||||
* @param[out] ncols The number of columns 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
|
} // \ namespace gtsam
|
||||||
|
|
||||||
|
#include <gtsam/linear/GaussianFactorGraph-impl.h>
|
||||||
|
|
Loading…
Reference in New Issue