testNonlinearClusterTree.cpp
Go to the documentation of this file.
1 
10 #include <gtsam/geometry/Pose2.h>
11 #include <gtsam/geometry/Point2.h>
12 
13 #include <gtsam/inference/Symbol.h>
14 
16 
17 using namespace gtsam;
18 
19 static const Symbol x1('x', 1), x2('x', 2), x3('x', 3);
20 static const Symbol l1('l', 1), l2('l', 2);
21 
22 /* ************************************************************************* */
25 
26  // Prior on pose x1 at the origin.
27  Pose2 prior(0.0, 0.0, 0.0);
28  auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
29  graph.addPrior(x1, prior, priorNoise);
30 
31  // Two odometry factors
32  Pose2 odometry(2.0, 0.0, 0.0);
33  auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
36 
37  // Add Range-Bearing measurements to two different landmarks
38  auto measurementNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.2));
39  Rot2 bearing11 = Rot2::fromDegrees(45), bearing21 = Rot2::fromDegrees(90),
40  bearing32 = Rot2::fromDegrees(90);
41  double range11 = std::sqrt(4.0 + 4.0), range21 = 2.0, range32 = 2.0;
42  graph.emplace_shared<BearingRangeFactor<Pose2, Point2> >(x1, l1, bearing11, range11, measurementNoise);
43  graph.emplace_shared<BearingRangeFactor<Pose2, Point2> >(x2, l1, bearing21, range21, measurementNoise);
44  graph.emplace_shared<BearingRangeFactor<Pose2, Point2> >(x3, l2, bearing32, range32, measurementNoise);
45 
46  return graph;
47 }
48 
49 /* ************************************************************************* */
50 // Create initial estimate
53  initial.insert(l1, Point2(1.8, 2.1));
54  initial.insert(l2, Point2(4.1, 1.8));
55  initial.insert(x1, Pose2(0.5, 0.0, 0.2));
56  initial.insert(x2, Pose2(2.3, 0.1, -0.2));
57  initial.insert(x3, Pose2(4.1, 0.1, 0.1));
58  return initial;
59 }
60 
61 /* ************************************************************************* */
65 
66  // Build the clusters
67  // NOTE(frank): Order matters here as factors are removed!
68  VariableIndex variableIndex(graph);
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(initial);
84  EXPECT_LONGS_EQUAL(3, gfg->size());
85 
86  // Calculate expected result of only evaluating the marginalCluster
88  ordering.push_back(x1);
89  const auto [bn, fg] = gfg->eliminatePartialSequential(ordering);
90  auto expectedFactor = std::dynamic_pointer_cast<HessianFactor>(fg->at(0));
91  if (!expectedFactor)
92  throw std::runtime_error("Expected HessianFactor");
93 
94  // Linearize and eliminate the marginalCluster
95  auto actual = marginalCluster->linearizeAndEliminate(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 /* ************************************************************************* */
104  // Build the clusters
105  // NOTE(frank): Order matters here as factors are removed!
107  VariableIndex variableIndex(graph);
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(rootCluster);
116  rootCluster->addChild(landmarkCluster);
117  landmarkCluster->addChild(marginalCluster);
118 
119  return clusterTree;
120 }
121 
122 /* ************************************************************************* */
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 /* ************************************************************************* */
133  NonlinearClusterTree clusterTree = Construct();
134 
136  expected.insert(l1, Point2(2, 2));
137  expected.insert(l2, Point2(4, 2));
138  expected.insert(x1, Pose2(0, 0, 0));
139  expected.insert(x2, Pose2(2, 0, 0));
140  expected.insert(x3, Pose2(4, 0, 0));
141 
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 /* ************************************************************************* */
150 int main() {
151  TestResult tr;
152  return TestRegistry::runAllTests(tr);
153 }
154 /* ************************************************************************* */
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:196
static int runAllTests(TestResult &result)
Eigen::Vector3d Vector3
Definition: Vector.h:43
Point2 prior(const Point2 &x)
Prior on a single pose.
Definition: simulated2D.h:88
Matrix expected
Definition: testMatrix.cpp:971
Vector2 Point2
Definition: Point2.h:32
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
static const Symbol x3('x', 3)
leaf::MyValues values
Values initial
static const Symbol x2('x', 2)
static const Symbol l1('l', 1)
Values updateCholesky(const Values &values)
NonlinearFactorGraph graph
static enum @1107 ordering
static Rot2 fromDegrees(double theta)
Named constructor from angle in degrees.
Definition: Rot2.h:66
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
static const Symbol l2('l', 2)
auto odometryNoise
#define EXPECT(condition)
Definition: Test.h:150
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static NonlinearClusterTree Construct()
traits
Definition: chartTesting.h:28
auto priorNoise
Eigen::Vector2d Vector2
Definition: Vector.h:42
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
Definition: NoiseModel.cpp:270
const sharedFactor at(size_t i) const
Definition: FactorGraph.h:343
A Gaussian factor using the canonical parameters (information form)
void insert(Key j, const Value &val)
Definition: Values.cpp:155
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
Pose2 odometry(2.0, 0.0, 0.0)
TEST(SmartFactorBase, Pinhole)
2D Pose
2D Point
static const Symbol x1('x', 1)
NonlinearFactorGraph planarSLAMGraph()
void addRoot(const std::shared_ptr< Cluster > &cluster)
Definition: ClusterTree.h:145
Values planarSLAMValues()


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:38:46