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

Merge pull request #677 from borglab/feature/sparseJacobian3 

additional `sparseJacobian` functions (new)
release/4.3a0
Gerry Chen 2021-01-19 13:59:05 -05:00 committed by GitHub
parent 654995e073 b76993b171
commit 8f13405f05
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 225 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

119
gtsam/linear/SparseEigen.h Normal file
View File

@ -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 <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <Eigen/Sparse>
namespace gtsam {
typedef Eigen::SparseMatrix<double> 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());
return Ab;
}
SparseEigen sparseJacobianEigen(const GaussianFactorGraph &gfg) {
return sparseJacobianEigen(gfg, Ordering(gfg.keys()));
}
} // namespace gtsam

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

@ -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<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;
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)));
}
}
/* ************************************************************************* */

72
gtsam/linear/tests/testSparseEigen.cpp Normal file
View File

@ -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);
}
/* ************************************************************************* */