70 lines
2.4 KiB
C++
70 lines
2.4 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* 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 <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...
|
|
const Ordering ordering{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);
|
|
}
|
|
/* ************************************************************************* */
|