organize/isolate sparseEigen functionality
Gerry Chen
parent
a477ec6811
commit
44c232a128
|
|
@ -0,0 +1,146 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 SparseEigen.h
|
||||
*
|
||||
* @brief Utilities for converting to Eigen's sparse matrix representations
|
||||
*
|
||||
* @date Aug 2019
|
||||
* @author Mandy Xie
|
||||
* @author Fan Jiang
|
||||
* @author Gerry Chen
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
|
||||
#include <Eigen/Sparse>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
typedef Eigen::SparseMatrix<double> SpMat;
|
||||
|
||||
SpMat sparseJacobianEigen(
|
||||
const GaussianFactorGraph &gfg, const Ordering &ordering) {
|
||||
// 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.
|
||||
SpMat Ab(row + 1, currentColIndex + 1);
|
||||
Ab.setFromTriplets(entries.begin(), entries.end());
|
||||
return Ab;
|
||||
}
|
||||
|
||||
SpMat sparseJacobianEigen(const GaussianFactorGraph &gfg) {
|
||||
return sparseJacobianEigen(gfg, Ordering(gfg.keys()));
|
||||
}
|
||||
|
||||
// /// obtain sparse matrix for eigen sparse solver
|
||||
// std::pair<SpMat, Eigen::VectorXd> obtainSparseMatrix(
|
||||
// const GaussianFactorGraph &gfg, const Ordering &ordering) {
|
||||
// gttic_(EigenOptimizer_obtainSparseMatrix);
|
||||
|
||||
// // Get sparse entries of Jacobian [A|b] augmented with RHS b.
|
||||
// auto entries = gfg.sparseJacobian(ordering);
|
||||
|
||||
// gttic_(EigenOptimizer_convertSparse);
|
||||
// // Convert boost tuples to Eigen triplets
|
||||
// vector<Eigen::Triplet<double>> triplets;
|
||||
// triplets.reserve(entries.size());
|
||||
// size_t rows = 0, cols = 0;
|
||||
// for (const auto &e : entries) {
|
||||
// size_t temp_rows = e.get<0>(), temp_cols = e.get<1>();
|
||||
// triplets.emplace_back(temp_rows, temp_cols, e.get<2>());
|
||||
// rows = std::max(rows, temp_rows);
|
||||
// cols = std::max(cols, temp_cols);
|
||||
// }
|
||||
|
||||
// // ...and make a sparse matrix with it.
|
||||
// SpMat Ab(rows + 1, cols + 1);
|
||||
// Ab.setFromTriplets(triplets.begin(), triplets.end());
|
||||
// Ab.makeCompressed();
|
||||
// gttoc_(EigenOptimizer_convertSparse);
|
||||
|
||||
// gttoc_(EigenOptimizer_obtainSparseMatrix);
|
||||
|
||||
// return make_pair<SpMat, Eigen::VectorXd>(Ab.block(0, 0, rows + 1, cols),
|
||||
// Ab.col(cols));
|
||||
// }
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -1,231 +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 SparseEigenSolver.cpp
|
||||
*
|
||||
* @brief Eigen SparseSolver based linear solver backend for GTSAM
|
||||
*
|
||||
* @date Aug 2019
|
||||
* @author Mandy Xie
|
||||
* @author Fan Jiang
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <gtsam/base/timing.h>
|
||||
#include <gtsam/linear/SparseEigenSolver.h>
|
||||
#include <vector>
|
||||
|
||||
#include <boost/shared_ptr.hpp>
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
using SpMat = Eigen::SparseMatrix<double, Eigen::ColMajor, Eigen::Index>;
|
||||
|
||||
Eigen::SparseMatrix<double>
|
||||
SparseEigenSolver::sparseJacobianEigen(
|
||||
const GaussianFactorGraph &gfg,
|
||||
const Ordering &ordering) {
|
||||
// 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
|
||||
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 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.
|
||||
Eigen::SparseMatrix<double, Eigen::ColMajor> Ab(row + 1, currentColIndex + 1);
|
||||
Ab.setFromTriplets(entries.begin(), entries.end());
|
||||
return Ab;
|
||||
}
|
||||
|
||||
|
||||
/// obtain sparse matrix for eigen sparse solver
|
||||
std::pair<SpMat, Eigen::VectorXd> obtainSparseMatrix(
|
||||
const GaussianFactorGraph &gfg,
|
||||
const Ordering &ordering) {
|
||||
|
||||
gttic_(EigenOptimizer_obtainSparseMatrix);
|
||||
|
||||
// Get sparse entries of Jacobian [A|b] augmented with RHS b.
|
||||
auto entries = gfg.sparseJacobian(ordering);
|
||||
|
||||
gttic_(EigenOptimizer_convertSparse);
|
||||
// Convert boost tuples to Eigen triplets
|
||||
vector<Eigen::Triplet<double>> triplets;
|
||||
triplets.reserve(entries.size());
|
||||
size_t rows = 0, cols = 0;
|
||||
for (const auto &e : entries) {
|
||||
size_t temp_rows = e.get<0>(), temp_cols = e.get<1>();
|
||||
triplets.emplace_back(temp_rows, temp_cols, e.get<2>());
|
||||
rows = std::max(rows, temp_rows);
|
||||
cols = std::max(cols, temp_cols);
|
||||
}
|
||||
|
||||
// ...and make a sparse matrix with it.
|
||||
SpMat Ab(rows + 1, cols + 1);
|
||||
Ab.setFromTriplets(triplets.begin(), triplets.end());
|
||||
Ab.makeCompressed();
|
||||
gttoc_(EigenOptimizer_convertSparse);
|
||||
|
||||
gttoc_(EigenOptimizer_obtainSparseMatrix);
|
||||
|
||||
return make_pair<SpMat, Eigen::VectorXd>(Ab.block(0, 0, rows + 1, cols),
|
||||
Ab.col(cols));
|
||||
}
|
||||
|
||||
bool SparseEigenSolver::isIterative() {
|
||||
return false;
|
||||
}
|
||||
|
||||
bool SparseEigenSolver::isSequential() {
|
||||
return false;
|
||||
}
|
||||
|
||||
VectorValues SparseEigenSolver::solve(const GaussianFactorGraph &gfg) {
|
||||
if (solverType == QR) {
|
||||
gttic_(EigenOptimizer_optimizeEigenQR);
|
||||
auto Ab_pair = obtainSparseMatrix(gfg, ordering);
|
||||
|
||||
// Solve A*x = b using sparse QR from Eigen
|
||||
gttic_(EigenOptimizer_optimizeEigenQR_create_solver);
|
||||
Eigen::SparseQR<SpMat, Eigen::NaturalOrdering<Eigen::Index>> solver(Ab_pair.first);
|
||||
gttoc_(EigenOptimizer_optimizeEigenQR_create_solver);
|
||||
|
||||
gttic_(EigenOptimizer_optimizeEigenQR_solve);
|
||||
Eigen::VectorXd x = solver.solve(Ab_pair.second);
|
||||
gttoc_(EigenOptimizer_optimizeEigenQR_solve);
|
||||
|
||||
return VectorValues(x, gfg.getKeyDimMap());
|
||||
} else if (solverType == CHOLESKY) {
|
||||
gttic_(EigenOptimizer_optimizeEigenCholesky);
|
||||
SpMat Ab = sparseJacobianEigen(gfg, ordering);
|
||||
auto rows = Ab.rows(), cols = Ab.cols();
|
||||
auto A = Ab.block(0, 0, rows, cols - 1);
|
||||
auto At = A.transpose();
|
||||
auto b = Ab.col(cols - 1);
|
||||
|
||||
SpMat AtA(A.cols(), A.cols());
|
||||
AtA.selfadjointView<Eigen::Upper>().rankUpdate(At);
|
||||
|
||||
gttic_(EigenOptimizer_optimizeEigenCholesky_create_solver);
|
||||
// Solve A*x = b using sparse Cholesky from Eigen
|
||||
Eigen::SimplicialLDLT<SpMat, Eigen::Upper, Eigen::NaturalOrdering<Eigen::Index>>
|
||||
solver(AtA);
|
||||
|
||||
gttoc_(EigenOptimizer_optimizeEigenCholesky_create_solver);
|
||||
|
||||
gttic_(EigenOptimizer_optimizeEigenCholesky_solve);
|
||||
Eigen::VectorXd x = solver.solve(At * b);
|
||||
gttoc_(EigenOptimizer_optimizeEigenCholesky_solve);
|
||||
|
||||
// NOTE: b is reordered now, so we need to transform back the order.
|
||||
// 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);
|
||||
}
|
||||
}
|
||||
|
||||
VectorValues vv;
|
||||
|
||||
std::map<Key, size_t> columnIndices;
|
||||
|
||||
{
|
||||
size_t currentColIndex = 0;
|
||||
for (const auto key : ordering) {
|
||||
columnIndices[key] = currentColIndex;
|
||||
currentColIndex += dims[key];
|
||||
}
|
||||
}
|
||||
|
||||
for (const pair<const Key, unsigned long> keyDim : dims) {
|
||||
vv.insert(keyDim.first, x.segment(columnIndices[keyDim.first], keyDim.second));
|
||||
}
|
||||
|
||||
return vv;
|
||||
}
|
||||
|
||||
throw std::exception();
|
||||
}
|
||||
|
||||
SparseEigenSolver::SparseEigenSolver(SparseEigenSolver::SparseEigenSolverType type, const Ordering &ordering) {
|
||||
solverType = type;
|
||||
this->ordering = ordering;
|
||||
}
|
||||
} // namespace gtsam
|
||||
|
|
@ -1,62 +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 SparseEigenSolver.h
|
||||
*
|
||||
* @brief Eigen SparseSolver based linear solver backend for GTSAM
|
||||
*
|
||||
* @date Aug 2019
|
||||
* @author Mandy Xie
|
||||
* @author Fan Jiang
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
#include <gtsam/linear/LinearSolver.h>
|
||||
#include <Eigen/Sparse>
|
||||
#include <string>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Eigen SparseSolver based Backend class
|
||||
*/
|
||||
class GTSAM_EXPORT SparseEigenSolver : public LinearSolver {
|
||||
public:
|
||||
|
||||
typedef enum {
|
||||
QR,
|
||||
CHOLESKY
|
||||
} SparseEigenSolverType;
|
||||
|
||||
|
||||
explicit SparseEigenSolver(SparseEigenSolver::SparseEigenSolverType type, const Ordering &ordering);
|
||||
|
||||
bool isIterative() override;
|
||||
|
||||
bool isSequential() override;
|
||||
|
||||
VectorValues solve(const GaussianFactorGraph &gfg) override;
|
||||
|
||||
static Eigen::SparseMatrix<double>
|
||||
sparseJacobianEigen(const GaussianFactorGraph &gfg, const Ordering &ordering);
|
||||
|
||||
protected:
|
||||
|
||||
SparseEigenSolverType solverType = QR;
|
||||
|
||||
Ordering ordering;
|
||||
};
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,72 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 testSparseMatrix.cpp
|
||||
* @author Mandy Xie
|
||||
* @author Fan Jiang
|
||||
* @author Gerry Chen
|
||||
* @author Frank Dellaert
|
||||
* @date Jan, 2021
|
||||
*/
|
||||
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/linear/SparseEigen.h>
|
||||
|
||||
#include <boost/assign/list_of.hpp>
|
||||
using boost::assign::list_of;
|
||||
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(SparseEigen, sparseJacobianEigen) {
|
||||
GaussianFactorGraph gfg;
|
||||
SharedDiagonal model = noiseModel::Isotropic::Sigma(2, 0.5);
|
||||
const Key x123 = 0, x45 = 1;
|
||||
gfg.add(x123, (Matrix(2, 3) << 1, 2, 3, 5, 6, 7).finished(),
|
||||
Vector2(4, 8), model);
|
||||
gfg.add(x123, (Matrix(2, 3) << 9, 10, 0, 0, 0, 0).finished(),
|
||||
x45, (Matrix(2, 2) << 11, 12, 14, 15.).finished(),
|
||||
Vector2(13, 16), model);
|
||||
|
||||
// Sparse Matrix
|
||||
auto sparseResult = sparseJacobianEigen(gfg);
|
||||
EXPECT_LONGS_EQUAL(16, sparseResult.nonZeros());
|
||||
EXPECT(assert_equal(4, sparseResult.rows()));
|
||||
EXPECT(assert_equal(6, sparseResult.cols()));
|
||||
EXPECT(assert_equal(gfg.augmentedJacobian(), Matrix(sparseResult)));
|
||||
|
||||
// Call sparseJacobian with optional ordering...
|
||||
auto ordering = Ordering(list_of(x45)(x123));
|
||||
|
||||
// Eigen Sparse with optional ordering
|
||||
EXPECT(assert_equal(gfg.augmentedJacobian(ordering),
|
||||
Matrix(sparseJacobianEigen(gfg, ordering))));
|
||||
|
||||
// Check matrix dimensions when zero rows / cols
|
||||
gfg.add(x123, Matrix23::Zero(), Vector2::Zero(), model); // zero row
|
||||
gfg.add(2, Matrix21::Zero(), Vector2::Zero(), model); // zero col
|
||||
sparseResult = sparseJacobianEigen(gfg);
|
||||
EXPECT_LONGS_EQUAL(16, sparseResult.nonZeros());
|
||||
EXPECT(assert_equal(8, sparseResult.rows()));
|
||||
EXPECT(assert_equal(7, sparseResult.cols()));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue