testSparseEigen.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
23 
26 
27 using namespace std;
28 using namespace gtsam;
29 
30 /* ************************************************************************* */
33  SharedDiagonal model = noiseModel::Isotropic::Sigma(2, 0.5);
34  const Key x123 = 0, x45 = 1;
35  gfg.add(x123, (Matrix(2, 3) << 1, 2, 3, 5, 6, 7).finished(),
36  Vector2(4, 8), model);
37  gfg.add(x123, (Matrix(2, 3) << 9, 10, 0, 0, 0, 0).finished(),
38  x45, (Matrix(2, 2) << 11, 12, 14, 15.).finished(),
39  Vector2(13, 16), model);
40 
41  // Sparse Matrix
42  auto sparseResult = sparseJacobianEigen(gfg);
43  EXPECT_LONGS_EQUAL(16, sparseResult.nonZeros());
44  EXPECT(assert_equal(4, sparseResult.rows()));
45  EXPECT(assert_equal(6, sparseResult.cols()));
46  EXPECT(assert_equal(gfg.augmentedJacobian(), Matrix(sparseResult)));
47 
48  // Call sparseJacobian with optional ordering...
49  const Ordering ordering{x45, x123};
50 
51  // Eigen Sparse with optional ordering
54 
55  // Check matrix dimensions when zero rows / cols
56  gfg.add(x123, Matrix23::Zero(), Vector2::Zero(), model); // zero row
57  gfg.add(2, Matrix21::Zero(), Vector2::Zero(), model); // zero col
58  sparseResult = sparseJacobianEigen(gfg);
59  EXPECT_LONGS_EQUAL(16, sparseResult.nonZeros());
60  EXPECT(assert_equal(8, sparseResult.rows()));
61  EXPECT(assert_equal(7, sparseResult.cols()));
62 }
63 
64 /* ************************************************************************* */
65 int main() {
66  TestResult tr;
67  return TestRegistry::runAllTests(tr);
68 }
69 /* ************************************************************************* */
Provides additional testing facilities for common data structures.
int main()
static int runAllTests(TestResult &result)
A versatible sparse matrix representation.
Definition: SparseMatrix.h:96
Utilities for creating Eigen sparse matrices (gtsam::SparseEigen)
noiseModel::Diagonal::shared_ptr model
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
Definition: BFloat16.h:88
static enum @1107 ordering
SparseEigen sparseJacobianEigen(const GaussianFactorGraph &gfg, const Ordering &ordering)
Constructs an Eigen-format SparseMatrix of a GaussianFactorGraph.
Definition: SparseEigen.h:38
void add(const GaussianFactor &factor)
#define EXPECT(condition)
Definition: Test.h:150
Linear Factor Graph where all factors are Gaussians.
Matrix augmentedJacobian(const Ordering &ordering) const
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:743
traits
Definition: chartTesting.h:28
Eigen::Vector2d Vector2
Definition: Vector.h:42
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
TEST(SparseEigen, sparseJacobianEigen)
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:39:41