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 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
simple_graph::b1
Vector2 b1(2, -1)
gtsam::HessianFactor
A Gaussian factor using the canonical parameters (information form)
Definition: HessianFactor.h:100
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
TestHarness.h
simple_graph::b2
Vector2 b2(4, -5)
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
X
#define X
Definition: icosphere.cpp:20
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
n
int n
Definition: BiCGSTAB_simple.cpp:1
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
ceres::Matrix
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
Definition: gtsam/3rdparty/ceres/eigen.h:42
main
int main()
Definition: testScatter.cpp:61
gtsam::Scatter
Definition: Scatter.h:49
Symbol.h
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
simple_graph::A21
Matrix A21
Definition: testJacobianFactor.cpp:200
TestResult
Definition: TestResult.h:26
A10
static const double A10[]
Definition: expn.h:15
TEST
TEST(HessianFactor, CombineAndEliminate)
Definition: testScatter.cpp:28
gtsam
traits
Definition: SFMdata.h:40
std
Definition: BFloat16.h:88
A11
static const double A11[]
Definition: expn.h:16
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
Scatter.h
Maps global variable indices to slot indices.
gtsam::GaussianFactorGraph::add
void add(const GaussianFactor &factor)
Definition: GaussianFactorGraph.h:125


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:41:19