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

Merge pull request #682 from borglab/feature/sparseJacobian3 

Speed up `sparseJacobianEigen()`
release/4.3a0
Gerry Chen 2021-01-28 03:02:20 -05:00 committed by GitHub
parent 364c437523 53261a5e16
commit 97723d1826
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 92 additions and 127 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

80
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -100,33 +100,35 @@ namespace gtsam {
}
/* ************************************************************************* */
vector<boost::tuple<size_t, size_t, double> > GaussianFactorGraph::sparseJacobian() const {
using SparseTriplets = std::vector<std::tuple<int, int, double> >;
SparseTriplets 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 (const auto& factor : *this) {
if (!static_cast<bool>(factor)) continue;
for (GaussianFactor::const_iterator key = factor->begin();
key != factor->end(); ++key) {
dims[*key] = factor->getDim(key);
for (auto it = factor->begin(); it != factor->end(); ++it) {
dims[*it] = factor->getDim(it);
}
}
// Compute first scalar column of each variable
size_t currentColIndex = 0;
ncols = 0;
KeySizeMap columnIndices = dims;
for (const KeySizeMap::value_type& col : dims) {
columnIndices[col.first] = currentColIndex;
currentColIndex += dims[col.first];
for (const auto key : ordering) {
columnIndices[key] = ncols;
ncols += dims[key];
}
// Iterate over all factors, adding sparse scalar entries
typedef boost::tuple<size_t, size_t, double> triplet;
vector<triplet> entries;
size_t row = 0;
for (const sharedFactor& factor : *this) {
SparseTriplets entries;
entries.reserve(30 * size());
nrows = 0;
for (const auto& factor : *this) {
if (!static_cast<bool>(factor)) continue;
// Convert to JacobianFactor if necessary
@ -138,52 +140,60 @@ namespace gtsam {
if (hessian)
jacobianFactor.reset(new JacobianFactor(*hessian));
else
throw invalid_argument(
"GaussianFactorGraph contains a factor that is neither a JacobianFactor nor a HessianFactor.");
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) {
for (auto 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++) {
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.push_back(boost::make_tuple(row + i, column_start + j, s));
entries.emplace_back(nrows + i, column_start + j, s);
}
}
JacobianFactor::constBVector whitenedb(whitened.getb());
size_t bcolumn = currentColIndex;
for (size_t i = 0; i < (size_t) whitenedb.size(); i++)
entries.push_back(boost::make_tuple(row + i, bcolumn, whitenedb(i)));
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
row += jacobianFactor->rows();
nrows += jacobianFactor->rows();
}
return vector<triplet>(entries.begin(), entries.end());
ncols++; // +1 for b-column
return entries;
}
/* ************************************************************************* */
SparseTriplets GaussianFactorGraph::sparseJacobian() const {
size_t nrows, ncols;
return sparseJacobian(Ordering(this->keys()), nrows, ncols);
}
/* ************************************************************************* */
Matrix GaussianFactorGraph::sparseJacobian_() const {
gttic_(GaussianFactorGraph_sparseJacobian_matrix);
// call sparseJacobian
typedef boost::tuple<size_t, size_t, double> triplet;
vector<triplet> result = sparseJacobian();
auto result = sparseJacobian();
// translate to base 1 matrix
size_t nzmax = result.size();
Matrix IJS(3,nzmax);
Matrix IJS(3, nzmax);
for (size_t k = 0; k < result.size(); k++) {
const triplet& entry = result[k];
IJS(0,k) = double(entry.get<0>() + 1);
IJS(1,k) = double(entry.get<1>() + 1);
IJS(2,k) = entry.get<2>();
const auto& entry = result[k];
IJS(0, k) = double(std::get<0>(entry) + 1);
IJS(1, k) = double(std::get<1>(entry) + 1);
IJS(2, k) = std::get<2>(entry);
}
return IJS;
}

20
gtsam/linear/GaussianFactorGraph.h
View File

@ -181,15 +181,25 @@ namespace gtsam {
///@{
/**
* Return vector of i, j, and s to generate an m-by-n sparse Jacobian matrix,
* where i(k) and j(k) are the base 0 row and column indices, s(k) a double.
* Returns a sparse augmented Jacbian matrix as a vector of i, j, and s,
* where i(k) and j(k) are the base 0 row and column indices, and s(k) is
* the entry as a double.
* The standard deviations are baked into A and b
* @return the sparse matrix as a std::vector of std::tuples
* @param ordering the column ordering
* @param[out] nrows The number of rows in the augmented Jacobian
* @param[out] ncols The number of columns in the augmented Jacobian
*/
std::vector<boost::tuple<size_t, size_t, double> > sparseJacobian() const;
std::vector<std::tuple<int, int, double> > sparseJacobian(
const Ordering& ordering, size_t& nrows, size_t& ncols) const;
/** Returns a sparse augmented Jacobian matrix with default ordering */
std::vector<std::tuple<int, int, double> > sparseJacobian() const;
/**
* Matrix version of sparseJacobian: generates a 3*m matrix with [i,j,s] entries
* such that S(i(k),j(k)) = s(k), which can be given to MATLAB's sparse.
* Matrix version of sparseJacobian: generates a 3*m matrix with [i,j,s]
* entries such that S(i(k),j(k)) = s(k), which can be given to MATLAB's
* sparse. Note: i, j are 1-indexed.
* The standard deviations are baked into A and b
*/
Matrix sparseJacobian_() const;

94
gtsam/linear/SparseEigen.h
View File

@ -30,89 +30,33 @@
namespace gtsam {
typedef Eigen::SparseMatrix<double> SparseEigen;
/// Eigen-format sparse matrix. Note: ColMajor is ~20% faster since
/// InnerIndices must be sorted
typedef Eigen::SparseMatrix<double, Eigen::ColMajor, int> SparseEigen;
/// Constructs an Eigen-format SparseMatrix of a GaussianFactorGraph
SparseEigen sparseJacobianEigen(
const GaussianFactorGraph &gfg, const Ordering &ordering) {
// TODO(gerry): eliminate copy/pasta by making GaussianFactorGraph version
// more general, or by creating an Eigen::Triplet compatible wrapper for
// boost::tuple return type
// First find dimensions of each variable
std::map<Key, size_t> dims;
for (const boost::shared_ptr<GaussianFactor> &factor : gfg) {
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
size_t currentColIndex = 0;
std::map<Key, size_t> columnIndices;
for (const auto key : ordering) {
columnIndices[key] = currentColIndex;
currentColIndex += dims[key];
}
// Iterate over all factors, adding sparse scalar entries
std::vector<Eigen::Triplet<double>> entries;
entries.reserve(60 * gfg.size());
size_t row = 0;
for (const boost::shared_ptr<GaussianFactor> &factor : gfg) {
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(row + i, column_start + j, s);
}
}
JacobianFactor::constBVector whitenedb(whitened.getb());
size_t bcolumn = currentColIndex;
for (size_t i = 0; i < (size_t)whitenedb.size(); i++) {
double s = whitenedb(i);
if (std::abs(s) > 1e-12) entries.emplace_back(row + i, bcolumn, s);
}
// Increment row index
row += jacobianFactor->rows();
}
// ...and make a sparse matrix with it.
SparseEigen Ab(row, currentColIndex + 1);
Ab.setFromTriplets(entries.begin(), entries.end());
gttic_(SparseEigen_sparseJacobianEigen);
// intermediate `entries` vector is kind of unavoidable due to how expensive
// factor->rows() is, which prevents us from populating SparseEigen directly.
size_t nrows, ncols;
auto entries = gfg.sparseJacobian(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[std::get<1>(entry)]++;
Ab.reserve(nnz);
// pass 2: insert the elements
for (const auto &entry : entries)
Ab.insert(std::get<0>(entry), std::get<1>(entry)) = std::get<2>(entry);
return Ab;
}
SparseEigen sparseJacobianEigen(const GaussianFactorGraph &gfg) {
gttic_(SparseEigen_sparseJacobianEigen_defaultOrdering);
return sparseJacobianEigen(gfg, Ordering(gfg.keys()));
}

25
gtsam/linear/tests/testGaussianFactorGraph.cpp
View File

@ -36,16 +36,16 @@ using namespace boost::assign;
using namespace std;
using namespace gtsam;
typedef boost::tuple<size_t, size_t, double> BoostTriplet;
bool triplet_equal(BoostTriplet a, BoostTriplet b) {
if (a.get<0>() == b.get<0>() && a.get<1>() == b.get<1>() &&
a.get<2>() == b.get<2>()) return true;
typedef std::tuple<size_t, size_t, double> SparseTriplet;
bool triplet_equal(SparseTriplet a, SparseTriplet b) {
if (get<0>(a) == get<0>(b) && get<1>(a) == get<1>(b) &&
get<2>(a) == get<2>(b)) return true;
cout << "not equal:" << endl;
cout << "\texpected: "
"(" << a.get<0>() << ", " << a.get<1>() << ") = " << a.get<2>() << endl;
"(" << get<0>(a) << ", " << get<1>(a) << ") = " << get<2>(a) << endl;
cout << "\tactual: "
"(" << b.get<0>() << ", " << b.get<1>() << ") = " << b.get<2>() << endl;
"(" << get<0>(b) << ", " << get<1>(b) << ") = " << get<2>(b) << endl;
return false;
}
@ -66,10 +66,11 @@ TEST(GaussianFactorGraph, initialization) {
// Test sparse, which takes a vector and returns a matrix, used in MATLAB
// Note that this the augmented vector and the RHS is in column 7
Matrix expectedIJS =
(Matrix(3, 22) << 1., 2., 1., 2., 3., 4., 3., 4., 3., 4., 5., 6., 5., 6., 5., 6., 7., 8., 7.,
8., 7., 8., 1., 2., 7., 7., 1., 2., 3., 4., 7., 7., 1., 2., 5., 6., 7., 7., 3., 4., 5., 6.,
7., 7., 10., 10., -1., -1., -10., -10., 10., 10., 2., -1., -5., -5., 5., 5., 0., 1., -5.,
-5., 5., 5., -1., 1.5).finished();
(Matrix(3, 21) <<
1., 2., 1., 2., 3., 4., 3., 4., 3., 4., 5., 6., 5., 6., 6., 7., 8., 7., 8., 7., 8.,
1., 2., 7., 7., 1., 2., 3., 4., 7., 7., 1., 2., 5., 6., 7., 3., 4., 5., 6., 7., 7.,
10., 10., -1., -1., -10., -10., 10., 10., 2., -1., -5., -5., 5., 5.,
1., -5., -5., 5., 5., -1., 1.5).finished();
Matrix actualIJS = fg.sparseJacobian_();
EQUALITY(expectedIJS, actualIJS);
}
@ -118,14 +119,14 @@ TEST(GaussianFactorGraph, sparseJacobian) {
EXPECT(assert_equal(expectedMatlab, actual));
// BoostTriplets
// SparseTriplets
auto boostActual = gfg.sparseJacobian();
// check the triplets size...
EXPECT_LONGS_EQUAL(16, boostActual.size());
// check content
for (int i = 0; i < 16; i++) {
EXPECT(triplet_equal(
BoostTriplet(expected(i, 0) - 1, expected(i, 1) - 1, expected(i, 2)),
SparseTriplet(expected(i, 0) - 1, expected(i, 1) - 1, expected(i, 2)),
boostActual.at(i)));
}
}