diff --git a/gtsam/linear/SparseEigen.h b/gtsam/linear/SparseEigen.h new file mode 100644 index 0000000000..7963d3ef58 --- /dev/null +++ b/gtsam/linear/SparseEigen.h @@ -0,0 +1,119 @@ +/* ---------------------------------------------------------------------------- + + * 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 creating Eigen sparse matrices (gtsam::SparseEigen) + * + * @date Aug 2019 + * @author Mandy Xie + * @author Fan Jiang + * @author Gerry Chen + * @author Frank Dellaert + */ + +#pragma once + +#include +#include + +#include + +namespace gtsam { + +typedef Eigen::SparseMatrix 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 dims; + for (const boost::shared_ptr &factor : gfg) { + if (!static_cast(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 columnIndices; + for (const auto key : ordering) { + columnIndices[key] = currentColIndex; + currentColIndex += dims[key]; + } + + // Iterate over all factors, adding sparse scalar entries + std::vector> entries; + entries.reserve(60 * gfg.size()); + + size_t row = 0; + for (const boost::shared_ptr &factor : gfg) { + if (!static_cast(factor)) continue; + + // Convert to JacobianFactor if necessary + JacobianFactor::shared_ptr jacobianFactor( + boost::dynamic_pointer_cast(factor)); + if (!jacobianFactor) { + HessianFactor::shared_ptr hessian( + boost::dynamic_pointer_cast(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()); + return Ab; +} + +SparseEigen sparseJacobianEigen(const GaussianFactorGraph &gfg) { + return sparseJacobianEigen(gfg, Ordering(gfg.keys())); +} + +} // namespace gtsam diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index 45f652d056..8b9ce94a9b 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -36,9 +36,18 @@ using namespace boost::assign; using namespace std; using namespace gtsam; -// static SharedDiagonal -// sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1), sigma_02 = noiseModel::Isotropic::Sigma(2,0.2), -// constraintModel = noiseModel::Constrained::All(2); +typedef boost::tuple 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; + + cout << "not equal:" << endl; + cout << "\texpected: " + "(" << a.get<0>() << ", " << a.get<1>() << ") = " << a.get<2>() << endl; + cout << "\tactual: " + "(" << b.get<0>() << ", " << b.get<1>() << ") = " << b.get<2>() << endl; + return false; +} /* ************************************************************************* */ TEST(GaussianFactorGraph, initialization) { @@ -74,8 +83,8 @@ TEST(GaussianFactorGraph, sparseJacobian) { // 9 10 0 11 12 13 // 0 0 0 14 15 16 - // Expected - NOTE that we transpose this! - Matrix expectedT = (Matrix(16, 3) << + // Expected + Matrix expected = (Matrix(16, 3) << 1., 1., 2., 1., 2., 4., 1., 3., 6., @@ -93,17 +102,32 @@ TEST(GaussianFactorGraph, sparseJacobian) { 3., 6.,26., 4., 6.,32.).finished(); - Matrix expected = expectedT.transpose(); + // expected: in matlab format - NOTE the transpose!) + Matrix expectedMatlab = expected.transpose(); GaussianFactorGraph gfg; SharedDiagonal model = noiseModel::Isotropic::Sigma(2, 0.5); - gfg.add(0, (Matrix(2, 3) << 1., 2., 3., 5., 6., 7.).finished(), Vector2(4., 8.), model); - gfg.add(0, (Matrix(2, 3) << 9., 10., 0., 0., 0., 0.).finished(), 1, - (Matrix(2, 2) << 11., 12., 14., 15.).finished(), Vector2(13., 16.), model); + 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); Matrix actual = gfg.sparseJacobian_(); - EXPECT(assert_equal(expected, actual)); + EXPECT(assert_equal(expectedMatlab, actual)); + + // BoostTriplets + 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)), + boostActual.at(i))); + } } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testSparseEigen.cpp b/gtsam/linear/tests/testSparseEigen.cpp new file mode 100644 index 0000000000..225e1dab22 --- /dev/null +++ b/gtsam/linear/tests/testSparseEigen.cpp @@ -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 +#include + +#include +using boost::assign::list_of; + +#include +#include + +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); +} +/* ************************************************************************* */