testScatter.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 
18 #include <gtsam/linear/Scatter.h>
20 #include <gtsam/inference/Symbol.h>
22 
23 using namespace std;
24 using namespace gtsam;
26 
27 /* ************************************************************************* */
28 TEST(HessianFactor, CombineAndEliminate) {
29  static const size_t m = 3, n = 3;
30  Matrix A01 =
31  (Matrix(m, n) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0).finished();
32  Vector3 b0(1.5, 1.5, 1.5);
33  Vector3 s0(1.6, 1.6, 1.6);
34 
35  Matrix A10 =
36  (Matrix(m, n) << 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 2.0).finished();
37  Matrix A11 = (Matrix(m, n) << -2.0, 0.0, 0.0, 0.0, -2.0, 0.0, 0.0, 0.0, -2.0)
38  .finished();
39  Vector3 b1(2.5, 2.5, 2.5);
40  Vector3 s1(2.6, 2.6, 2.6);
41 
42  Matrix A21 =
43  (Matrix(m, n) << 3.0, 0.0, 0.0, 0.0, 3.0, 0.0, 0.0, 0.0, 3.0).finished();
44  Vector3 b2(3.5, 3.5, 3.5);
45  Vector3 s2(3.6, 3.6, 3.6);
46 
48  gfg.add(X(1), A01, b0, noiseModel::Diagonal::Sigmas(s0, true));
49  gfg.add(X(0), A10, X(1), A11, b1, noiseModel::Diagonal::Sigmas(s1, true));
50  gfg.add(X(1), A21, b2, noiseModel::Diagonal::Sigmas(s2, true));
51 
52  Scatter scatter(gfg);
53  EXPECT_LONGS_EQUAL(2, scatter.size());
54  EXPECT(assert_equal(X(0), scatter.at(0).key));
55  EXPECT(assert_equal(X(1), scatter.at(1).key));
56  EXPECT_LONGS_EQUAL(n, scatter.at(0).dimension);
57  EXPECT_LONGS_EQUAL(n, scatter.at(1).dimension);
58 }
59 
60 /* ************************************************************************* */
61 int main() {
62  TestResult tr;
63  return TestRegistry::runAllTests(tr);
64 }
65 /* ************************************************************************* */
Matrix3f m
static int runAllTests(TestResult &result)
Eigen::Vector3d Vector3
Definition: Vector.h:43
int n
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Vector2 b2(4, -5)
Definition: BFloat16.h:88
int main()
Definition: testScatter.cpp:61
void add(const GaussianFactor &factor)
#define EXPECT(condition)
Definition: Test.h:150
Linear Factor Graph where all factors are Gaussians.
Maps global variable indices to slot indices.
TEST(HessianFactor, CombineAndEliminate)
Definition: testScatter.cpp:28
traits
Definition: chartTesting.h:28
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
A Gaussian factor using the canonical parameters (information form)
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
#define X
Definition: icosphere.cpp:20
Vector2 b1(2, -1)


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