1/**
2 * @file testNonlinearClusterTree.cpp
3 * @author Frank Dellaert
4 * @date March, 2016
5 */
6
7#include <gtsam_unstable/nonlinear/NonlinearClusterTree.h>
8#include <gtsam/sam/BearingRangeFactor.h>
9#include <gtsam/slam/BetweenFactor.h>
10#include <gtsam/geometry/Pose2.h>
11#include <gtsam/geometry/Point2.h>
12
13#include <gtsam/inference/Symbol.h>
14
15#include <CppUnitLite/TestHarness.h>
16
17using namespace gtsam;
18
19static const Symbol x1('x', 1), x2('x', 2), x3('x', 3);
20static const Symbol l1('l', 1), l2('l', 2);
21
22/* ************************************************************************* */
23NonlinearFactorGraph planarSLAMGraph() {
24 NonlinearFactorGraph graph;
25
26 // Prior on pose x1 at the origin.
27 Pose2 prior(0.0, 0.0, 0.0);
28 auto priorNoise = noiseModel::Diagonal::Sigmas(sigmas: Vector3(0.3, 0.3, 0.1));
29 graph.addPrior(key: x1, prior, model: priorNoise);
30
31 // Two odometry factors
32 Pose2 odometry(2.0, 0.0, 0.0);
33 auto odometryNoise = noiseModel::Diagonal::Sigmas(sigmas: Vector3(0.2, 0.2, 0.1));
34 graph.emplace_shared<BetweenFactor<Pose2> >(args: x1, args: x2, args&: odometry, args&: odometryNoise);
35 graph.emplace_shared<BetweenFactor<Pose2> >(args: x2, args: x3, args&: odometry, args&: odometryNoise);
36
37 // Add Range-Bearing measurements to two different landmarks
38 auto measurementNoise = noiseModel::Diagonal::Sigmas(sigmas: Vector2(0.1, 0.2));
39 Rot2 bearing11 = Rot2::fromDegrees(theta: 45), bearing21 = Rot2::fromDegrees(theta: 90),
40 bearing32 = Rot2::fromDegrees(theta: 90);
41 double range11 = std::sqrt(x: 4.0 + 4.0), range21 = 2.0, range32 = 2.0;
42 graph.emplace_shared<BearingRangeFactor<Pose2, Point2> >(args: x1, args: l1, args&: bearing11, args&: range11, args&: measurementNoise);
43 graph.emplace_shared<BearingRangeFactor<Pose2, Point2> >(args: x2, args: l1, args&: bearing21, args&: range21, args&: measurementNoise);
44 graph.emplace_shared<BearingRangeFactor<Pose2, Point2> >(args: x3, args: l2, args&: bearing32, args&: range32, args&: measurementNoise);
45
46 return graph;
47}
48
49/* ************************************************************************* */
50// Create initial estimate
51Values planarSLAMValues() {
52 Values initial;
53 initial.insert(j: l1, val: Point2(1.8, 2.1));
54 initial.insert(j: l2, val: Point2(4.1, 1.8));
55 initial.insert(j: x1, val: Pose2(0.5, 0.0, 0.2));
56 initial.insert(j: x2, val: Pose2(2.3, 0.1, -0.2));
57 initial.insert(j: x3, val: Pose2(4.1, 0.1, 0.1));
58 return initial;
59}
60
61/* ************************************************************************* */
62TEST(NonlinearClusterTree, Clusters) {
63 NonlinearFactorGraph graph = planarSLAMGraph();
64 Values initial = planarSLAMValues();
65
66 // Build the clusters
67 // NOTE(frank): Order matters here as factors are removed!
68 VariableIndex variableIndex(graph);
69 typedef NonlinearClusterTree::NonlinearCluster Cluster;
70 auto marginalCluster = std::shared_ptr<Cluster>(new Cluster(variableIndex, {x1}, &graph));
71 auto landmarkCluster = std::shared_ptr<Cluster>(new Cluster(variableIndex, {l1, l2}, &graph));
72 auto rootCluster = std::shared_ptr<Cluster>(new Cluster(variableIndex, {x2, x3}, &graph));
73
74 EXPECT_LONGS_EQUAL(3, marginalCluster->nrFactors());
75 EXPECT_LONGS_EQUAL(2, landmarkCluster->nrFactors());
76 EXPECT_LONGS_EQUAL(1, rootCluster->nrFactors());
77
78 EXPECT_LONGS_EQUAL(1, marginalCluster->nrFrontals());
79 EXPECT_LONGS_EQUAL(2, landmarkCluster->nrFrontals());
80 EXPECT_LONGS_EQUAL(2, rootCluster->nrFrontals());
81
82 // Test linearize
83 auto gfg = marginalCluster->linearize(values: initial);
84 EXPECT_LONGS_EQUAL(3, gfg->size());
85
86 // Calculate expected result of only evaluating the marginalCluster
87 Ordering ordering;
88 ordering.push_back(x: x1);
89 const auto [bn, fg] = gfg->eliminatePartialSequential(ordering);
90 auto expectedFactor = fg->at<HessianFactor>(i: 0);
91 if (!expectedFactor)
92 throw std::runtime_error("Expected HessianFactor");
93
94 // Linearize and eliminate the marginalCluster
95 auto actual = marginalCluster->linearizeAndEliminate(values: initial);
96 const GaussianBayesNet& bayesNet = actual.first;
97 const HessianFactor& factor = *actual.second;
98 EXPECT(assert_equal(*bn->at(0), *bayesNet.at(0), 1e-6));
99 EXPECT(assert_equal(*expectedFactor, factor, 1e-6));
100}
101
102/* ************************************************************************* */
103static NonlinearClusterTree Construct() {
104 // Build the clusters
105 // NOTE(frank): Order matters here as factors are removed!
106 NonlinearFactorGraph graph = planarSLAMGraph();
107 VariableIndex variableIndex(graph);
108 typedef NonlinearClusterTree::NonlinearCluster Cluster;
109 auto marginalCluster = std::shared_ptr<Cluster>(new Cluster(variableIndex, {x1}, &graph));
110 auto landmarkCluster = std::shared_ptr<Cluster>(new Cluster(variableIndex, {l1, l2}, &graph));
111 auto rootCluster = std::shared_ptr<Cluster>(new Cluster(variableIndex, {x2, x3}, &graph));
112
113 // Build the tree
114 NonlinearClusterTree clusterTree;
115 clusterTree.addRoot(cluster: rootCluster);
116 rootCluster->addChild(cluster: landmarkCluster);
117 landmarkCluster->addChild(cluster: marginalCluster);
118
119 return clusterTree;
120}
121
122/* ************************************************************************* */
123TEST(NonlinearClusterTree, Construct) {
124 NonlinearClusterTree clusterTree = Construct();
125
126 EXPECT_LONGS_EQUAL(3, clusterTree[0].problemSize());
127 EXPECT_LONGS_EQUAL(3, clusterTree[0][0].problemSize());
128 EXPECT_LONGS_EQUAL(3, clusterTree[0][0][0].problemSize());
129}
130
131/* ************************************************************************* */
132TEST(NonlinearClusterTree, Solve) {
133 NonlinearClusterTree clusterTree = Construct();
134
135 Values expected;
136 expected.insert(j: l1, val: Point2(2, 2));
137 expected.insert(j: l2, val: Point2(4, 2));
138 expected.insert(j: x1, val: Pose2(0, 0, 0));
139 expected.insert(j: x2, val: Pose2(2, 0, 0));
140 expected.insert(j: x3, val: Pose2(4, 0, 0));
141
142 Values values = planarSLAMValues();
143 for (size_t i = 0; i < 4; i++)
144 values = clusterTree.updateCholesky(values);
145
146 EXPECT(assert_equal(expected, values, 1e-7));
147}
148
149/* ************************************************************************* */
150int main() {
151 TestResult tr;
152 return TestRegistry::runAllTests(result&: tr);
153}
154/* ************************************************************************* */
155