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